From 0ef315c91bab61c6bd5056a621a53a463234b8f8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 27 Jun 2019 13:21:15 +0200
Subject: [PATCH] bug fixed

---
 src/processor/processor_tracker_feature.cpp | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index f27d3b9c1..33ec7f2a6 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -97,19 +97,22 @@ unsigned int ProcessorTrackerFeature::processKnown()
     // Check/correct incoming-origin correspondences
     if (origin_ptr_ != nullptr)
     {
-        for (auto feature_in_incoming : known_features_incoming_)
+        auto ftr_inc_it = known_features_incoming_.begin();
+        while (ftr_inc_it != known_features_incoming_.end())
         {
-            SizeStd        track_id          = feature_in_incoming->trackId();
+            SizeStd        track_id          = (*ftr_inc_it)->trackId();
             FeatureBasePtr feature_in_last   = track_matrix_.feature(track_id, last_ptr_);
             FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_);
-            if (!correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
+            if (!correctFeatureDrift(feature_in_origin, feature_in_last, (*ftr_inc_it)))
             {
                 // Remove this feature from many places:
-                matches_last_from_incoming_ .erase (feature_in_incoming); // remove match
-                track_matrix_               .remove(feature_in_incoming); // remove from track matrix
-                known_features_incoming_    .remove(feature_in_incoming); // remove from known features list
-                feature_in_incoming        ->remove();                    // remove from wolf tree
+                matches_last_from_incoming_          .erase (*ftr_inc_it);  // remove match
+                track_matrix_                        .remove(*ftr_inc_it);  // remove from track matrix
+                ftr_inc_it = known_features_incoming_.erase(ftr_inc_it);    // remove from known features list
+                (*ftr_inc_it)->                       remove();             // remove from wolf tree
             }
+            else
+                ftr_inc_it++;
         }
     }
 
-- 
GitLab