From 0cc12df8a663b488654eb7065b7b8d7adee99581 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 3 Jun 2020 19:57:36 +0200
Subject: [PATCH] hotfix (lost commit)

---
 include/laser/factor/factor_point_2d.h         | 12 +++---------
 include/laser/factor/factor_point_to_line_2d.h | 11 +++--------
 2 files changed, 6 insertions(+), 17 deletions(-)

diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h
index f85c588f6..5cf97ff2b 100644
--- a/include/laser/factor/factor_point_2d.h
+++ b/include/laser/factor/factor_point_2d.h
@@ -31,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,
@@ -46,16 +47,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
     {
         //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;
-    }
-
-    virtual ~FactorPoint2d() = default;
 
-    virtual void updateMeasurementAndSquareRootInformationUpper() override
-    {
-        if (not getFeature())
-            return;
-
-        // cast
         auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
 
         // measurement
@@ -68,6 +60,8 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
         measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance.inverse());
     }
 
+    virtual ~FactorPoint2d() = default;
+
     virtual std::string getTopology() const override
     {
         return std::string("LMK");
diff --git a/include/laser/factor/factor_point_to_line_2d.h b/include/laser/factor/factor_point_to_line_2d.h
index 34cbe485d..1fad54e85 100644
--- a/include/laser/factor/factor_point_to_line_2d.h
+++ b/include/laser/factor/factor_point_to_line_2d.h
@@ -30,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,
@@ -52,14 +53,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
         //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_));
-    }
-
-    virtual ~FactorPointToLine2d() = default;
-
-    virtual void updateMeasurementAndSquareRootInformationUpper() override
-    {
-        if (not getFeature())
-            return;
 
         // cast
         auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature());
@@ -74,6 +67,8 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
         measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
     }
 
+    virtual ~FactorPointToLine2d() = default;
+
     virtual std::string getTopology() const override
     {
         return std::string("GEOM");
-- 
GitLab