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

Closes #13 (closed)

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>