From 49aa3dcf5a58bca9cb0b3bfb90b36b5596354213 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Tue, 17 May 2022 19:58:02 +0200
Subject: [PATCH] HotFix: Adapt to core #453: TrackMatrix API

---
 .../processor/processor_visual_odometry.h     |  2 --
 src/processor/processor_visual_odometry.cpp   | 33 ++-----------------
 2 files changed, 3 insertions(+), 32 deletions(-)

diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 9cf5b0160..2377c08ac 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -199,8 +199,6 @@ class ProcessorVisualOdometry : public ProcessorTracker
         ActiveSearchGrid cell_grid_;
 
     private:
-        int frame_count_;
-
         // camera
         cv::Mat Kcv_;
 
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index b52a01825..652f5b057 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -33,8 +33,7 @@ namespace wolf{
 
 ProcessorVisualOdometry::ProcessorVisualOdometry(ParamsProcessorVisualOdometryPtr _params_vo) :
                 ProcessorTracker("ProcessorVisualOdometry", "PO", 3, _params_vo),
-                params_visual_odometry_(_params_vo),
-                frame_count_(0)
+                params_visual_odometry_(_params_vo)
 {
     // Preprocessor stuff
     detector_ = cv::FastFeatureDetector::create(_params_vo->fast.threshold,
@@ -195,8 +194,6 @@ void ProcessorVisualOdometry::preProcess()
     // Input: ID of Wkp in last. Output: ID of the tracked Wkp in incoming.
     TracksMap tracks_last_incoming = kltTrack(img_last, img_incoming, mwkps_last, mwkps_incoming);
 
-//    WOLF_DEBUG( "Tracked " , mwkps_incoming.size(), " keypoints to incoming" );
-
     // TracksMap between origin and last
     // Input: ID of Wkp in origin. Output: ID of the tracked Wkp in last.
     TracksMap tracks_origin_last = capture_image_last_->getTracksOrigin();
@@ -205,8 +202,6 @@ void ProcessorVisualOdometry::preProcess()
     // Input: ID of Wkp in origin. Output: ID of the tracked Wkp in incoming.
     TracksMap tracks_origin_incoming = mergeTracks(tracks_origin_last, tracks_last_incoming);
 
-//    WOLF_DEBUG( "Merged " , tracks_last_incoming.size(), " tracks..." );
-
     // Outliers rejection with essential matrix
     cv::Mat E;
     filterWithEssential(mwkps_origin, mwkps_incoming, tracks_origin_incoming, E);
@@ -233,13 +228,9 @@ void ProcessorVisualOdometry::preProcess()
     ////////////////////////////////
     size_t n_tracks_origin = tracks_origin_incoming.size();
 
-//    WOLF_DEBUG("# of tracks: ", n_tracks_origin, "; min # of tracks: ", params_visual_odometry_->min_features_for_keyframe);
 
     if (n_tracks_origin < params_visual_odometry_->min_features_for_keyframe)
     {
-
-//        WOLF_DEBUG( "Too Few Tracks. Detecting more keypoints in last" );
-
         // Erase all keypoints previously added to the cell grid
         cell_grid_.renew();
 
@@ -289,8 +280,6 @@ void ProcessorVisualOdometry::preProcess()
             }
         }
 
-//        WOLF_DEBUG("Detected ", kps_last_new.size(), " new raw keypoints");
-        
         // Create a map of wolf KeyPoints to track only the new ones
         KeyPointsMap mwkps_last_new, mwkps_incoming_new;
         for (auto & cvkp : kps_last_new){
@@ -303,8 +292,6 @@ void ProcessorVisualOdometry::preProcess()
 
         WOLF_DEBUG("Tracked ", mwkps_incoming_new.size(), " inliers in incoming");
 
-//        WOLF_DEBUG("Tracked ", mwkps_incoming_new.size(), " new keypoints to incoming");
-
         // Concatenation of old tracks and new tracks
         for (auto & track: tracks_last_incoming_new){
             tracks_last_incoming_filtered[track.first] = track.second;
@@ -325,10 +312,6 @@ void ProcessorVisualOdometry::preProcess()
         capture_image_last_->addKeyPoints(mwkps_last_new);
 
     }
-    else
-    {
-        WOLF_DEBUG("\n\n");
-    }
 
     // Update captures
     capture_image_incoming_->addKeyPoints(mwkps_incoming_filtered);
@@ -539,19 +522,11 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
 
 void ProcessorVisualOdometry::postProcess()
 {
-    frame_count_ ++;
-
-    // delete tracks with no keyframes
-    for (auto track_it = track_matrix_.getTracks().begin(); track_it != track_matrix_.getTracks().end(); /* do not increment iterator yet... */)
+    // Delete tracks with no keyframes
+    for (const auto& track_id : track_matrix_.trackIds())
     {
-        auto track_id = track_it->first;
         if (track_matrix_.trackAtKeyframes(track_id).empty())
-        {
-            ++track_it;                      // ... increment iterator **before** erasing the element!!!
             track_matrix_.remove(track_id);
-        }
-        else
-            ++track_it;
     }
 
     // print a bar with the number of active tracks in incoming
@@ -562,8 +537,6 @@ void ProcessorVisualOdometry::postProcess()
     n = getProblem()->getMap()->getLandmarkList().size();
     s = std::string(n/2, '-');
     WOLF_INFO("LMARKS: ", n, " ", s);
-
-
 }
 
 bool ProcessorVisualOdometry::voteForKeyFrame() const
-- 
GitLab