diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index d7ab951643fbc2760b8d81110a9f2338a96dad36..f10ade8dca3daf70de737fe7123f203017bb9685 100644 --- a/include/core/processor/processor_tracker.h +++ b/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; diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 56d5ae4ae45ed40962274292328076087da7fb9f..2c698267838a3ea4dbe7098bcb871012ac0ad7ad 100644 --- a/src/processor/processor_tracker.cpp +++ b/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 diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index 211a1aba12b12ecfb10afea00041cf82e07493be..be7767a14d79d7de703a19350190444f1be648ab 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/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_)) diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 7035dd38de2fcfb177deef2f4ed30d28fcdd92cf..4e71ea537dd3f82b61bb3da3d6bce98b9cec6469 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/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