working!

include/core/processor/processor_tracker.h

@@ -103,6 +103,7 @@ class ProcessorTracker : public ProcessorBase
         CaptureBasePtr incoming_ptr_;           ///< Pointer to the incoming capture being processed.
         FeatureBasePtrList new_features_last_;     ///< List of new features in \b last for landmark initialization and new key-frame creation.
         FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
+        FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
         FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
         int _dim;
 

src/processor/processor_tracker.cpp

@@ -170,7 +170,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // process
                 processNew(params_tracker_->max_new_features);
 
-                //TODO abort KF if last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe
+                //TODO abort KF if known_features_last_.size() < params_tracker_->min_features_for_keyframe
 
                 // We create a KF
                 // set KF on last

src/processor/processor_tracker_feature.cpp

@@ -79,14 +79,14 @@ unsigned int ProcessorTrackerFeature::processKnown()
     matches_last_from_incoming_.clear();
     known_features_incoming_.clear();
 
-    if (!last_ptr_ || last_ptr_->getFeatureList().empty())
+    if (!last_ptr_ || known_features_last_.empty())
     {
         WOLF_TRACE("Empty last feature list, returning...");
         return 0;
     }
 
     // Track features from last_ptr_ to incoming_ptr_
-    trackFeatures(last_ptr_->getFeatureList(),
+    trackFeatures(known_features_last_,
                   incoming_ptr_,
                   known_features_incoming_,
                   matches_last_from_incoming_);
@@ -132,6 +132,9 @@ void ProcessorTrackerFeature::advanceDerived()
 {
     // Reset here the list of correspondences.
     matches_last_from_incoming_.clear();
+    known_features_last_ = std::move(known_features_incoming_);
+    //new_features_incoming should be zero!
+    //known_features_last_.splice(new_features_incoming_);
 
     // // remove last from track matrix in case you want to have only KF in the track matrix
     // track_matrix_.remove(last_ptr_);
@@ -141,6 +144,8 @@ void ProcessorTrackerFeature::resetDerived()
 {
     // Reset here the list of correspondences.
     matches_last_from_incoming_.clear();
+    known_features_last_ = std::move(known_features_incoming_);
+    known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
 
     // Debug
     //for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))

src/processor/processor_tracker_landmark.cpp

@@ -30,6 +30,8 @@ void ProcessorTrackerLandmark::advanceDerived()
 {
     matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
     new_features_last_ = std::move(new_features_incoming_);
+    known_features_last_ = std::move(known_features_incoming_);
+    known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
 
     matches_landmark_from_incoming_.clear();
     new_features_incoming_.clear();
@@ -40,6 +42,8 @@ void ProcessorTrackerLandmark::resetDerived()
 {
     matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
     new_features_last_ = std::move(new_features_incoming_);
+    known_features_last_ = std::move(known_features_incoming_);
+    known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
 
     matches_landmark_from_incoming_.clear();
     new_features_incoming_.clear();
@@ -129,15 +133,17 @@ void ProcessorTrackerLandmark::emplaceNewLandmarks()
 
 void ProcessorTrackerLandmark::establishFactors()
 {
-    // Loop all features in last_ptr_
-    for (auto last_feature : last_ptr_->getFeatureList())
-    {
-        assert(matches_landmark_from_last_.count(last_feature) == 1);
+    // Loop all features tracked in last_ptr_ (two lists known_features_last_ and new_features_last_)
+    std::list<FeatureBasePtrList> both_lists{known_features_last_, new_features_last_};
+    for (auto feature_list : both_lists)
+        for (auto last_feature : feature_list)
+        {
+            assert(matches_landmark_from_last_.count(last_feature) == 1);
 
-        FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
+            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()");
-    }
+            assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()");
+        }
 }
 
 } // namespace wolf