Merge remote-tracking branch 'origin/devel' into 12-migrate-to-state-composites

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:
 
@@ -33,6 +31,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
                   bool _apply_loss_function,
                   FactorStatus _status = FAC_ACTIVE) :
         FactorAutodiff<FactorPoint2d,2,2,1,2,1,2>("FactorPoint2d",
+                                                  _ftr_ptr,
                                                   nullptr, nullptr, nullptr, _lmk_ptr,
                                                   _processor_ptr,
                                                   _apply_loss_function,
@@ -44,15 +43,21 @@ 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
+
+        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 ~FactorPoint2d() = default;
@@ -103,27 +108,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>

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:
 
@@ -31,6 +30,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
                             unsigned int _ftr_point_id, int _lmk_point_id,  int _lmk_point_aux_id,
                             bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
         FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>("FactorPointToLine2d",
+                                                           _ftr_ptr,
                                                            nullptr,
                                                            nullptr,
                                                            nullptr,
@@ -48,16 +48,23 @@ 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
+
+        // 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 ~FactorPointToLine2d() = default;
@@ -99,27 +106,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>