diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h
index 395ef5e844c48a0d761d413c31f4c2a47c9d7f63..f85c588f68e5871ac22a8c98e6aa147085e9dedf 100644
--- a/include/laser/factor/factor_point_2d.h
+++ b/include/laser/factor/factor_point_2d.h
@@ -5,6 +5,7 @@
#include "core/factor/factor_autodiff.h"
#include "laser/feature/feature_polyline_2d.h"
#include "laser/landmark/landmark_polyline_2d.h"
+#include "core/math/covariance.h"
namespace wolf {
@@ -19,9 +20,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
unsigned int feature_point_id_;
int landmark_point_id_;
StateBlockPtr point_state_ptr_;
- Eigen::VectorXd measurement_; ///< the measurement vector
- Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix
public:
@@ -44,19 +42,32 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
_lmk_ptr->getPointStateBlock(_lmk_point_id)),
feature_point_id_(_ftr_point_id),
landmark_point_id_(_lmk_point_id),
- point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)),
- measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)),
- measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id))
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
- Eigen::LLT<Eigen::MatrixXd> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXd measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
}
virtual ~FactorPoint2d() = default;
+ virtual void updateMeasurementAndSquareRootInformationUpper() override
+ {
+ if (not getFeature())
+ return;
+
+ // cast
+ auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
+
+ // measurement
+ measurement_ = ftr->getPoints().col(feature_point_id_);
+
+ // covariance (ensuring symmetry)
+ Eigen::Matrix2d measurement_covariance = ftr->getPointsCov().middleCols(feature_point_id_*2,2).selfadjointView<Eigen::Upper>();
+
+ // square root information upper matrix
+ measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance.inverse());
+ }
+
virtual std::string getTopology() const override
{
return std::string("LMK");
@@ -103,27 +114,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
*/
template <typename T>
bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXd& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXd& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
};
template<typename T>
diff --git a/include/laser/factor/factor_point_to_line_2d.h b/include/laser/factor/factor_point_to_line_2d.h
index f2aee9c6701ba9083524dc51537374f7b4b562b2..34cbe485d123b74f3ead4e4d1273ad5a21ae6f5e 100644
--- a/include/laser/factor/factor_point_to_line_2d.h
+++ b/include/laser/factor/factor_point_to_line_2d.h
@@ -5,6 +5,7 @@
#include "core/factor/factor_autodiff.h"
#include "laser/feature/feature_polyline_2d.h"
#include "laser/landmark/landmark_polyline_2d.h"
+#include "core/math/covariance.h"
namespace wolf {
@@ -19,9 +20,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
unsigned int feature_point_id_;
StateBlockPtr point_state_ptr_;
StateBlockPtr point_aux_state_ptr_;
- Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix
public:
@@ -48,20 +47,33 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
landmark_point_aux_id_(_lmk_point_aux_id),
feature_point_id_(_ftr_point_id),
point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)),
- point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
- measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)),
- measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id))
{
//std::cout << "FactorPointToLine2d" << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_));
- Eigen::LLT<Eigen::MatrixXd> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXd measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
}
virtual ~FactorPointToLine2d() = default;
+ virtual void updateMeasurementAndSquareRootInformationUpper() override
+ {
+ if (not getFeature())
+ return;
+
+ // cast
+ auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
+
+ // measurement
+ measurement_ = ftr->getPoints().col(feature_point_id_);
+
+ // covariance (ensuring symmetry)
+ measurement_covariance_ = ftr->getPointsCov().middleCols(feature_point_id_*2,2).selfadjointView<Eigen::Upper>();
+
+ // square root information upper matrix
+ measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
+ }
+
virtual std::string getTopology() const override
{
return std::string("GEOM");
@@ -99,27 +111,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
template <typename T>
bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXd& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXd& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
};
template<typename T>