diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index b0d02ec63273b32ef61e26aa7059917a89fee82b..8a6dd8fdc5370ef980ed93136809bc0a721cc3c3 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -190,13 +190,18 @@ void ProcessorVisualOdometry::preProcess()
     }
     WOLF_INFO( "Retained " , tracks_last_incoming_filtered.size(), " inliers..." );
 
+    // Remove from mwkps_incoming all the keypoints that have not been tracked
+    // TODO: there might be a faster way than creating a new map from scratch?
+    KeyPointsMap mwkps_incoming_fitered;
+    for (auto track : tracks_last_incoming_filtered)
+        mwkps_incoming_fitered[track.second] = mwkps_incoming[track.second];
+
     // Update captures
-    capture_image_incoming_->addKeyPoints(mwkps_incoming);
+    capture_image_incoming_->addKeyPoints(mwkps_incoming_fitered);
     capture_image_incoming_->setTracksPrev(tracks_last_incoming_filtered);
     capture_image_incoming_->setTracksOrigin(tracks_origin_incoming);
 
 
-    // Add valid 
 
     ////////////////////////////////
     // if too few tracks left in incoming