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