added new parameter to processor landmark external

include/core/processor/processor_tracker_feature_landmark_external.h

@@ -32,6 +32,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
 
 struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
 {
+    bool use_orientation;                 ///< use orientation measure or not when emplacing factors
     double filter_quality_th;             ///< min quality to consider the detection
     double filter_dist_th;                ///< for considering tracked detection: distance threshold to previous detection
     unsigned int filter_track_length_th;  ///< length of the track necessary to consider the detection
@@ -42,6 +43,7 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
                                        const wolf::ParamsServer & _server):
         ParamsProcessorTrackerFeature(_unique_name, _server)
     {
+        use_orientation         = _server.getParam<bool>        (prefix + _unique_name + "/use_orientation");
         filter_quality_th       = _server.getParam<double>      (prefix + _unique_name + "/filter_quality_th");
         filter_dist_th          = _server.getParam<double>      (prefix + _unique_name + "/filter_dist_th");
         filter_track_length_th  = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");

src/processor/processor_tracker_feature_landmark_external.cpp

@@ -240,7 +240,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
     LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(_feature_ptr->landmarkId());
     
     // 2D - Relative Position
-    if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 2)
+    if (getProblem()->getDim() == 2 and (_feature_ptr->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
     {
         // no landmark, create it
         if (not lmk)
@@ -267,7 +267,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
                                                                            params_tfle_->apply_loss_function);
     }
     // 2D - Relative Pose
-    else if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 3)
+    else if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 3 and params_tfle_->use_orientation)
     {
         // no landmark, create it
         if (not lmk)
@@ -305,7 +305,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
                                                                        params_tfle_->apply_loss_function);
     }
     // 3D - Relative Position
-    else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 3)
+    else if (getProblem()->getDim() == 3 and (_feature_ptr->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
     {
         // no landmark, create it
         if (not lmk)
@@ -332,7 +332,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
                                                                            params_tfle_->apply_loss_function);
     }
     // 3D - Relative Pose
-    else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7)
+    else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7 and params_tfle_->use_orientation)
     {
         // no landmark, create it
         if (not lmk)

test/gtest_processor_tracker_feature_landmark_external.cpp

@@ -131,6 +131,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
     params->max_new_features = -1;
     params->voting_active = true;
     params->apply_loss_function = false;
+    params->use_orientation = orientation;
     params->filter_quality_th = _quality_th;
     params->filter_dist_th = _dist_th;
     params->filter_track_length_th = _track_length_th;