Skip to content
Snippets Groups Projects
Commit 6e86aca1 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Added asserts checking emplaced features

parent 471007b5
No related branches found
No related tags found
1 merge request!274Resolve "Emplace API inconsistent with ProcessorTrackerFeature/Landmark functions"
Pipeline #3824 passed
......@@ -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() );
......
......@@ -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()");
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment