From 236615cc8953c67c401ba7ee5aac18b599cd4715 Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Sat, 16 Apr 2022 18:35:37 +0200 Subject: [PATCH] Implement retainBest() to get exactly the best N points... Code copied from https://stackoverflow.com/questions/29085594/cvkeypointsfilterretainbest-isnt-working-as-intended --- .../vision/processor/processor_visual_odometry.h | 4 +++- src/processor/processor_visual_odometry.cpp | 13 ++++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h index 1d45c864d..643da2854 100644 --- a/include/vision/processor/processor_visual_odometry.h +++ b/include/vision/processor/processor_visual_odometry.h @@ -244,8 +244,10 @@ class ProcessorVisualOdometry : public ProcessorTracker const TrackMatrix& getTrackMatrix() const {return track_matrix_;} -}; + private: + void retainBest(std::vector<cv::KeyPoint> &_keypoints, int n); +}; } //namespace wolf diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 81b8247ff..b0df0fecb 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -634,7 +634,18 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev return true; } - +VisualOdometry::retainBest(std::vector<cv::KeyPoint> &_keypoints, int n) +{ + if (_keypoints.size() > n) { + if (n == 0) { + _keypoints.clear(); + return; + } + std::nth_element(_keypoints.begin(), _keypoints.begin() + n, _keypoints.end(), + [](cv::KeyPoint& a, cv::KeyPoint& b) { return a.response > b.response; }); + _keypoints.resize(n); + } +} } //namespace wolf // Register in the FactoryProcessor -- GitLab