diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index eaa92966964f27e7ca209590a32b79a0b6ae2ab4..18d869706625f01050367d79ade911fb66757876 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -28,28 +28,27 @@ ProcessorTrackerLandmark::~ProcessorTrackerLandmark() void ProcessorTrackerLandmark::advanceDerived() { - for (auto match : matches_landmark_from_last_) - { - match.second.reset(); // TODO: Should we just remove the entries? What about match.first? - } matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_); new_features_last_ = std::move(new_features_incoming_); + + matches_landmark_from_incoming_.clear(); + new_features_incoming_.clear(); + new_landmarks_.clear(); } void ProcessorTrackerLandmark::resetDerived() { - for (auto match : matches_landmark_from_last_) - { - match.second.reset(); // TODO: Should we just remove the entries? What about match.first? - } matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_); new_features_last_ = std::move(new_features_incoming_); + + matches_landmark_from_incoming_.clear(); + new_features_incoming_.clear(); + new_landmarks_.clear(); } unsigned int ProcessorTrackerLandmark::processKnown() { - // clear matches list - matches_landmark_from_incoming_.clear(); + assert(matches_landmark_from_incoming_.empty()); // Find landmarks in incoming_ptr_ FeatureBasePtrList known_features_list_incoming; @@ -66,18 +65,14 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features) /* Rationale: A keyFrame will be created using the last Capture. * * First, we work on this Capture to detect new Features, - * eventually create Landmarks with them, - * and in such case create the new Factors feature-landmark. - * When done, we need to track these new Features to the incoming Capture. - * - * At the end, all new Features are appended to the lists of known Features in - * the last and incoming Captures. + * eventually create Landmarks with them. + * When done, we need to find these new Landmarks in the incoming Capture. */ - // clear new lists - new_features_last_.clear(); - new_features_incoming_.clear(); - new_landmarks_.clear(); + // assuming cleared new lists + assert(new_features_last_.empty()); + assert(new_features_incoming_.empty()); + assert(new_landmarks_.empty()); // We first need to populate the \b last Capture with new Features unsigned int n = detectNewFeatures(_max_features, @@ -102,8 +97,8 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features) void ProcessorTrackerLandmark::emplaceNewLandmarks() { - // First, make sure the list is empty and will only contain new lmks - new_landmarks_.clear(); + // assuming empty list + assert(new_landmarks_.empty()); // Create a landmark for each new feature in last Capture: for (auto new_feature_ptr : new_features_last_)