Resolve "Loss function API"

include/laser/factor/factor_container.h

@@ -22,7 +22,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
                           const LandmarkContainerPtr& _lmk_ptr,
                           const ProcessorBasePtr& _processor_ptr,
                           const unsigned int _corner,
-                          bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+                          bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
             FactorAutodiff<FactorContainer,3,2,1,2,1>("FactorContainer",
                                                       nullptr,
                                                       nullptr,
@@ -130,17 +130,6 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
 
 			return true;
 		}
-
-  public:
-    static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
-                                const NodeBasePtr& _correspondant_ptr,
-                                const ProcessorBasePtr& _processor_ptr = nullptr)
-    {
-        unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
-
-        return std::make_shared<FactorContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
-    }
-
 };
 
 } // namespace wolf

include/laser/factor/factor_point_2D.h

@@ -26,12 +26,27 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
     public:
 
     FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
-                      const LandmarkPolyline2DPtr& _lmk_ptr,
-                      const ProcessorBasePtr& _processor_ptr,
-                      unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+                  const LandmarkPolyline2DPtr& _lmk_ptr,
+                  const ProcessorBasePtr& _processor_ptr,
+                  unsigned int _ftr_point_id,
+                  int _lmk_point_id,
+                  bool _apply_loss_function,
+                  FactorStatus _status = FAC_ACTIVE) :
         FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("FactorPoint2D",
-                nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _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))
+                                                  nullptr, nullptr, nullptr, _lmk_ptr,
+                                                  _processor_ptr,
+                                                  _apply_loss_function,
+                                                  _status,
+                                                  _ftr_ptr->getFrame()->getP(),
+                                                  _ftr_ptr->getFrame()->getO(),
+                                                  _lmk_ptr->getP(),
+                                                  _lmk_ptr->getO(),
+                                                  _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))
     {
         //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;

include/laser/factor/factor_point_to_line_2D.h

@@ -29,10 +29,28 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
                             const LandmarkPolyline2DPtr& _lmk_ptr,
                             const ProcessorBasePtr& _processor_ptr,
                             unsigned int _ftr_point_id, int _lmk_point_id,  int _lmk_point_aux_id,
-                            bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+                            bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
         FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("FactorPointToLine2D",
-                nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
-        landmark_point_id_(_lmk_point_id), 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))
+                                                           nullptr,
+                                                           nullptr,
+                                                           nullptr,
+                                                           _lmk_ptr,
+                                                           _processor_ptr,
+                                                           _apply_loss_function,
+                                                           _status,
+                                                           _ftr_ptr->getFrame()->getP(),
+                                                           _ftr_ptr->getFrame()->getO(),
+                                                           _lmk_ptr->getP(),
+                                                           _lmk_ptr->getO(),
+                                                           _lmk_ptr->getPointStateBlock(_lmk_point_id),
+                                                           _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+        landmark_point_id_(_lmk_point_id),
+        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))
     {
         //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;

src/processor/processor_closeloop_icp.cpp

@@ -228,11 +228,12 @@ CapturesAligned ProcessorCloseLoopICP::bestCandidate(std::map<double, CapturesAl
     return _capture_candidates.begin()->second;
 }
 FactorBasePtr ProcessorCloseLoopICP::emplaceFeatureAndFactor(CapturesAligned &_captures_aligned)
-    {
-        _captures_aligned.align_result.res_covar = Eigen::Vector3s(0.01,0.01,0.01).asDiagonal();
-        auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own, _captures_aligned.align_result);
-        return FactorBase::emplace<FactorOdom2D>(ftr, ftr, _captures_aligned.capture_other->getFrame(), shared_from_this());
-    }
+{
+    _captures_aligned.align_result.res_covar = Eigen::Vector3s(0.01,0.01,0.01).asDiagonal();
+    auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own, _captures_aligned.align_result);
+    return FactorBase::emplace<FactorOdom2D>(ftr, ftr, _captures_aligned.capture_other->getFrame(), shared_from_this(), params_->apply_loss_function);
+}
+
 }
 // Register in the SensorFactory
 #include "core/processor/processor_factory.h"

src/processor/processor_odom_icp.cpp

@@ -311,8 +311,11 @@ FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
 
 FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)
 {
-    return FactorBase::emplace<FactorOdom2D>(_feature, _feature, origin_ptr_->getFrame(),
-                                                      shared_from_this());
+    return FactorBase::emplace<FactorOdom2D>(_feature,
+                                             _feature,
+                                             origin_ptr_->getFrame(),
+                                             shared_from_this(),
+                                             params_->apply_loss_function);
 }
 
 

src/processor/processor_tracker_feature_polyline_2D.cpp

@@ -575,7 +575,8 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline
                                                                      shared_from_this(),
                                                                      _ftr_point_id,
                                                                      _lmk_point_id,
-                                                                     _lmk_prev_point_id);
+                                                                     _lmk_prev_point_id,
+                                                                     params_->apply_loss_function);
 }
 
 void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
@@ -590,7 +591,8 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr
                                                                _polyline_landmark,
                                                                shared_from_this(),
                                                                _ftr_point_id,
-                                                               _lmk_point_id);
+                                                               _lmk_point_id,
+                                                               params_->apply_loss_function);
 }
 
 LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePtr _feature_ptr)