From 9612c824cc513177f7e9ace59f4cbe43ee7d3ff6 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 8 Feb 2024 16:50:50 +0100
Subject: [PATCH] fixed auto bug for eigen

---
 src/processor/processor_landmark_external.cpp | 235 +++++++++---------
 test/gtest_processor_landmark_external.cpp    |   2 +-
 2 files changed, 124 insertions(+), 113 deletions(-)

diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index ce22608d7..1544904df 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -87,9 +87,9 @@ void ProcessorLandmarkExternal::preProcess()
         // add new feature
         new_features_incoming_.push_back(ftr);
     }
-    WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
-               new_features_incoming_.size(),
-               " features in incoming capture");
+    WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ",
+              new_features_incoming_.size(),
+              " features in incoming capture");
 }
 
 unsigned int ProcessorLandmarkExternal::processKnown()
@@ -100,7 +100,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
     // Track features from last_ptr_ to incoming_ptr_
     if (not last_ptr_) return 0;
 
-    WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
+    WOLF_INFO("Searching ", known_features_last_.size(), " tracked features...");
     auto pose_sen      = getSensor()->getState("PO");
     auto pose_last     = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
     auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
@@ -109,14 +109,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
     {
         auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
         bool matched       = false;
-        WOLF_DEBUG("Tracking feature last: ",
-                   feat_lmk_last->id(),
-                   " - ID: ",
-                   feat_lmk_last->getExternalId(),
-                   " - TYPE: ",
-                   feat_lmk_last->getExternalType(),
-                   " meas: ",
-                   feat_lmk_last->getMeasurement().transpose());
+        WOLF_INFO("Tracking feature last: ",
+                  feat_lmk_last->id(),
+                  " - ID: ",
+                  feat_lmk_last->getExternalId(),
+                  " - TYPE: ",
+                  feat_lmk_last->getExternalType(),
+                  " meas: ",
+                  feat_lmk_last->getMeasurement().transpose());
 
         // auto last_global_meas = (dim_ == 2 ? Eigen::VectorXd(pose_last.at('P') +
         //                                         Rotation2Dd(pose_last.at('O')(0)) *
@@ -126,13 +126,13 @@ unsigned int ProcessorLandmarkExternal::processKnown()
         //                                         Quaterniond(Vector4d(pose_last.at('O'))) *
         //                                             (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
         //                                                                     feat_lmk_last->getMeasurement().head<3>())));
-        // WOLF_DEBUG("pose_last: ", pose_last);
-        // WOLF_DEBUG("pose_sen: ", pose_sen);
+        // WOLF_INFO("pose_last: ", pose_last);
+        // WOLF_INFO("pose_sen: ", pose_sen);
         // auto last_global_meas =
         //     pose_last.at('P') +
         //     Rotation2Dd(pose_last.at('O')(0)) *
         //         (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>());
-        // WOLF_DEBUG("Last global meas: ", last_global_meas.transpose());
+        // WOLF_INFO("Last global meas: ", last_global_meas.transpose());
 
         // First we try to match by EXTERNAL_ID
         if (feat_lmk_last->getExternalId() != -1)
@@ -142,14 +142,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
             {
                 auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
 
-                WOLF_DEBUG("Feature incoming candidate (by ID): ",
-                           feat_lmk_incoming->id(),
-                           " - ID: ",
-                           feat_lmk_incoming->getExternalId(),
-                           " - TYPE: ",
-                           feat_lmk_incoming->getExternalType(),
-                           " meas: ",
-                           feat_lmk_incoming->getMeasurement().transpose());
+                WOLF_INFO("Feature incoming candidate (by ID): ",
+                          feat_lmk_incoming->id(),
+                          " - ID: ",
+                          feat_lmk_incoming->getExternalId(),
+                          " - TYPE: ",
+                          feat_lmk_incoming->getExternalType(),
+                          " meas: ",
+                          feat_lmk_incoming->getMeasurement().transpose());
 
                 // auto global_meas =
                 //     (dim_ == 2
@@ -166,8 +166,8 @@ unsigned int ProcessorLandmarkExternal::processKnown()
                 //     pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
                 //                                 (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
                 //                                                         feat_lmk_incoming->getMeasurement().head<2>());
-                // WOLF_DEBUG("pose_incoming: ", pose_incoming);
-                // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose());
+                // WOLF_INFO("pose_incoming: ", pose_incoming);
+                // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
 
                 // MATCH NECESSARY CONDITIONS:
                 // 1. Same EXTERNAL_ID
@@ -181,12 +181,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
                         feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) <  // cond 3
                         params_tfle_->match_dist_th_id)
                 {
-                    WOLF_DEBUG("Feature last: ",
-                               feat_lmk_last->id(),
-                               " matched with feature ",
-                               feat_lmk_incoming->id(),
-                               " with landmark ID: ",
-                               feat_lmk_last->landmarkId());
+                    WOLF_INFO("Feature last: ",
+                              feat_lmk_last->id(),
+                              " matched with feature ",
+                              feat_lmk_incoming->id(),
+                              " with landmark ID: ",
+                              feat_lmk_last->landmarkId());
                     matched = true;
 
                     // set LANDMARK_ID if defined
@@ -217,14 +217,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
             while (feature_incoming_it != new_features_incoming_.end())
             {
                 auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
-                WOLF_DEBUG("Feature incoming candidate (by TYPE): ",
-                           feat_lmk_incoming->id(),
-                           " - ID: ",
-                           feat_lmk_incoming->getExternalId(),
-                           " - TYPE: ",
-                           feat_lmk_incoming->getExternalType(),
-                           " meas: ",
-                           feat_lmk_incoming->getMeasurement().transpose());
+                WOLF_INFO("Feature incoming candidate (by TYPE): ",
+                          feat_lmk_incoming->id(),
+                          " - ID: ",
+                          feat_lmk_incoming->getExternalId(),
+                          " - TYPE: ",
+                          feat_lmk_incoming->getExternalType(),
+                          " meas: ",
+                          feat_lmk_incoming->getMeasurement().transpose());
 
                 // auto global_meas =
                 //     (dim_ == 2
@@ -241,8 +241,8 @@ unsigned int ProcessorLandmarkExternal::processKnown()
                 //     pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
                 //                                 (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
                 //                                                         feat_lmk_incoming->getMeasurement().head<2>());
-                // WOLF_DEBUG("pose_incoming: ", pose_incoming);
-                // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose());
+                // WOLF_INFO("pose_incoming: ", pose_incoming);
+                // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
 
                 // MATCH NECESSARY CONDITIONS:
                 // 1. Compatible EXTERNAL_ID (either not defined or same)
@@ -256,12 +256,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
                         feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) <  // cond 3
                         params_tfle_->match_dist_th_type)
                 {
-                    WOLF_DEBUG("Feature last: ",
-                               feat_lmk_last->id(),
-                               " matched with feature ",
-                               feat_lmk_incoming->id(),
-                               " with landmark ID: ",
-                               feat_lmk_last->landmarkId());
+                    WOLF_INFO("Feature last: ",
+                              feat_lmk_last->id(),
+                              " matched with feature ",
+                              feat_lmk_incoming->id(),
+                              " with landmark ID: ",
+                              feat_lmk_last->landmarkId());
                     matched = true;
 
                     // set LANDMARK_ID last -> incoming
@@ -295,14 +295,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
         {
             auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
 
-            WOLF_DEBUG("Feature incoming candidate (by distance): ",
-                       feat_lmk_incoming->id(),
-                       " - ID: ",
-                       feat_lmk_incoming->getExternalId(),
-                       " - TYPE: ",
-                       feat_lmk_incoming->getExternalType(),
-                       " meas: ",
-                       feat_lmk_incoming->getMeasurement().transpose());
+            WOLF_INFO("Feature incoming candidate (by distance): ",
+                      feat_lmk_incoming->id(),
+                      " - ID: ",
+                      feat_lmk_incoming->getExternalId(),
+                      " - TYPE: ",
+                      feat_lmk_incoming->getExternalType(),
+                      " meas: ",
+                      feat_lmk_incoming->getMeasurement().transpose());
 
             // auto global_meas =
             //     (dim_ == 2
@@ -319,8 +319,8 @@ unsigned int ProcessorLandmarkExternal::processKnown()
             //     pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
             //                                 (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
             //                                                         feat_lmk_incoming->getMeasurement().head<2>());
-            // WOLF_DEBUG("pose_incoming: ", pose_incoming);
-            // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose());
+            // WOLF_INFO("pose_incoming: ", pose_incoming);
+            // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
 
             // MATCH NECESSARY CONDITIONS:
             // 1. Compatible EXTERNAL_ID (either not defined or same)
@@ -335,12 +335,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
                 detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) <  // cond 3
                     params_tfle_->match_dist_th_unknown)
             {
-                WOLF_DEBUG("Feature last: ",
-                           feat_lmk_last->id(),
-                           " matched with feature ",
-                           feat_lmk_incoming->id(),
-                           " with landmark ID: ",
-                           feat_lmk_last->landmarkId());
+                WOLF_INFO("Feature last: ",
+                          feat_lmk_last->id(),
+                          " matched with feature ",
+                          feat_lmk_incoming->id(),
+                          " with landmark ID: ",
+                          feat_lmk_last->landmarkId());
                 matched = true;
 
                 // set LANDMARK_ID last -> incoming
@@ -369,7 +369,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
         }
         WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked.");
     }
