Added asserts checking emplaced features

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() );

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()");
     }
 }