diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 59ee59659227c8ca889d68f3a776744e30b95223..2b5ff2126c19684bca11bf6da230114dd671b452 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -33,15 +33,23 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
for (auto ftr : new_features_last_)
{
ftr->setTrackId( ftr->id() );
- WOLF_INFO("New ftr track: ", ftr->trackId(), ", last: ", ftr->id());
+ WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", ftr->id());
}
// Track new features from last to incoming. This will append new correspondences to matches_last_incoming
trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
+ for (auto ftr : new_features_incoming_)
+ {
+ ftr->setTrackId(matches_last_from_incoming_[ftr]->feature_ptr_->trackId());
+
+ // Print new tracked features
+ WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", matches_last_from_incoming_[ftr]->feature_ptr_->id(), ", inc: ", ftr->id());
+ }
+
+ // Print all tracks
for (auto match : matches_last_from_incoming_)
{
- match.first->setTrackId( match.second->feature_ptr_->trackId() );
- WOLF_INFO("New ftr track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
+ WOLF_DEBUG("New total track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
}
// Append all new Features to the incoming Captures' list of Features
@@ -56,9 +64,6 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
unsigned int ProcessorTrackerFeature::processKnown()
{
-
-// std::cout << "ProcessorTrackerFeature::processKnown()" << std::endl;
-
assert(incoming_ptr_->getFeatureList().size() == 0
&& "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
assert(matches_last_from_incoming_.size() == 0
@@ -67,12 +72,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
// Track features from last_ptr_ to incoming_ptr_
trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_);
for (auto match : matches_last_from_incoming_)
- {
match.first->setTrackId( match.second->feature_ptr_->trackId() );
- WOLF_INFO("Known ftr track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
- }
-
-// std::cout << "Tracked: " << known_features_incoming_.size() << std::endl;
// Check/correct incoming-origin correspondences
if (origin_ptr_ != nullptr)
@@ -93,21 +93,21 @@ unsigned int ProcessorTrackerFeature::processKnown()
known_incoming_feature_it++;
}
}
- // std::cout << "known features incoming: ";
- // for (auto ftr:known_features_incoming_)
- // std::cout << ftr->id() << ", ";
- // std::cout << std::endl;
- // Append remaining incoming features -> this empties known_features_incoming_
+
incoming_ptr_->addFeatureList(known_features_incoming_);
known_features_incoming_.clear();
- std::cout << "Added to incoming features: " << incoming_ptr_->getFeatureList().size() << std::endl;
+
+ // Print resulting list of matches
+ for (auto match : matches_last_from_incoming_)
+ {
+ WOLF_DEBUG("Known total track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
+ }
return matches_last_from_incoming_.size();
}
void ProcessorTrackerFeature::advanceDerived()
{
- // std::cout << "ProcessorTrackerFeature::advance()" << std::endl;
// Compose correspondences to get origin_from_incoming
for (auto match : matches_last_from_incoming_)
{
@@ -116,37 +116,31 @@ void ProcessorTrackerFeature::advanceDerived()
}
matches_origin_from_last_ = std::move(matches_last_from_incoming_);
-
-// std::cout << "advanced correspondences: " << std::endl;
-// std::cout << "\tincoming 2 last: " << matches_last_from_incoming_.size() << std::endl;
-// for (auto match : matches_last_from_incoming_)
-// std::cout << "inc -> last: \t\t" << match.second->feature_ptr_->id() << " <- " << match.first->id() << std::endl;
-// std::cout << "\tlast 2 origin: " << std::endl;
-// for (auto match : matches_origin_from_last_)
-// std::cout << "ori <- last: \t\t" << match.second->feature_ptr_->id() << match.first->id() << std::endl;
-
// We set problem here because we could not determine Problem from incoming capture at the time of adding the features to incoming's feature list.
for (auto ftr : incoming_ptr_->getFeatureList())
ftr->setProblem(getProblem());
+
+ // Print resulting list
+ for (auto match: matches_origin_from_last_)
+ {
+ WOLF_DEBUG("Matches advanced: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
+ }
}
void ProcessorTrackerFeature::resetDerived()
{
// We also reset here the list of correspondences, which passes from last--incoming to origin--last.
-
- std::cout << "\tincoming 2 last: " << matches_last_from_incoming_.size() << std::endl;
- for (auto match : matches_last_from_incoming_)
- std::cout << "last <- inc: \t\t" << match.second->feature_ptr_->id() << " <- " << match.first->id() << std::endl;
- std::cout << "\tlast 2 origin: " << std::endl;
- for (auto match : matches_origin_from_last_)
- std::cout << "ori <- last: \t\t" << match.second->feature_ptr_->id() << " <- " << match.first->id() << std::endl;
-
matches_origin_from_last_ = std::move(matches_last_from_incoming_);
// Update features according to the move above.
for (auto match: matches_origin_from_last_)
match.first->setProblem(getProblem()); // Since these features were in incoming_, they had no Problem assigned.
+ // Print resulting list
+ for (auto match: matches_origin_from_last_)
+ {
+ WOLF_DEBUG("Matches reset: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
+ }
}
void ProcessorTrackerFeature::establishConstraints()
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index 4dc8bd1199d35639bbd49b26b42c3c34da8a7dc5..9ffd9e2e4be6c001f996be7639b68b01c33b8928 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -15,7 +15,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
FeatureBaseList& _feature_list_out,
FeatureMatchMap& _feature_correspondences)
{
- std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl;
+ WOLF_INFO("tracking " , _feature_list_in.size() , " features...");
// loosing the track of the first 2 features
auto features_lost = 0;
@@ -25,14 +25,16 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
if ( rand() % static_cast<int>(101) < 30 )
{
features_lost++;
- std::cout << "feature " << feat_in_ptr->id() << " lost!" << std::endl;
+
+ WOLF_INFO("track: " , feat_in_ptr->trackId() , " feature: " , feat_in_ptr->id() , " lost!");
}
else
{
FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
_feature_list_out.push_back(ftr);
_feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
- std::cout << "feature " << feat_in_ptr->id() << " tracked to feature " << ftr->id() << " !" << std::endl;
+
+ WOLF_INFO("track: " , feat_in_ptr->trackId() , " last: " , feat_in_ptr->id() , " inc: " , ftr->id() , " !" );
}
}
@@ -41,27 +43,32 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
{
- std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
+ WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
+
bool vote = incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
- std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
+
+ WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
return incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
}
unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features)
{
- std::cout << "Detecting " << _max_features << " new features..." << std::endl;
+ WOLF_INFO("Detecting " , _max_features , " new features..." );
// detecting new features
for (unsigned int i = 1; i <= _max_features; i++)
{
n_feature_++;
- FeatureBasePtr ftr(
- std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
+ FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
+ n_feature_* Eigen::Vector1s::Ones(),
+ Eigen::MatrixXs::Ones(1, 1)));
new_features_last_.push_back(ftr);
- std::cout << "feature " << ftr->id() << " detected!" << std::endl;
+
+ WOLF_INFO("feature " , ftr->id() , " detected!" );
}
- std::cout << new_features_last_.size() << " features detected!" << std::endl;
+
+ WOLF_INFO(new_features_last_.size() , " features detected!");
return new_features_last_.size();
}
@@ -69,11 +76,11 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- std::cout << "creating constraint: last feature " << _feature_ptr->id()
- << " with origin feature " << _feature_other_ptr->id() << std::endl;
+ WOLF_INFO( "creating constraint: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
+ , " with origin feature " , _feature_other_ptr->id() );
+
auto ctr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
- // _feature_ptr->addConstraint(ctr);
- // _feature_other_ptr->addConstrainedBy(ctr);
+
return ctr;
}
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
index ef654f8a58092fcb6efc6850b81261ac6dd505f6..2e061da25b7b9294b1da48b0ecfda79412a0eb39 100644
--- a/src/test/gtest_processor_base.cpp
+++ b/src/test/gtest_processor_base.cpp
@@ -222,7 +222,7 @@ TEST(ProcessorBase, KeyFrameCallback)
CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
proc_trk->process(capt_trk);
- for (size_t ii=0; ii<6; ii++ )
+ for (size_t ii=0; ii<10; ii++ )
{
// Move
t = t+dt;