From c3c4cbe44e01310cf11f6d222bb5dba69e1b4563 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 10 Oct 2022 18:01:08 +0200
Subject: [PATCH] added new parameter to processor landmark external
---
.../processor_tracker_feature_landmark_external.h | 2 ++
.../processor_tracker_feature_landmark_external.cpp | 8 ++++----
.../gtest_processor_tracker_feature_landmark_external.cpp | 1 +
3 files changed, 7 insertions(+), 4 deletions(-)
diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h
index b16168db2..ae2f71789 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 50a1730c2..69d09ec77 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 a82e385f3..fa1979aa5 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;
--
GitLab