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