Merge branch '13-copying-measurement-and-sqrtinfo-in-factorbase-multi-threading' into 'devel'

Resolve "Copying measurement and sqrtinfo in FactorBase (multi-threading)"

Closes #13

See merge request !19

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>

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>