diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 1d45c864d5c946f4f61fbd8d7a885ff9e53419ec..643da28549ea6496b76363e156acde7bed6c859f 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 81b8247ff0ba39104dbd275ba9981a87416fa44b..b0df0fecb8823cd35656b8fbae010eacc2eb3fab 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