diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index 58aa4d53b826e4a85f3d7f8fcd4d0b1c483dbbb9..f27d3b9c1bc6f664e135b4ca6893b6a977e7d206 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -37,11 +37,23 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features) // Populate the last Capture with new Features. The result is in new_features_last_. unsigned int n = detectNewFeatures(_max_new_features, last_ptr_, new_features_last_); + + // check all features have been emplaced + assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) && + "any not linked feature returned by detectNewFeatures()"); + + // fill the track matrix for (auto ftr : new_features_last_) track_matrix_.newTrack(ftr); // Track new features from last to incoming. This will append new correspondences to matches_last_incoming trackFeatures(new_features_last_, incoming_ptr_, new_features_incoming_, matches_last_from_incoming_); + + // check all features have been emplaced + assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) && + "any not linked feature returned by trackFeatures()"); + + // fill the track matrix for (auto ftr : new_features_incoming_) { assert(matches_last_from_incoming_.count(ftr) != 0); @@ -71,6 +83,11 @@ unsigned int ProcessorTrackerFeature::processKnown() known_features_incoming_, matches_last_from_incoming_); + // check all features have been emplaced + assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) && + "any not linked feature returned by trackFeatures()"); + + // fill the track matrix for (auto ftr : known_features_incoming_) { assert(matches_last_from_incoming_.count(ftr) != 0); @@ -135,6 +152,8 @@ void ProcessorTrackerFeature::establishFactors() auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin); + assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()"); + WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), " origin: " , feature_in_origin->id() , " from last: " , feature_in_last->id() ); diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 18d869706625f01050367d79ade911fb66757876..0d7c50c2a9e6ac8a3e91009a1ed361251a4104aa 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -57,6 +57,10 @@ unsigned int ProcessorTrackerLandmark::processKnown() known_features_list_incoming, matches_landmark_from_incoming_); + // check all features have been emplaced + assert(std::all_of(known_features_list_incoming.begin(), known_features_list_incoming.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) && + "any not linked feature returned by findLandmarks()"); + return n; } @@ -79,6 +83,10 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features) last_ptr_, new_features_last_); + // check all features have been emplaced + assert(std::all_of(new_features_last_.begin(), new_features_last_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) && + "any not linked feature returned by detectNewFeatures()"); + // create new landmarks with the new features discovered emplaceNewLandmarks(); @@ -89,6 +97,10 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features) incoming_ptr_, new_features_incoming_, matches_landmark_from_incoming_); + + // check all features have been emplaced + assert(std::all_of(new_features_incoming_.begin(), new_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) && + "any not linked feature returned by findLandmarks()"); } // return the number of new features detected in \b last @@ -106,6 +118,8 @@ void ProcessorTrackerLandmark::emplaceNewLandmarks() // create new landmark LandmarkBasePtr new_lmk_ptr = emplaceLandmark(new_feature_ptr); + assert(new_lmk_ptr->getMap() != nullptr && "not linked landmark returned by emplaceLandmark()"); + new_landmarks_.push_back(new_lmk_ptr); // create new correspondence @@ -119,7 +133,10 @@ void ProcessorTrackerLandmark::establishFactors() for (auto last_feature : last_ptr_->getFeatureList()) { assert(matches_landmark_from_last_.count(last_feature) == 1); + FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_); + + assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()"); } }