-    WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
+    WOLF_INFO("Tracked ", known_features_incoming_.size(), " features.");
 
     // Add new features (not tracked) as known features
     WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
@@ -451,14 +451,14 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr         _ftr,
 
     if (getProblem()->getDim() == 2)
     {
-        auto pose_s = SE2::compose(_pose_frm, _pose_sen);
-        auto p      = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>();
+        auto            pose_s = SE2::compose(_pose_frm, _pose_sen);
+        Eigen::Vector2d p      = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>();
         return (p - _lmk->getP()->getState().head<2>()).norm();
     }
     else
     {
-        auto pose_s = SE3::compose(_pose_frm, _pose_sen);
-        auto p      = pose_s.at('P') + Quaterniond(Vector4d(pose_s.at('O'))) * _ftr->getMeasurement().head<3>();
+        auto            pose_s = SE3::compose(_pose_frm, _pose_sen);
+        Eigen::Vector3d p = pose_s.at('P') + Quaterniond(Vector4d(pose_s.at('O'))) * _ftr->getMeasurement().head<3>();
         return (p - _lmk->getP()->getState().head<3>()).norm();
     }
 }
@@ -467,7 +467,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
 {
     auto track_ids_last = track_matrix_.trackIds(last_ptr_);
 
-    WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
+    WOLF_INFO("Active feature tracks: ", track_ids_last.size());
 
     // number of tracks longer than track_length_th
     auto n_tracks     = 0;
@@ -483,50 +483,50 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
 
     // Necessary condition: active valid tracks
     bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
-    WOLF_DEBUG("vote_min_features: ",
-               vote_min_features,
-               " - Active feature tracks longer than ",
-               params_tfle_->track_length_th,
-               ": ",
-               n_tracks,
-               " (should be equal or bigger than ",
-               params_tfle_->min_features_for_keyframe,
-               ")");
+    WOLF_INFO("vote_min_features: ",
+              vote_min_features,
+              " - Active feature tracks longer than ",
+              params_tfle_->track_length_th,
+              ": ",
+              n_tracks,
+              " (should be equal or bigger than ",
+              params_tfle_->min_features_for_keyframe,
+              ")");
 
     bool vote_new_features(true), vote_time_span(true);
     if (vote_min_features and origin_ptr_)
     {
         // Sufficient condition: new valid tracks
         vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
-        WOLF_DEBUG("vote_new_features: ",
-                   vote_new_features,
-                   " - n_new_tracks = ",
-                   n_new_tracks,
-                   " (should be equal or bigger than ",
-                   params_tfle_->new_features_for_keyframe,
-                   ")");
+        WOLF_INFO("vote_new_features: ",
+                  vote_new_features,
+                  " - n_new_tracks = ",
+                  n_new_tracks,
+                  " (should be equal or bigger than ",
+                  params_tfle_->new_features_for_keyframe,
+                  ")");
 
         // Sufficient condition: time span
         vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
-        WOLF_DEBUG("vote_time_span: ",
-                   vote_time_span,
-                   " - time_span = ",
-                   last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
-                   " (should be bigger than ",
-                   params_tfle_->time_span,
-                   ")");
+        WOLF_INFO("vote_time_span: ",
+                  vote_time_span,
+                  " - time_span = ",
+                  last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+                  " (should be bigger than ",
+                  params_tfle_->time_span,
+                  ")");
     }
 
     bool vote = vote_min_features and (vote_new_features or vote_time_span);
 
-    WOLF_DEBUG((vote ? "Vote " : "Do NOT vote "), "for KF");
+    WOLF_INFO((vote ? "Vote " : "Do NOT vote "), "for KF");
 
     return vote;
 }
 
 void ProcessorLandmarkExternal::establishFactors()
 {
-    WOLF_DEBUG("establishFactors");
+    WOLF_INFO("establishFactors");
     // if (origin_ptr_ == last_ptr_) return;
 
     // // reset n_tracks_origin_
@@ -540,23 +540,30 @@ void ProcessorLandmarkExternal::establishFactors()
 
     for (auto track_id : track_ids_last)
     {
-        WOLF_DEBUG("Feature ", track_matrix_.feature(track_id, last_ptr_)->id());
+        auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
+
+        WOLF_INFO("Feature ",
+                  feature->id(),
+                  ": ID: ",
+                  feature->landmarkId(),
+                  " EXTERNAL_ID: ",
+                  feature->getExternalId(),
+                  " TYPE: ",
+                  feature->getExternalType());
 
         // not enough long track
+        WOLF_INFO_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
+                       "Track NOT long enough ",
+                       track_matrix_.trackSize(track_id));
         if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue;
 
-        WOLF_DEBUG("Track long enough: ", track_matrix_.trackSize(track_id));
-
-        auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
-
         // Landmark match
         LandmarkExternalPtr lmk;
         if (feature->landmarkId() == 0)  // landmark id 0 never used
         {
-            WOLF_DEBUG("Landmark id not set");
-
             // LOOP CLOSURE
             // By ID
+            WOLF_INFO("Searching Loop closure by ID...");
             if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1)
             {
                 auto lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -569,12 +576,16 @@ void ProcessorLandmarkExternal::establishFactors()
                         detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id)
                     {
                         lmk = lmk_ext;
-                        WOLF_DEBUG("Found loop closure by EXTERNAL_ID with ", lmk->id());
+                        WOLF_INFO("Found loop closure by EXTERNAL_ID with ", lmk->id());
                         break;
                     }
+                    WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
+                                   "Landmark with EXTERNAL_ID found but not matched due to distance: ",
+                                   detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
                 }
             }
             // By TYPE
