From 9612c824cc513177f7e9ace59f4cbe43ee7d3ff6 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Thu, 8 Feb 2024 16:50:50 +0100 Subject: [PATCH] fixed auto bug for eigen --- src/processor/processor_landmark_external.cpp | 235 +++++++++--------- test/gtest_processor_landmark_external.cpp | 2 +- 2 files changed, 124 insertions(+), 113 deletions(-) diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp index ce22608d7..1544904df 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_DEBUG("ProcessorLandmarkExternal::preprocess: found ", - new_features_incoming_.size(), - " features in incoming capture"); + WOLF_INFO("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_DEBUG("Searching ", known_features_last_.size(), " tracked features..."); + WOLF_INFO("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,14 +109,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() { auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last); bool matched = false; - 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()); + 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)) * @@ -126,13 +126,13 @@ unsigned int ProcessorLandmarkExternal::processKnown() // Quaterniond(Vector4d(pose_last.at('O'))) * // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) * // feat_lmk_last->getMeasurement().head<3>()))); - // WOLF_DEBUG("pose_last: ", pose_last); - // WOLF_DEBUG("pose_sen: ", pose_sen); + // 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_DEBUG("Last global meas: ", last_global_meas.transpose()); + // WOLF_INFO("Last global meas: ", last_global_meas.transpose()); // First we try to match by EXTERNAL_ID if (feat_lmk_last->getExternalId() != -1) @@ -142,14 +142,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() { auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); - 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()); + 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 @@ -166,8 +166,8 @@ unsigned int ProcessorLandmarkExternal::processKnown() // 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_DEBUG("pose_incoming: ", pose_incoming); - // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose()); + // WOLF_INFO("pose_incoming: ", pose_incoming); + // WOLF_INFO("Incoming global meas: ", global_meas.transpose()); // MATCH NECESSARY CONDITIONS: // 1. Same EXTERNAL_ID @@ -181,12 +181,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3 params_tfle_->match_dist_th_id) { - WOLF_DEBUG("Feature last: ", - feat_lmk_last->id(), - " matched with feature ", - feat_lmk_incoming->id(), - " with landmark ID: ", - feat_lmk_last->landmarkId()); + WOLF_INFO("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,14 +217,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_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()); + 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 @@ -241,8 +241,8 @@ unsigned int ProcessorLandmarkExternal::processKnown() // 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_DEBUG("pose_incoming: ", pose_incoming); - // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose()); + // WOLF_INFO("pose_incoming: ", pose_incoming); + // WOLF_INFO("Incoming global meas: ", global_meas.transpose()); // MATCH NECESSARY CONDITIONS: // 1. Compatible EXTERNAL_ID (either not defined or same) @@ -256,12 +256,12 @@ unsigned int ProcessorLandmarkExternal::processKnown() feat_lmk_last, feat_lmk_incoming, pose_last, pose_incoming, pose_sen) < // cond 3 params_tfle_->match_dist_th_type) { - WOLF_DEBUG("Feature last: ", - feat_lmk_last->id(), - " matched with feature ", - feat_lmk_incoming->id(), - " with landmark ID: ", - feat_lmk_last->landmarkId()); + WOLF_INFO("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,14 +295,14 @@ unsigned int ProcessorLandmarkExternal::processKnown() { auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it); - 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()); + 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 @@ -319,8 +319,8 @@ unsigned int ProcessorLandmarkExternal::processKnown() // 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_DEBUG("pose_incoming: ", pose_incoming); - // WOLF_DEBUG("Incoming global meas: ", global_meas.transpose()); + // WOLF_INFO("pose_incoming: ", pose_incoming); + // WOLF_INFO("Incoming global meas: ", global_meas.transpose()); // MATCH NECESSARY CONDITIONS: // 1. Compatible EXTERNAL_ID (either not defined or same) @@ -335,12 +335,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_DEBUG("Feature last: ", - feat_lmk_last->id(), - " matched with feature ", - feat_lmk_incoming->id(), - " with landmark ID: ", - feat_lmk_last->landmarkId()); + WOLF_INFO("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 +369,7 @@ unsigned int ProcessorLandmarkExternal::processKnown() } WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked."); } - WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features."); + WOLF_INFO("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()); @@ -451,14 +451,14 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr, if (getProblem()->getDim() == 2) { - auto pose_s = SE2::compose(_pose_frm, _pose_sen); - auto p = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>(); + auto pose_s = SE2::compose(_pose_frm, _pose_sen); + Eigen::Vector2d p = pose_s.at('P') + Rotation2Dd(pose_s.at('O')(0)) * _ftr->getMeasurement().head<2>(); return (p - _lmk->getP()->getState().head<2>()).norm(); } else { - auto pose_s = SE3::compose(_pose_frm, _pose_sen); - auto p = pose_s.at('P') + Quaterniond(Vector4d(pose_s.at('O'))) * _ftr->getMeasurement().head<3>(); + auto pose_s = SE3::compose(_pose_frm, _pose_sen); + Eigen::Vector3d p = pose_s.at('P') + Quaterniond(Vector4d(pose_s.at('O'))) * _ftr->getMeasurement().head<3>(); return (p - _lmk->getP()->getState().head<3>()).norm(); } } @@ -467,7 +467,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const { auto track_ids_last = track_matrix_.trackIds(last_ptr_); - WOLF_DEBUG("Active feature tracks: ", track_ids_last.size()); + WOLF_INFO("Active feature tracks: ", track_ids_last.size()); // number of tracks longer than track_length_th auto n_tracks = 0; @@ -483,50 +483,50 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const // Necessary condition: active valid tracks bool vote_min_features = n_tracks >= 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, - ")"); + 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, + ")"); 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_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, - ")"); + 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, + ")"); // Sufficient condition: time span vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > 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, - ")"); + WOLF_INFO("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_DEBUG((vote ? "Vote " : "Do NOT vote "), "for KF"); + WOLF_INFO((vote ? "Vote " : "Do NOT vote "), "for KF"); return vote; } void ProcessorLandmarkExternal::establishFactors() { - WOLF_DEBUG("establishFactors"); + WOLF_INFO("establishFactors"); // if (origin_ptr_ == last_ptr_) return; // // reset n_tracks_origin_ @@ -540,23 +540,30 @@ void ProcessorLandmarkExternal::establishFactors() for (auto track_id : track_ids_last) { - WOLF_DEBUG("Feature ", track_matrix_.feature(track_id, last_ptr_)->id()); + 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()); // 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)); if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue; - WOLF_DEBUG("Track long enough: ", track_matrix_.trackSize(track_id)); - - auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_)); - // Landmark match LandmarkExternalPtr lmk; if (feature->landmarkId() == 0) // landmark id 0 never used { - WOLF_DEBUG("Landmark id not set"); - // LOOP CLOSURE // By ID + WOLF_INFO("Searching Loop closure by ID..."); if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1) { auto lmk_list = getProblem()->getMap()->getLandmarkList(); @@ -569,12 +576,16 @@ void ProcessorLandmarkExternal::establishFactors() detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_id) { lmk = lmk_ext; - WOLF_DEBUG("Found loop closure by EXTERNAL_ID with ", lmk->id()); + WOLF_INFO("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)) } } // By TYPE + WOLF_INFO("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(); @@ -587,9 +598,12 @@ void ProcessorLandmarkExternal::establishFactors() detectionDistance(feature, lmk_ext, pose_frm, pose_sen) < params_tfle_->match_dist_th_type) { lmk = lmk_ext; - WOLF_DEBUG("Found loop closure by TYPE with ", lmk->id()); + WOLF_INFO("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)) } // update Landmark EXTERNAL_ID (if available) @@ -600,17 +614,17 @@ void ProcessorLandmarkExternal::establishFactors() if (not lmk) { lmk = emplaceLandmark(feature); - WOLF_DEBUG("New landmark ", lmk->id()); + WOLF_INFO("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_DEBUG("Setting landmark id in feature ", - feat_pair.second->id(), - " landmark_id: ", - feat_pair.second->landmarkId()); + WOLF_INFO("Setting landmark id in feature ", + feat_pair.second->id(), + " landmark_id: ", + feat_pair.second->landmarkId()); } } // landmarkId already set @@ -635,12 +649,9 @@ void ProcessorLandmarkExternal::establishFactors() auto fac = emplaceFactor(feature, lmk); if (fac) fac_list.push_back(fac); } - - // // store tracked landmarks from origin (used by voteForKeyframe()) - // lmks_ids_origin_.insert(lmk->id()); } - WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!"); + WOLF_INFO("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!"); } FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark) diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp index 218263497..8336e31a6 100644 --- a/test/gtest_processor_landmark_external.cpp +++ b/test/gtest_processor_landmark_external.cpp @@ -178,7 +178,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, // Emplace 3 random landmarks for (auto i = 0; i < 3; i++) { - bool init_landmark = _init_landmarks and mode != 3 and i % 3 != 2; + bool init_landmark = _init_landmarks and not (mode == 3 and i % 3 == 2); LandmarkExternalPtr lmk; if (dim == 2) lmk = LandmarkBase::emplace<LandmarkExternal>( -- GitLab