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