+            WOLF_INFO("Searching Loop closure by TYPE...");
             if (not lmk and params_tfle_->close_loops_by_type and feature->getExternalType() != -1)
             {
                 auto lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -587,9 +598,12 @@ void ProcessorLandmarkExternal::establishFactors()
                         detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type)
                     {
                         lmk = lmk_ext;
-                        WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id());
+                        WOLF_INFO("Found loop closure by TYPE with ", lmk->id());
                         break;
                     }
+                    WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
+                                   "Landmark with TYPE found but not matched due to distance: ",
+                                   detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
                 }
 
                 // update Landmark EXTERNAL_ID (if available)
@@ -600,17 +614,17 @@ void ProcessorLandmarkExternal::establishFactors()
             if (not lmk)
             {
                 lmk = emplaceLandmark(feature);
-                WOLF_DEBUG("New landmark ", lmk->id());
+                WOLF_INFO("Emplaced new landmark ", lmk->id());
             }
 
             // set LANDMARK_ID in all features of the track
             for (auto feat_pair : track_matrix_.track(track_id))
             {
                 feat_pair.second->setLandmarkId(lmk->id());
-                WOLF_DEBUG("Setting landmark id in feature ",
-                           feat_pair.second->id(),
-                           " landmark_id: ",
-                           feat_pair.second->landmarkId());
+                WOLF_INFO("Setting landmark id in feature ",
+                          feat_pair.second->id(),
+                          " landmark_id: ",
+                          feat_pair.second->landmarkId());
             }
         }
         // landmarkId already set
@@ -635,12 +649,9 @@ void ProcessorLandmarkExternal::establishFactors()
                 auto fac = emplaceFactor(feature, lmk);
                 if (fac) fac_list.push_back(fac);
             }
-
-        // // store tracked landmarks from origin (used by voteForKeyframe())
-        // lmks_ids_origin_.insert(lmk->id());
     }
 
-    WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
+    WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
 }
 
 FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index 218263497..8336e31a6 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -178,7 +178,7 @@ void ProcessorLandmarkExternalTest::initProblem(int          _dim,
     // Emplace 3 random landmarks
     for (auto i = 0; i < 3; i++)
     {
-        bool                init_landmark = _init_landmarks and mode != 3 and i % 3 != 2;
+        bool                init_landmark = _init_landmarks and not (mode == 3 and i % 3 == 2);
         LandmarkExternalPtr lmk;
         if (dim == 2)
             lmk = LandmarkBase::emplace<LandmarkExternal>(
-- 
GitLab