Implement retainBest() to get exactly the best N points...

Code copied from https://stackoverflow.com/questions/29085594/cvkeypointsfilterretainbest-isnt-working-as-intended

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
 

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