From b649fdf434c4f0a08bacde7735b12ab1e2c23904 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 10 Apr 2020 17:18:59 +0200
Subject: [PATCH] working!
---
include/core/processor/processor_tracker.h | 1 +
src/processor/processor_tracker.cpp | 2 +-
src/processor/processor_tracker_feature.cpp | 9 +++++++--
src/processor/processor_tracker_landmark.cpp | 20 +++++++++++++-------
4 files changed, 22 insertions(+), 10 deletions(-)
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index d7ab95164..f10ade8dc 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 56d5ae4ae..2c6982678 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 211a1aba1..be7767a14 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 7035dd38d..4e71ea537 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
--
GitLab