Bug fixed: remove elements in a for loop

in ProcessorTrackerFeature::processNew() there was a bug: inside a for range loop, an element of the list in which we iterate is removed if correctFeatureDrift() returns false. Changed to while with iterators.

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++;
         }
     }