From 31d15fa1499fa553f72c113be6a4d10340ccf21b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 13 Feb 2024 11:51:54 +0100
Subject: [PATCH] ProcessorLandmarkExternal less verbose and cleanup
---
src/processor/processor_landmark_external.cpp | 308 +++++++-----------
1 file changed, 119 insertions(+), 189 deletions(-)
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index 1544904df..7a2de8df3 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -87,9 +87,9 @@ void ProcessorLandmarkExternal::preProcess()
// add new feature
new_features_incoming_.push_back(ftr);
}
- WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ",
- new_features_incoming_.size(),
- " features in incoming capture");
+ WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
+ new_features_incoming_.size(),
+ " features in incoming capture");
}
unsigned int ProcessorLandmarkExternal::processKnown()
@@ -100,7 +100,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
// Track features from last_ptr_ to incoming_ptr_
if (not last_ptr_) return 0;
- WOLF_INFO("Searching ", known_features_last_.size(), " tracked features...");
+ WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
auto pose_sen = getSensor()->getState("PO");
auto pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
@@ -109,30 +109,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
{
auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
bool matched = false;
- WOLF_INFO("Tracking feature last: ",
- feat_lmk_last->id(),
- " - ID: ",
- feat_lmk_last->getExternalId(),
- " - TYPE: ",
- feat_lmk_last->getExternalType(),
- " meas: ",
- feat_lmk_last->getMeasurement().transpose());
-
- // auto last_global_meas = (dim_ == 2 ? Eigen::VectorXd(pose_last.at('P') +
- // Rotation2Dd(pose_last.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_last->getMeasurement().head<2>()))
- // : Eigen::VectorXd(pose_last.at('P') +
- // Quaterniond(Vector4d(pose_last.at('O'))) *
- // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
- // feat_lmk_last->getMeasurement().head<3>())));
- // WOLF_INFO("pose_last: ", pose_last);
- // WOLF_INFO("pose_sen: ", pose_sen);
- // auto last_global_meas =
- // pose_last.at('P') +
- // Rotation2Dd(pose_last.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>());
- // WOLF_INFO("Last global meas: ", last_global_meas.transpose());
+ WOLF_DEBUG("Tracking feature last: ",
+ feat_lmk_last->id(),
+ " - ID: ",
+ feat_lmk_last->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_last->getExternalType(),
+ " meas: ",
+ feat_lmk_last->getMeasurement().transpose());
// First we try to match by EXTERNAL_ID
if (feat_lmk_last->getExternalId() != -1)
@@ -142,32 +126,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
{
auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
- WOLF_INFO("Feature incoming candidate (by ID): ",
- feat_lmk_incoming->id(),
- " - ID: ",
- feat_lmk_incoming->getExternalId(),
- " - TYPE: ",
- feat_lmk_incoming->getExternalType(),
- " meas: ",
- feat_lmk_incoming->getMeasurement().transpose());
-
- // auto global_meas =
- // (dim_ == 2
- // ? Eigen::VectorXd(pose_incoming.at('P') +
- // Rotation2Dd(pose_incoming.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_incoming->getMeasurement().head<2>()))
- // : Eigen::VectorXd(
- // pose_incoming.at('P') +
- // Quaterniond(Vector4d(pose_incoming.at('O'))) *
- // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
- // feat_lmk_incoming->getMeasurement().head<3>())));
- // auto global_meas =
- // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_incoming->getMeasurement().head<2>());
- // WOLF_INFO("pose_incoming: ", pose_incoming);
- // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
+ WOLF_DEBUG("Feature incoming candidate (by ID): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
// MATCH NECESSARY CONDITIONS:
// 1. Same EXTERNAL_ID
@@ -177,16 +143,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
(feat_lmk_incoming->getExternalType() == -1 or // cond 2
feat_lmk_last->getExternalType() == -1 or //
feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
- detectionDistance(
- feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
- params_tfle_->match_dist_th_id)
+ detectionDistance( // cond 3
+ feat_lmk_last,
+ feat_lmk_incoming,
+ pose_last,
+ pose_incoming,
+ pose_sen) < params_tfle_->match_dist_th_id)
{
- WOLF_INFO("Feature last: ",
- feat_lmk_last->id(),
- " matched with feature ",
- feat_lmk_incoming->id(),
- " with landmark ID: ",
- feat_lmk_last->landmarkId());
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
matched = true;
// set LANDMARK_ID if defined
@@ -217,32 +186,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
while (feature_incoming_it != new_features_incoming_.end())
{
auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
- WOLF_INFO("Feature incoming candidate (by TYPE): ",
- feat_lmk_incoming->id(),
- " - ID: ",
- feat_lmk_incoming->getExternalId(),
- " - TYPE: ",
- feat_lmk_incoming->getExternalType(),
- " meas: ",
- feat_lmk_incoming->getMeasurement().transpose());
-
- // auto global_meas =
- // (dim_ == 2
- // ? Eigen::VectorXd(pose_incoming.at('P') +
- // Rotation2Dd(pose_incoming.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_incoming->getMeasurement().head<2>()))
- // : Eigen::VectorXd(
- // pose_incoming.at('P') +
- // Quaterniond(Vector4d(pose_incoming.at('O'))) *
- // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
- // feat_lmk_incoming->getMeasurement().head<3>())));
- // auto global_meas =
- // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_incoming->getMeasurement().head<2>());
- // WOLF_INFO("pose_incoming: ", pose_incoming);
- // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
+ WOLF_DEBUG("Feature incoming candidate (by TYPE): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
// MATCH NECESSARY CONDITIONS:
// 1. Compatible EXTERNAL_ID (either not defined or same)
@@ -252,16 +203,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
feat_lmk_last->getExternalId() == -1 or //
feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType() and // cond 2
- detectionDistance(
- feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
- params_tfle_->match_dist_th_type)
+ detectionDistance( // cond 3
+ feat_lmk_last,
+ feat_lmk_incoming,
+ pose_last,
+ pose_incoming,
+ pose_sen) < params_tfle_->match_dist_th_type)
{
- WOLF_INFO("Feature last: ",
- feat_lmk_last->id(),
- " matched with feature ",
- feat_lmk_incoming->id(),
- " with landmark ID: ",
- feat_lmk_last->landmarkId());
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
matched = true;
// set LANDMARK_ID last -> incoming
@@ -295,32 +249,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
{
auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
- WOLF_INFO("Feature incoming candidate (by distance): ",
- feat_lmk_incoming->id(),
- " - ID: ",
- feat_lmk_incoming->getExternalId(),
- " - TYPE: ",
- feat_lmk_incoming->getExternalType(),
- " meas: ",
- feat_lmk_incoming->getMeasurement().transpose());
-
- // auto global_meas =
- // (dim_ == 2
- // ? Eigen::VectorXd(pose_incoming.at('P') +
- // Rotation2Dd(pose_incoming.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_incoming->getMeasurement().head<2>()))
- // : Eigen::VectorXd(
- // pose_incoming.at('P') +
- // Quaterniond(Vector4d(pose_incoming.at('O'))) *
- // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
- // feat_lmk_incoming->getMeasurement().head<3>())));
- // auto global_meas =
- // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
- // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
- // feat_lmk_incoming->getMeasurement().head<2>());
- // WOLF_INFO("pose_incoming: ", pose_incoming);
- // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
+ WOLF_DEBUG("Feature incoming candidate (by distance): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
// MATCH NECESSARY CONDITIONS:
// 1. Compatible EXTERNAL_ID (either not defined or same)
@@ -335,12 +271,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
params_tfle_->match_dist_th_unknown)
{
- WOLF_INFO("Feature last: ",
- feat_lmk_last->id(),
- " matched with feature ",
- feat_lmk_incoming->id(),
- " with landmark ID: ",
- feat_lmk_last->landmarkId());
+ WOLF_DEBUG("Feature last: ",
+ feat_lmk_last->id(),
+ " matched with feature ",
+ feat_lmk_incoming->id(),
+ " with landmark ID: ",
+ feat_lmk_last->landmarkId());
matched = true;
// set LANDMARK_ID last -> incoming
@@ -369,7 +305,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
}
WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked.");
}
- WOLF_INFO("Tracked ", known_features_incoming_.size(), " features.");
+ WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
// Add new features (not tracked) as known features
WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
@@ -467,7 +403,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
{
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
- WOLF_INFO("Active feature tracks: ", track_ids_last.size());
+ WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
// number of tracks longer than track_length_th
auto n_tracks = 0;
@@ -483,55 +419,49 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
// Necessary condition: active valid tracks
bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
- WOLF_INFO("vote_min_features: ",
- vote_min_features,
- " - Active feature tracks longer than ",
- params_tfle_->track_length_th,
- ": ",
- n_tracks,
- " (should be equal or bigger than ",
- params_tfle_->min_features_for_keyframe,
- ")");
+ WOLF_DEBUG("vote_min_features: ",
+ vote_min_features,
+ " - Active feature tracks longer than ",
+ params_tfle_->track_length_th,
+ ": ",
+ n_tracks,
+ " (should be equal or bigger than ",
+ params_tfle_->min_features_for_keyframe,
+ ")");
bool vote_new_features(true), vote_time_span(true);
if (vote_min_features and origin_ptr_)
{
// Sufficient condition: new valid tracks
vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
- WOLF_INFO("vote_new_features: ",
- vote_new_features,
- " - n_new_tracks = ",
- n_new_tracks,
- " (should be equal or bigger than ",
- params_tfle_->new_features_for_keyframe,
- ")");
+ WOLF_DEBUG("vote_new_features: ",
+ vote_new_features,
+ " - n_new_tracks = ",
+ n_new_tracks,
+ " (should be equal or bigger than ",
+ params_tfle_->new_features_for_keyframe,
+ ")");
// Sufficient condition: time span
vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
- WOLF_INFO("vote_time_span: ",
- vote_time_span,
- " - time_span = ",
- last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
- " (should be bigger than ",
- params_tfle_->time_span,
- ")");
+ WOLF_DEBUG("vote_time_span: ",
+ vote_time_span,
+ " - time_span = ",
+ last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+ " (should be bigger than ",
+ params_tfle_->time_span,
+ ")");
}
bool vote = vote_min_features and (vote_new_features or vote_time_span);
- WOLF_INFO((vote ? "Vote " : "Do NOT vote "), "for KF");
+ WOLF_DEBUG((vote ? "Vote " : "Do NOT vote "), "for KF");
return vote;
}
void ProcessorLandmarkExternal::establishFactors()
{
- WOLF_INFO("establishFactors");
- // if (origin_ptr_ == last_ptr_) return;
-
- // // reset n_tracks_origin_
- // lmks_ids_origin_.clear();
-
// will emplace a factor (and landmark if needed) for each known feature in last with long tracks
FactorBasePtrList fac_list;
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
@@ -542,19 +472,19 @@ void ProcessorLandmarkExternal::establishFactors()
{
auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
- WOLF_INFO("Feature ",
- feature->id(),
- ": ID: ",
- feature->landmarkId(),
- " EXTERNAL_ID: ",
- feature->getExternalId(),
- " TYPE: ",
- feature->getExternalType());
+ WOLF_DEBUG("Feature ",
+ feature->id(),
+ ": ID: ",
+ feature->landmarkId(),
+ " EXTERNAL_ID: ",
+ feature->getExternalId(),
+ " TYPE: ",
+ feature->getExternalType());
// not enough long track
- WOLF_INFO_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
- "Track NOT long enough ",
- track_matrix_.trackSize(track_id));
+ WOLF_DEBUG_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
+ "Track NOT long enough ",
+ track_matrix_.trackSize(track_id));
if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue;
// Landmark match
@@ -563,7 +493,7 @@ void ProcessorLandmarkExternal::establishFactors()
{
// LOOP CLOSURE
// By ID
- WOLF_INFO("Searching Loop closure by ID...");
+ WOLF_DEBUG("Searching Loop closure by ID...");
if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1)
{
auto lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -576,16 +506,16 @@ void ProcessorLandmarkExternal::establishFactors()
detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id)
{
lmk = lmk_ext;
- WOLF_INFO("Found loop closure by EXTERNAL_ID with ", lmk->id());
+ WOLF_DEBUG("Found loop closure by EXTERNAL_ID with ", lmk->id());
break;
}
- WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
- "Landmark with EXTERNAL_ID found but not matched due to distance: ",
- detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
+ WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
+ "Landmark with EXTERNAL_ID found but not matched due to distance: ",
+ detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
}
}
// By TYPE
- WOLF_INFO("Searching Loop closure by TYPE...");
+ WOLF_DEBUG("Searching Loop closure by TYPE...");
if (not lmk and params_tfle_->close_loops_by_type and feature->getExternalType() != -1)
{
auto lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -598,12 +528,12 @@ void ProcessorLandmarkExternal::establishFactors()
detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type)
{
lmk = lmk_ext;
- WOLF_INFO("Found loop closure by TYPE with ", lmk->id());
+ WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id());
break;
}
- WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
- "Landmark with TYPE found but not matched due to distance: ",
- detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
+ WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
+ "Landmark with TYPE found but not matched due to distance: ",
+ detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
}
// update Landmark EXTERNAL_ID (if available)
@@ -614,17 +544,17 @@ void ProcessorLandmarkExternal::establishFactors()
if (not lmk)
{
lmk = emplaceLandmark(feature);
- WOLF_INFO("Emplaced new landmark ", lmk->id());
+ WOLF_DEBUG("Emplaced new landmark ", lmk->id());
}
// set LANDMARK_ID in all features of the track
for (auto feat_pair : track_matrix_.track(track_id))
{
feat_pair.second->setLandmarkId(lmk->id());
- WOLF_INFO("Setting landmark id in feature ",
- feat_pair.second->id(),
- " landmark_id: ",
- feat_pair.second->landmarkId());
+ WOLF_DEBUG("Setting landmark id in feature ",
+ feat_pair.second->id(),
+ " landmark_id: ",
+ feat_pair.second->landmarkId());
}
}
// landmarkId already set
@@ -651,7 +581,7 @@ void ProcessorLandmarkExternal::establishFactors()
}
}
- WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
+ WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
}
FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
--
GitLab