diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h index b16168db2d60f618401a09848de6fb368f711db3..ae2f71789bee991f93a8c75b7529d9b273e53cc8 100644 --- a/include/core/processor/processor_tracker_feature_landmark_external.h +++ b/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"); diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp index 50a1730c2d3bf08c7020022448913ca7c857cf3c..69d09ec77a5c9a7f07879fe8580702aadc18b427 100644 --- a/src/processor/processor_tracker_feature_landmark_external.cpp +++ b/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) diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_tracker_feature_landmark_external.cpp index a82e385f35739d3065749cdfc67b43b07c71589d..fa1979aa5fc588e00ce5a55dfee8433dd1f3a4a1 100644 --- a/test/gtest_processor_tracker_feature_landmark_external.cpp +++ b/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;