Skip to content
Snippets Groups Projects
Commit 31d15fa1 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

ProcessorLandmarkExternal less verbose and cleanup

parent b24a8cdd
No related branches found
No related tags found
3 merge requests!476spdlog version upgrade,!473Rerefactor,!472Merge ProcessorLandmarkExternal
Pipeline #17594 canceled
...@@ -87,9 +87,9 @@ void ProcessorLandmarkExternal::preProcess() ...@@ -87,9 +87,9 @@ void ProcessorLandmarkExternal::preProcess()
// add new feature // add new feature
new_features_incoming_.push_back(ftr); new_features_incoming_.push_back(ftr);
} }
WOLF_INFO("ProcessorLandmarkExternal::preprocess: found ", WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
new_features_incoming_.size(), new_features_incoming_.size(),
" features in incoming capture"); " features in incoming capture");
} }
unsigned int ProcessorLandmarkExternal::processKnown() unsigned int ProcessorLandmarkExternal::processKnown()
...@@ -100,7 +100,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -100,7 +100,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
// Track features from last_ptr_ to incoming_ptr_ // Track features from last_ptr_ to incoming_ptr_
if (not last_ptr_) return 0; 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_sen = getSensor()->getState("PO");
auto pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO"); auto pose_last = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO"); auto pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
...@@ -109,30 +109,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -109,30 +109,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
{ {
auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last); auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
bool matched = false; bool matched = false;
WOLF_INFO("Tracking feature last: ", WOLF_DEBUG("Tracking feature last: ",
feat_lmk_last->id(), feat_lmk_last->id(),
" - ID: ", " - ID: ",
feat_lmk_last->getExternalId(), feat_lmk_last->getExternalId(),
" - TYPE: ", " - TYPE: ",
feat_lmk_last->getExternalType(), feat_lmk_last->getExternalType(),
" meas: ", " meas: ",
feat_lmk_last->getMeasurement().transpose()); 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());
// First we try to match by EXTERNAL_ID // First we try to match by EXTERNAL_ID
if (feat_lmk_last->getExternalId() != -1) if (feat_lmk_last->getExternalId() != -1)
...@@ -142,32 +126,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -142,32 +126,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
{ {
auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
WOLF_INFO("Feature incoming candidate (by ID): ", WOLF_DEBUG("Feature incoming candidate (by ID): ",
feat_lmk_incoming->id(), feat_lmk_incoming->id(),
" - ID: ", " - ID: ",
feat_lmk_incoming->getExternalId(), feat_lmk_incoming->getExternalId(),
" - TYPE: ", " - TYPE: ",
feat_lmk_incoming->getExternalType(), feat_lmk_incoming->getExternalType(),
" meas: ", " meas: ",
feat_lmk_incoming->getMeasurement().transpose()); 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());
// MATCH NECESSARY CONDITIONS: // MATCH NECESSARY CONDITIONS:
// 1. Same EXTERNAL_ID // 1. Same EXTERNAL_ID
...@@ -177,16 +143,19 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -177,16 +143,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
(feat_lmk_incoming->getExternalType() == -1 or // cond 2 (feat_lmk_incoming->getExternalType() == -1 or // cond 2
feat_lmk_last->getExternalType() == -1 or // feat_lmk_last->getExternalType() == -1 or //
feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and // feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType()) and //
detectionDistance( detectionDistance( // cond 3
feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3 feat_lmk_last,
params_tfle_->match_dist_th_id) feat_lmk_incoming,
pose_last,
pose_incoming,
pose_sen) < params_tfle_->match_dist_th_id)
{ {
WOLF_INFO("Feature last: ", WOLF_DEBUG("Feature last: ",
feat_lmk_last->id(), feat_lmk_last->id(),
" matched with feature ", " matched with feature ",
feat_lmk_incoming->id(), feat_lmk_incoming->id(),
" with landmark ID: ", " with landmark ID: ",
feat_lmk_last->landmarkId()); feat_lmk_last->landmarkId());
matched = true; matched = true;
// set LANDMARK_ID if defined // set LANDMARK_ID if defined
...@@ -217,32 +186,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -217,32 +186,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
while (feature_incoming_it != new_features_incoming_.end()) while (feature_incoming_it != new_features_incoming_.end())
{ {
auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
WOLF_INFO("Feature incoming candidate (by TYPE): ", WOLF_DEBUG("Feature incoming candidate (by TYPE): ",
feat_lmk_incoming->id(), feat_lmk_incoming->id(),
" - ID: ", " - ID: ",
feat_lmk_incoming->getExternalId(), feat_lmk_incoming->getExternalId(),
" - TYPE: ", " - TYPE: ",
feat_lmk_incoming->getExternalType(), feat_lmk_incoming->getExternalType(),
" meas: ", " meas: ",
feat_lmk_incoming->getMeasurement().transpose()); 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());
// MATCH NECESSARY CONDITIONS: // MATCH NECESSARY CONDITIONS:
// 1. Compatible EXTERNAL_ID (either not defined or same) // 1. Compatible EXTERNAL_ID (either not defined or same)
...@@ -252,16 +203,19 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -252,16 +203,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
feat_lmk_last->getExternalId() == -1 or // feat_lmk_last->getExternalId() == -1 or //
feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and // feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType() and // cond 2 feat_lmk_incoming->getExternalType() == feat_lmk_last->getExternalType() and // cond 2
detectionDistance( detectionDistance( // cond 3
feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3 feat_lmk_last,
params_tfle_->match_dist_th_type) feat_lmk_incoming,
pose_last,
pose_incoming,
pose_sen) < params_tfle_->match_dist_th_type)
{ {
WOLF_INFO("Feature last: ", WOLF_DEBUG("Feature last: ",
feat_lmk_last->id(), feat_lmk_last->id(),
" matched with feature ", " matched with feature ",
feat_lmk_incoming->id(), feat_lmk_incoming->id(),
" with landmark ID: ", " with landmark ID: ",
feat_lmk_last->landmarkId()); feat_lmk_last->landmarkId());
matched = true; matched = true;
// set LANDMARK_ID last -> incoming // set LANDMARK_ID last -> incoming
...@@ -295,32 +249,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -295,32 +249,14 @@ unsigned int ProcessorLandmarkExternal::processKnown()
{ {
auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
WOLF_INFO("Feature incoming candidate (by distance): ", WOLF_DEBUG("Feature incoming candidate (by distance): ",
feat_lmk_incoming->id(), feat_lmk_incoming->id(),
" - ID: ", " - ID: ",
feat_lmk_incoming->getExternalId(), feat_lmk_incoming->getExternalId(),
" - TYPE: ", " - TYPE: ",
feat_lmk_incoming->getExternalType(), feat_lmk_incoming->getExternalType(),
" meas: ", " meas: ",
feat_lmk_incoming->getMeasurement().transpose()); 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());
// MATCH NECESSARY CONDITIONS: // MATCH NECESSARY CONDITIONS:
// 1. Compatible EXTERNAL_ID (either not defined or same) // 1. Compatible EXTERNAL_ID (either not defined or same)
...@@ -335,12 +271,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -335,12 +271,12 @@ unsigned int ProcessorLandmarkExternal::processKnown()
detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3 detectionDistance(feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3
params_tfle_->match_dist_th_unknown) params_tfle_->match_dist_th_unknown)
{ {
WOLF_INFO("Feature last: ", WOLF_DEBUG("Feature last: ",
feat_lmk_last->id(), feat_lmk_last->id(),
" matched with feature ", " matched with feature ",
feat_lmk_incoming->id(), feat_lmk_incoming->id(),
" with landmark ID: ", " with landmark ID: ",
feat_lmk_last->landmarkId()); feat_lmk_last->landmarkId());
matched = true; matched = true;
// set LANDMARK_ID last -> incoming // set LANDMARK_ID last -> incoming
...@@ -369,7 +305,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() ...@@ -369,7 +305,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
} }
WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked."); 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 // Add new features (not tracked) as known features
WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size()); WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
...@@ -467,7 +403,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const ...@@ -467,7 +403,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
{ {
auto track_ids_last = track_matrix_.trackIds(last_ptr_); 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 // number of tracks longer than track_length_th
auto n_tracks = 0; auto n_tracks = 0;
...@@ -483,55 +419,49 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const ...@@ -483,55 +419,49 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
// Necessary condition: active valid tracks // Necessary condition: active valid tracks
bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe; bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
WOLF_INFO("vote_min_features: ", WOLF_DEBUG("vote_min_features: ",
vote_min_features, vote_min_features,
" - Active feature tracks longer than ", " - Active feature tracks longer than ",
params_tfle_->track_length_th, params_tfle_->track_length_th,
": ", ": ",
n_tracks, n_tracks,
" (should be equal or bigger than ", " (should be equal or bigger than ",
params_tfle_->min_features_for_keyframe, params_tfle_->min_features_for_keyframe,
")"); ")");
bool vote_new_features(true), vote_time_span(true); bool vote_new_features(true), vote_time_span(true);
if (vote_min_features and origin_ptr_) if (vote_min_features and origin_ptr_)
{ {
// Sufficient condition: new valid tracks // Sufficient condition: new valid tracks
vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe; vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
WOLF_INFO("vote_new_features: ", WOLF_DEBUG("vote_new_features: ",
vote_new_features, vote_new_features,
" - n_new_tracks = ", " - n_new_tracks = ",
n_new_tracks, n_new_tracks,
" (should be equal or bigger than ", " (should be equal or bigger than ",
params_tfle_->new_features_for_keyframe, params_tfle_->new_features_for_keyframe,
")"); ")");
// Sufficient condition: time span // Sufficient condition: time span
vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span; vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
WOLF_INFO("vote_time_span: ", WOLF_DEBUG("vote_time_span: ",
vote_time_span, vote_time_span,
" - time_span = ", " - time_span = ",
last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
" (should be bigger than ", " (should be bigger than ",
params_tfle_->time_span, params_tfle_->time_span,
")"); ")");
} }
bool vote = vote_min_features and (vote_new_features or vote_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; return vote;
} }
void ProcessorLandmarkExternal::establishFactors() 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 // will emplace a factor (and landmark if needed) for each known feature in last with long tracks
FactorBasePtrList fac_list; FactorBasePtrList fac_list;
auto track_ids_last = track_matrix_.trackIds(last_ptr_); auto track_ids_last = track_matrix_.trackIds(last_ptr_);
...@@ -542,19 +472,19 @@ void ProcessorLandmarkExternal::establishFactors() ...@@ -542,19 +472,19 @@ void ProcessorLandmarkExternal::establishFactors()
{ {
auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_)); auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
WOLF_INFO("Feature ", WOLF_DEBUG("Feature ",
feature->id(), feature->id(),
": ID: ", ": ID: ",
feature->landmarkId(), feature->landmarkId(),
" EXTERNAL_ID: ", " EXTERNAL_ID: ",
feature->getExternalId(), feature->getExternalId(),
" TYPE: ", " TYPE: ",
feature->getExternalType()); feature->getExternalType());
// not enough long track // not enough long track
WOLF_INFO_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th, WOLF_DEBUG_COND(track_matrix_.trackSize(track_id) < params_tfle_->track_length_th,
"Track NOT long enough ", "Track NOT long enough ",
track_matrix_.trackSize(track_id)); track_matrix_.trackSize(track_id));
if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue; if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue;
// Landmark match // Landmark match
...@@ -563,7 +493,7 @@ void ProcessorLandmarkExternal::establishFactors() ...@@ -563,7 +493,7 @@ void ProcessorLandmarkExternal::establishFactors()
{ {
// LOOP CLOSURE // LOOP CLOSURE
// By ID // 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) if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1)
{ {
auto lmk_list = getProblem()->getMap()->getLandmarkList(); auto lmk_list = getProblem()->getMap()->getLandmarkList();
...@@ -576,16 +506,16 @@ void ProcessorLandmarkExternal::establishFactors() ...@@ -576,16 +506,16 @@ void ProcessorLandmarkExternal::establishFactors()
detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id) detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id)
{ {
lmk = lmk_ext; 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; break;
} }
WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(), WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalId() == feature->getExternalId(),
"Landmark with EXTERNAL_ID found but not matched due to distance: ", "Landmark with EXTERNAL_ID found but not matched due to distance: ",
detectionDistance(feature, lmk_ext, pose_frm, pose_sen)) detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
} }
} }
// By TYPE // 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) if (not lmk and params_tfle_->close_loops_by_type and feature->getExternalType() != -1)
{ {
auto lmk_list = getProblem()->getMap()->getLandmarkList(); auto lmk_list = getProblem()->getMap()->getLandmarkList();
...@@ -598,12 +528,12 @@ void ProcessorLandmarkExternal::establishFactors() ...@@ -598,12 +528,12 @@ void ProcessorLandmarkExternal::establishFactors()
detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type) detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type)
{ {
lmk = lmk_ext; lmk = lmk_ext;
WOLF_INFO("Found loop closure by TYPE with ", lmk->id()); WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id());
break; break;
} }
WOLF_INFO_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(), WOLF_DEBUG_COND(lmk_ext and lmk_ext->getExternalType() == feature->getExternalType(),
"Landmark with TYPE found but not matched due to distance: ", "Landmark with TYPE found but not matched due to distance: ",
detectionDistance(feature, lmk_ext, pose_frm, pose_sen)) detectionDistance(feature, lmk_ext, pose_frm, pose_sen))
} }
// update Landmark EXTERNAL_ID (if available) // update Landmark EXTERNAL_ID (if available)
...@@ -614,17 +544,17 @@ void ProcessorLandmarkExternal::establishFactors() ...@@ -614,17 +544,17 @@ void ProcessorLandmarkExternal::establishFactors()
if (not lmk) if (not lmk)
{ {
lmk = emplaceLandmark(feature); 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 // set LANDMARK_ID in all features of the track
for (auto feat_pair : track_matrix_.track(track_id)) for (auto feat_pair : track_matrix_.track(track_id))
{ {
feat_pair.second->setLandmarkId(lmk->id()); feat_pair.second->setLandmarkId(lmk->id());
WOLF_INFO("Setting landmark id in feature ", WOLF_DEBUG("Setting landmark id in feature ",
feat_pair.second->id(), feat_pair.second->id(),
" landmark_id: ", " landmark_id: ",
feat_pair.second->landmarkId()); feat_pair.second->landmarkId());
} }
} }
// landmarkId already set // landmarkId already set
...@@ -651,7 +581,7 @@ void ProcessorLandmarkExternal::establishFactors() ...@@ -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) FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment