Merge branch '490-processorlandmarkexternal-add-class-and-id-not-mandatory-2' into 'devel'

Resolve "ProcessorLandmarkExternal add class and id not mandatory"

Closes #490

See merge request !470

.gitlab-ci.yml

@@ -23,6 +23,7 @@ stages:
   - fi
   - export WOLF_CORE_BRANCH=$CI_COMMIT_BRANCH
   # Print variables
+  - echo $CI_COMMIT_REF_NAME
   - echo $CI_COMMIT_BRANCH
   - echo $WOLF_IMU_BRANCH
   - echo $WOLF_GNSS_BRANCH

include/core/processor/processor_landmark_external.h

@@ -36,32 +36,40 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
 
 struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
 {
-    bool         use_orientation;  ///< use orientation measure or not when emplacing factors
-    double       match_dist_th;    ///< for considering tracked detection: distance threshold to previous detection
+    bool use_orientation;  ///< use orientation measure or not when emplacing factors
+
     unsigned int new_features_for_keyframe;  ///< for keyframe voting: amount of new features with respect to origin
                                              ///< (sufficient condition if more than min_features_for_keyframe)
-    double       filter_quality_th;          ///< min quality to consider the detection
-    unsigned int filter_track_length_th;     ///< length of the track necessary to consider the detection
-    double       time_span;  ///< for keyframe voting: time span since last frame (sufficient condition if more than
-                             ///< min_features_for_keyframe)
-    bool check_dist_when_ids_match;    ///< check the match_dist_th also in detections with matching external ID
-    bool close_loop_when_id_match;    ///< emplace loop closure factors if external ID matches
-    bool close_loop_when_type_match;  ///< emplace loop closure factors if external TYPE matches (and match_dist_th holds)
+    double time_span;  ///< for keyframe voting: time span since last frame (sufficient condition if more than
+                       ///< min_features_for_keyframe)
+
+    double quality_th;  ///< min quality to consider the detection
+
+    // Matching distance threshold to previous detection considering motion (necessary condition)
+    double match_dist_th_id;       ///< Match by ID
+    double match_dist_th_type;     ///< Match by TYPE
+    double match_dist_th_unknown;  ///< No ID/TYPE information
+
+    unsigned int track_length_th;  ///< Track length threshold to emplace factors (necessary condition)
+
+    bool close_loops_by_id;    ///< Close loop if ID matches (ID unchanged guaranteed)
+    bool close_loops_by_type;  ///< Close loop if TYPE matches (also distance check)
 
     ParamsProcessorLandmarkExternal() = default;
     ParamsProcessorLandmarkExternal(std::string _unique_name, const wolf::ParamsServer& _server)
         : ParamsProcessorTracker(_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_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
-        match_dist_th          = _server.getParam<double>(prefix + _unique_name + "/match_dist_th");
-        time_span              = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
+        use_orientation = _server.getParam<bool>(prefix + _unique_name + "/use_orientation");
         new_features_for_keyframe =
             _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
-        check_dist_when_ids_match   = _server.getParam<bool>(prefix + _unique_name + "/check_dist_when_ids_match");
-        close_loop_when_id_match   = _server.getParam<bool>(prefix + _unique_name + "/close_loop_when_id_match");
-        close_loop_when_type_match = _server.getParam<bool>(prefix + _unique_name + "/close_loop_when_type_match");
+        time_span             = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
+        quality_th            = _server.getParam<double>(prefix + _unique_name + "/quality_th");
+        match_dist_th_id      = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_id");
+        match_dist_th_type    = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_type");
+        match_dist_th_unknown = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_unknown");
+        track_length_th       = _server.getParam<unsigned int>(prefix + _unique_name + "/track_length_th");
+        close_loops_by_id     = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_id");
+        close_loops_by_type   = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_type");
     }
 };
 
@@ -82,7 +90,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
   protected:
     ParamsProcessorLandmarkExternalPtr params_tfle_;
     TrackMatrix                        track_matrix_;
-    std::set<SizeStd>                  lmks_ids_origin_;
+    // std::set<SizeStd>                  lmks_ids_origin_;
 
     /** Pre-process incoming Capture
      *
@@ -154,16 +162,19 @@ class ProcessorLandmarkExternal : public ProcessorTracker
                              const VectorComposite& _pose1,
                              const VectorComposite& _pose2,
                              const VectorComposite& _pose_sen) const;
+
+    double detectionDistance(FeatureBasePtr         _ftr,
+                             LandmarkBasePtr        _lmk,
+                             const VectorComposite& _pose_frm,
+                             const VectorComposite& _pose_sen) const;
 };
 
 inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
     : ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
-      params_tfle_(_params_tfle),
-      lmks_ids_origin_()
+      params_tfle_(_params_tfle)//,lmks_ids_origin_()
 {
     //
 }
 
-
 }  // namespace wolf
 #endif
\ No newline at end of file

test/CMakeLists.txt

@@ -204,7 +204,7 @@ wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model
 wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
 
 # ProcessorLandmarkExternal class test
-# wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
+wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
 
 # ProcessorLoopClosure class test
 wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)