Merge branch 'devel' into 22-adapt-to-core-cmakelists-txt-refactor

include/vision/processor/processor_visual_odometry.h

@@ -198,8 +198,6 @@ class ProcessorVisualOdometry : public ProcessorTracker
         ActiveSearchGrid cell_grid_;
 
     private:
-        int frame_count_;
-
         // camera
         cv::Mat Kcv_;
 

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,31 +522,27 @@ 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
+    // print a bar with the number of active features in incoming
     unsigned int n = capture_image_incoming_->getKeyPoints().size();
     std::string s(n/2, '#');
-    WOLF_INFO("TRACKS: ", n, " ", s);
+    WOLF_INFO("FEATRS/2: ", n, " ", s);
 
+    // print a bar with the number of active tracks
+    n = track_matrix_.trackIds().size();
+    s = std::string(n/4, 'o');
+    WOLF_INFO("TRACKS/4: ", n, " ", s);
+
+    // print a bar with the number of landmarks
     n = getProblem()->getMap()->getLandmarkList().size();
     s = std::string(n/2, '-');
-    WOLF_INFO("LMARKS: ", n, " ", s);
-
-
+    WOLF_INFO("LMARKS/2: ", n, " ", s);
 }
 
 bool ProcessorVisualOdometry::voteForKeyFrame() const