diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index ae305ab7c6a9b2c9fb7578a1ec5d53c7a938ab21..9017022ef0e9bf5bf1258ad1b5fbee7850cfa5ec 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -36,32 +36,40 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
{
- bool use_orientation; ///< use orientation measure or not when emplacing factors
- double match_dist_th; ///< for considering tracked detection: distance threshold to previous detection
+ bool use_orientation; ///< use orientation measure or not when emplacing factors
+
unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin
///< (sufficient condition if more than min_features_for_keyframe)
- double filter_quality_th; ///< min quality to consider the detection
- unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
- double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than
- ///< min_features_for_keyframe)
- bool check_dist_when_ids_match; ///< check the match_dist_th also in detections with matching external ID
- bool close_loop_when_id_match; ///< emplace loop closure factors if external ID matches
- bool close_loop_when_type_match; ///< emplace loop closure factors if external TYPE matches (and match_dist_th holds)
+ double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than
+ ///< min_features_for_keyframe)
+
+ double quality_th; ///< min quality to consider the detection
+
+ // Matching distance threshold to previous detection considering motion (necessary condition)
+ double match_dist_th_id; ///< Match by ID
+ double match_dist_th_type; ///< Match by TYPE
+ double match_dist_th_unknown; ///< No ID/TYPE information
+
+ unsigned int track_length_th; ///< Track length threshold to emplace factors (necessary condition)
+
+ bool close_loops_by_id; ///< Close loop if ID matches (ID unchanged guaranteed)
+ bool close_loops_by_type; ///< Close loop if TYPE matches (also distance check)
ParamsProcessorLandmarkExternal() = default;
ParamsProcessorLandmarkExternal(std::string _unique_name, const wolf::ParamsServer& _server)
: ParamsProcessorTracker(_unique_name, _server)
{
- use_orientation = _server.getParam<bool>(prefix + _unique_name + "/use_orientation");
- filter_quality_th = _server.getParam<double>(prefix + _unique_name + "/filter/quality_th");
- filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter/track_length_th");
- match_dist_th = _server.getParam<double>(prefix + _unique_name + "/match_dist_th");
- time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
+ use_orientation = _server.getParam<bool>(prefix + _unique_name + "/use_orientation");
new_features_for_keyframe =
_server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
- check_dist_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/check_dist_when_ids_match");
- close_loop_when_id_match = _server.getParam<bool>(prefix + _unique_name + "/close_loop_when_id_match");
- close_loop_when_type_match = _server.getParam<bool>(prefix + _unique_name + "/close_loop_when_type_match");
+ time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
+ quality_th = _server.getParam<double>(prefix + _unique_name + "/quality_th");
+ match_dist_th_id = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_id");
+ match_dist_th_type = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_type");
+ match_dist_th_unknown = _server.getParam<double>(prefix + _unique_name + "/match_dist_th_unknown");
+ track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/track_length_th");
+ close_loops_by_id = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_id");
+ close_loops_by_type = _server.getParam<bool>(prefix + _unique_name + "/close_loops_by_type");
}
};
@@ -82,7 +90,7 @@ class ProcessorLandmarkExternal : public ProcessorTracker
protected:
ParamsProcessorLandmarkExternalPtr params_tfle_;
TrackMatrix track_matrix_;
- std::set<SizeStd> lmks_ids_origin_;
+ // std::set<SizeStd> lmks_ids_origin_;
/** Pre-process incoming Capture
*
@@ -154,16 +162,19 @@ class ProcessorLandmarkExternal : public ProcessorTracker
const VectorComposite& _pose1,
const VectorComposite& _pose2,
const VectorComposite& _pose_sen) const;
+
+ double detectionDistance(FeatureBasePtr _ftr,
+ LandmarkBasePtr _lmk,
+ const VectorComposite& _pose_frm,
+ const VectorComposite& _pose_sen) const;
};
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
: ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
- params_tfle_(_params_tfle),
- lmks_ids_origin_()
+ params_tfle_(_params_tfle)//,lmks_ids_origin_()
{
//
}
-
} // namespace wolf
#endif
\ No newline at end of file
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index f12dffc60a4fc5805e6f69464d5d748ed6fb9ebd..680e88132be2b1444fa7e48ec5011c0718a3c019 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -56,7 +56,7 @@ void ProcessorLandmarkExternal::preProcess()
"ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
// Filter by quality
- if (detection.quality < params_tfle_->filter_quality_th or ids.count(detection.id)) continue;
+ if (detection.quality < params_tfle_->quality_th or ids.count(detection.id)) continue;
// measure and covariance
VectorXd meas;
@@ -98,154 +98,274 @@ unsigned int ProcessorLandmarkExternal::processKnown()
known_features_incoming_.clear();
// Track features from last_ptr_ to incoming_ptr_
- if (last_ptr_)
- {
- WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
- auto pose_sen = getSensor()->getState("PO");
- auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
- auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+ if (not last_ptr_) return 0;
- for (auto feat_last : known_features_last_)
- {
- auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
- bool matched = false;
+ 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");
- // First we try to match by EXTERNAL_ID (if defined)
- if (feat_lmk_last->getExternalId() != -1)
+ for (auto feat_last : known_features_last_)
+ {
+ 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());
+
+ // 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>())));
+
+ 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("global meas last: ", last_global_meas.transpose());
+
+ // First we try to match by EXTERNAL_ID
+ if (feat_lmk_last->getExternalId() != -1)
+ {
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
{
- WOLF_DEBUG("Tracking feature last: ", feat_lmk_last);
- auto feat_candidate_it = new_features_incoming_.begin();
- while (feat_candidate_it != new_features_incoming_.end())
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+
+ WOLF_DEBUG("Feature incoming candidate: ",
+ 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_DEBUG(" - global meas: ", global_meas.transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Same EXTERNAL_ID
+ // 2. Compatible TYPE (either not defined or same)
+ // 3. Detections close enough
+ if (feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId() and // cond 1
+ (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)
{
- auto feat_lmk_candidate = std::static_pointer_cast<FeatureLandmarkExternal>(*feat_candidate_it);
- if (feat_lmk_candidate->getExternalId() == feat_lmk_last->getExternalId())
- {
- // Check distance (if enabled)
- if (not params_tfle_->check_dist_when_ids_match or
- detectionDistance(feat_lmk_last, feat_lmk_candidate, pose_in, pose_out, pose_sen) <
- params_tfle_->match_dist_th)
- {
- WOLF_DEBUG("Feature last: ",
- feat_lmk_last->id(),
- " matched with feature ",
- feat_lmk_candidate->id(),
- " with landmark ID: ",
- feat_lmk_last->landmarkId());
- matched = true;
-
- // set LANDMARK_ID if defined
- if (feat_lmk_last->landmarkId() != 0)
- feat_lmk_candidate->setLandmarkId(feat_lmk_last->landmarkId());
-
- // grow track
- track_matrix_.add(feat_lmk_last, feat_lmk_candidate);
-
- // feature is known
- known_features_incoming_.push_back(feat_lmk_candidate);
-
- // remove from unmatched
- feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
- break;
- }
- else
- feat_candidate_it++;
- }
+ 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
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
}
- if (matched) continue;
+ else
+ feature_incoming_it++;
}
- // Second we try to match by TYPE (if defined)
- if (feat_lmk_last->getExternalType() != -1)
+ }
+ // skip search (already found match)
+ if (matched) continue;
+
+ // Second we try to match by TYPE (if defined)
+ if (feat_lmk_last->getExternalType() != -1)
+ {
+ auto feature_incoming_it = new_features_incoming_.begin();
+ while (feature_incoming_it != new_features_incoming_.end())
{
- auto feat_candidate_it = new_features_incoming_.begin();
- while (feat_candidate_it != new_features_incoming_.end())
+ auto feat_lmk_incoming = std::static_pointer_cast<FeatureLandmarkExternal>(*feature_incoming_it);
+ WOLF_DEBUG("Feature incoming candidate: ",
+ 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_DEBUG(" - global meas: ", global_meas.transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Compatible EXTERNAL_ID (either not defined or same)
+ // 2. Same TYPE
+ // 3. Detections close enough
+ if ((feat_lmk_incoming->getExternalId() == -1 or // cond 1
+ 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)
{
- auto feat_lmk_candidate = std::static_pointer_cast<FeatureLandmarkExternal>(*feat_candidate_it);
- if (feat_lmk_candidate->getExternalType() == feat_lmk_last->getExternalType())
- {
- // Check distance (if enabled)
- if (detectionDistance(feat_lmk_last, feat_lmk_candidate, pose_in, pose_out, pose_sen) <
- params_tfle_->match_dist_th)
- {
- WOLF_DEBUG("Feature last: ",
- feat_lmk_last->id(),
- " matched with feature ",
- feat_lmk_candidate->id(),
- " with landmark ID: ",
- feat_lmk_last->landmarkId());
- matched = true;
-
- // set LANDMARK_ID if defined
- if (feat_lmk_last->landmarkId() != 0)
- feat_lmk_candidate->setLandmarkId(feat_lmk_last->landmarkId());
-
- // set EXTERNAL_ID if defined
- if (feat_lmk_last->getExternalId() != -1)
- feat_lmk_candidate->setExternalId(feat_lmk_last->getExternalId());
-
- // grow track
- track_matrix_.add(feat_lmk_last, feat_lmk_candidate);
-
- // feature is known
- known_features_incoming_.push_back(feat_lmk_candidate);
-
- // remove from unmatched
- feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
- break;
- }
- else
- feat_candidate_it++;
- }
+ 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
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // set EXTERNAL_ID last -> incoming
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_incoming->setExternalId(feat_lmk_last->getExternalId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
}
- if (matched) continue;
+ else
+ feature_incoming_it++;
}
- // Finally, we try to match by distance
- auto feat_candidate_it = new_features_incoming_.begin();
- while (feat_candidate_it != new_features_incoming_.end())
+ }
+ // skip search (already found match)
+ if (matched) continue;
+
+ // Finally, we try to match by distance
+ auto feature_incoming_it = new_features_incoming_.begin();
+ 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: ",
+ 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_DEBUG(" - global meas: ", global_meas.transpose());
+
+ // MATCH NECESSARY CONDITIONS:
+ // 1. Compatible EXTERNAL_ID (either not defined or same)
+ // 2. Compatible TYPE (either not defined or same)
+ // 3. Detections close enough
+ if ((feat_lmk_incoming->getExternalId() == -1 or // cond 1
+ feat_lmk_last->getExternalId() == -1 or //
+ feat_lmk_incoming->getExternalId() == feat_lmk_last->getExternalId()) and //
+ (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_unknown)
{
- auto feat_lmk_candidate = std::static_pointer_cast<FeatureLandmarkExternal>(*feat_candidate_it);
- if (feat_lmk_candidate->getExternalType() == feat_lmk_last->getExternalType())
- {
- // Check distance (if enabled)
- if (detectionDistance(feat_lmk_last, feat_lmk_candidate, pose_in, pose_out, pose_sen) <
- params_tfle_->match_dist_th)
- {
- WOLF_DEBUG("Feature last: ",
- feat_lmk_last->id(),
- " matched with feature ",
- feat_lmk_candidate->id(),
- " with landmark ID: ",
- feat_lmk_last->landmarkId());
- matched = true;
-
- // set LANDMARK_ID if defined
- if (feat_lmk_last->landmarkId() != 0)
- feat_lmk_candidate->setLandmarkId(feat_lmk_last->landmarkId());
-
- // set TYPE if defined
- if (feat_lmk_last->getExternalType() != -1)
- feat_lmk_candidate->setExternalType(feat_lmk_last->getExternalType());
-
- // set EXTERNAL_ID if defined
- if (feat_lmk_last->getExternalId() != -1)
- feat_lmk_candidate->setExternalId(feat_lmk_last->getExternalId());
-
- // grow track
- track_matrix_.add(feat_lmk_last, feat_lmk_candidate);
-
- // feature is known
- known_features_incoming_.push_back(feat_lmk_candidate);
-
- // remove from unmatched
- feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
- break;
- }
- else
- feat_candidate_it++;
- }
+ 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
+ if (feat_lmk_last->landmarkId() != 0) feat_lmk_incoming->setLandmarkId(feat_lmk_last->landmarkId());
+
+ // set TYPE last -> incoming
+ if (feat_lmk_last->getExternalType() != -1)
+ feat_lmk_incoming->setExternalType(feat_lmk_last->getExternalType());
+
+ // set EXTERNAL_ID last -> incoming
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_incoming->setExternalId(feat_lmk_last->getExternalId());
+
+ // grow track
+ track_matrix_.add(feat_lmk_last, feat_lmk_incoming);
+
+ // feature is known
+ known_features_incoming_.push_back(feat_lmk_incoming);
+
+ // remove from unmatched
+ feature_incoming_it = new_features_incoming_.erase(feature_incoming_it);
+ break;
}
+ else
+ feature_incoming_it++;
}
- WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
+ WOLF_DEBUG_COND(not matched, "Feature ", feat_lmk_last->id(), " not tracked.");
}
+ 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());
@@ -278,6 +398,21 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
const VectorComposite& _pose2,
const VectorComposite& _pose_sen) const
{
+ WOLF_DEBUG("ProcessorLandmarkExternal::detectionDistance: feature ",
+ _ftr1->id(),
+ " detection: ",
+ _ftr1->getMeasurement().transpose(),
+ " - feature ",
+ _ftr2->id(),
+ " detection: ",
+ _ftr2->getMeasurement().transpose(),
+ " - _pose1: ",
+ _pose1,
+ " - _pose2: ",
+ _pose2,
+ " - _pose_sen: ",
+ _pose_sen);
+
// Any not available info of poses, assume identity
if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or
not _pose_sen.includesStructure("PO"))
@@ -291,10 +426,16 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
{
if (getProblem()->getDim() == 2)
{
- auto pose_s1 = SE2::compose(_pose1, _pose_sen);
- auto pose_s2 = SE2::compose(_pose2, _pose_sen);
- auto p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
- auto p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+ // auto pose_s1 = SE2::compose(_pose1, _pose_sen);
+ // auto pose_s2 = SE2::compose(_pose2, _pose_sen);
+ auto p1 = _pose1.at('P') +
+ Rotation2Dd(_pose1.at('O')(0)) *
+ (_pose_sen.at('P') + Rotation2Dd(_pose_sen.at('O')(0)) * _ftr1->getMeasurement().head<2>());
+ auto p2 = _pose2.at('P') +
+ Rotation2Dd(_pose2.at('O')(0)) *
+ (_pose_sen.at('P') + Rotation2Dd(_pose_sen.at('O')(0)) * _ftr2->getMeasurement().head<2>());
+
+ WOLF_DEBUG("p1: ", p1.transpose(), " p2: ", p2.transpose(), " - norm: ", (p1 - p2).norm());
return (p1 - p2).norm();
}
else
@@ -303,27 +444,53 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1
auto pose_s2 = SE3::compose(_pose2, _pose_sen);
auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+
+ WOLF_DEBUG("p1: ", p1.transpose(), " p2: ", p2.transpose(), " - norm: ", (p1 - p2).norm());
return (p1 - p2).norm();
}
}
}
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr,
+ LandmarkBasePtr _lmk,
+ const VectorComposite& _pose_frm,
+ const VectorComposite& _pose_sen) const
+{
+ // Needed all poses
+ if (not _pose_frm.includesStructure("PO") or not _pose_sen.includesStructure("PO") or not _lmk->hasStateBlock('P'))
+ {
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::detectionDistance: Missing any required geometric information");
+ }
+
+ 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>();
+ 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>();
+ return (p - _lmk->getP()->getState().head<3>()).norm();
+ }
+}
+
bool ProcessorLandmarkExternal::voteForKeyFrame() const
{
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
- // number of tracks longer than filter_track_length_th
+ // number of tracks longer than track_length_th
auto n_tracks = 0;
auto n_new_tracks = 0;
for (auto track_id : track_ids_last)
{
- if (track_matrix_.trackSize(track_id) >= params_tfle_->filter_track_length_th)
+ if (track_matrix_.trackSize(track_id) >= params_tfle_->track_length_th)
{
n_tracks++;
- // if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId()))
- // n_new_tracks++;
if (track_matrix_.feature(track_id, last_ptr_)->landmarkId() == 0) n_new_tracks++;
}
}
@@ -333,7 +500,7 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
WOLF_DEBUG("vote_min_features: ",
vote_min_features,
" - Active feature tracks longer than ",
- params_tfle_->filter_track_length_th,
+ params_tfle_->track_length_th,
": ",
n_tracks,
" (should be equal or bigger than ",
@@ -376,19 +543,21 @@ void ProcessorLandmarkExternal::establishFactors()
WOLF_DEBUG("establishFactors");
// if (origin_ptr_ == last_ptr_) return;
- // reset n_tracks_origin_
- lmks_ids_origin_.clear();
+ // // 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_);
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_frm = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
for (auto track_id : track_ids_last)
{
WOLF_DEBUG("Feature ", track_matrix_.feature(track_id, last_ptr_)->id());
// not enough long track
- if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th) continue;
+ if (track_matrix_.trackSize(track_id) < params_tfle_->track_length_th) continue;
WOLF_DEBUG("Track long enough: ", track_matrix_.trackSize(track_id));
@@ -401,17 +570,44 @@ void ProcessorLandmarkExternal::establishFactors()
WOLF_DEBUG("Landmark id not set");
// LOOP CLOSURE
- // loop closure with TYPE
- if (params_tfle_->close_loop_when_type_match and feature->getExternalType() != -1)
+ // By ID
+ if (params_tfle_->close_loops_by_id and feature->getExternalId() != -1)
{
- // TODO
- WOLF_DEBUG_COND(lmk, "Found landmark ", lmk->id());
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
+
+ for (auto lmk_ptr : lmk_list)
+ {
+ auto lmk_ext = std::dynamic_pointer_cast<LandmarkExternal>(lmk_ptr);
+
+ if (lmk_ext and lmk_ext->getExternalId() == feature->getExternalId() and
+ 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());
+ break;
+ }
+ }
}
- // loop closure with ID
- if (params_tfle_->close_loop_when_id_match and feature->getExternalId() != -1)
+ // By TYPE
+ if (not lmk and params_tfle_->close_loops_by_type and feature->getExternalType() != -1)
{
- // TODO
- WOLF_DEBUG_COND(lmk, "Found landmark ", lmk->id());
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
+
+ for (auto lmk_ptr : lmk_list)
+ {
+ auto lmk_ext = std::dynamic_pointer_cast<LandmarkExternal>(lmk_ptr);
+
+ if (lmk_ext and lmk_ext->getExternalType() == feature->getExternalType() and
+ 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());
+ break;
+ }
+ }
+
+ // update Landmark EXTERNAL_ID (if available)
+ if (feature->getExternalId() != -1) lmk->setExternalId(feature->getExternalId());
}
// Emplace new landmark (loop closure not found or not enabled)
@@ -429,21 +625,21 @@ void ProcessorLandmarkExternal::establishFactors()
feat_pair.second->id(),
" landmark_id: ",
feat_pair.second->landmarkId());
- // TODO: check all features do not disagree
}
}
- // get landmark
+ // landmarkId already set
else
{
lmk = std::dynamic_pointer_cast<LandmarkExternal>(
getProblem()->getMap()->getLandmark(feature->landmarkId()));
if (not lmk)
throw std::runtime_error(
- "ProcessorLandmarkExternal::establishFactors: Feature has LANDMARK_ID but there is no landmark of "
+ "ProcessorLandmarkExternal::establishFactors: Feature has LANDMARK_ID but there is no "
+ "landmark of "
"type LandmarkExternal with this id.");
}
- // modify landmark (add missing state blocks)
+ // modify landmark (add missing state blocks if needed)
modifyLandmark(lmk, feature);
// Emplace factors for all tracked features in keyframes
@@ -454,8 +650,8 @@ void ProcessorLandmarkExternal::establishFactors()
if (fac) fac_list.push_back(fac);
}
- // store tracked landmarks from origin (used by voteForKeyframe())
- lmks_ids_origin_.insert(lmk->id());
+ // // store tracked landmarks from origin (used by voteForKeyframe())
+ // lmks_ids_origin_.insert(lmk->id());
}
WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
@@ -509,7 +705,8 @@ LandmarkExternalPtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkEx
if (_feature->landmarkId() != 0)
throw std::runtime_error(
- "ProcessorLandmarkExternal::emplaceLandmark: attempting to emplace a landmark for a feature that has been "
+ "ProcessorLandmarkExternal::emplaceLandmark: attempting to emplace a landmark for a feature that has "
+ "been "
"already matched with an existing one");
LandmarkExternalPtr lmk;
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index 77995d6a50f136c17dc305db655758b550f29ec4..89fb825fed8e04232a341bb38f6b75513166f7b9 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -129,19 +129,20 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Processors
ParamsProcessorLandmarkExternalPtr params = std::make_shared<ParamsProcessorLandmarkExternal>();
params->time_tolerance = dt / 2;
- params->max_new_features = -1;
- params->voting_active = true;
params->apply_loss_function = false;
- params->use_orientation = orientation;
- params->filter_quality_th = _quality_th;
- params->match_dist_th = _dist_th;
- params->check_dist_when_ids_match = false;
+ params->voting_active = true;
+ params->max_new_features = -1;
params->min_features_for_keyframe = 1;
params->new_features_for_keyframe = 1000;
- params->filter_track_length_th = _track_length_th;
params->time_span = _time_span;
- params->close_loop_when_id_match = _init_landmarks;
- params->close_loop_when_type_match = false;
+ params->use_orientation = orientation;
+ params->quality_th = _quality_th;
+ params->track_length_th = _track_length_th;
+ params->match_dist_th_id = _dist_th;
+ params->match_dist_th_type = _dist_th;
+ params->match_dist_th_unknown = _dist_th;
+ params->close_loops_by_id = _init_landmarks;
+ params->close_loops_by_type = _init_landmarks;
processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
if (dim == 2)
@@ -176,7 +177,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
lmk = LandmarkBase::emplace<LandmarkExternal>(
_init_landmarks ? problem->getMap() : nullptr,
i + 1,
- i + 1,
+ 3*i + 10,
std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
(orientation ? std::make_shared<StateAngle>(Vector1d::Random() * M_PI) : nullptr));
@@ -184,7 +185,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
lmk = LandmarkBase::emplace<LandmarkExternal>(
_init_landmarks ? problem->getMap() : nullptr,
i + 1,
- i + 1,
+ 3*i + 10,
std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
(orientation ? std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) : nullptr));
landmarks.push_back(lmk);
@@ -203,7 +204,7 @@ void ProcessorLandmarkExternalTest::randomStep()
MatrixXd delta_cov;
if (dim == 2)
{
- delta = Vector2d::Random() * 0.1;
+ delta = Vector2d::Random() * 5;
delta_cov = Matrix2d::Identity() * 0.1;
}
else
@@ -262,10 +263,10 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
auto step = (int)round(t.get() / dt);
// with ID
if (mode == 0 or (mode == 3 and i % 3 == 0) or (mode == 4 and step % 3 == 0))
- cap->addDetection(landmarks.at(i)->id(), -1, meas, cov, quality);
+ cap->addDetection(landmarks.at(i)->getExternalId(), -1, meas, cov, quality);
// with TYPE
else if (mode == 1 or (mode == 3 and i % 3 == 1) or (mode == 4 and step % 3 == 1))
- cap->addDetection(-1, landmarks.at(i)->id(), meas, cov, quality);
+ cap->addDetection(-1, landmarks.at(i)->getExternalType(), meas, cov, quality);
// with nothing
else if (mode == 2 or (mode == 3 and i % 3 == 2) or (mode == 4 and step % 3 == 2))
cap->addDetection(-1, -1, meas, cov, quality);
@@ -292,6 +293,8 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
for (auto i = 0; i < 10; i++)
{
WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
+ WOLF_INFO("robot state: ", state_robot);
+ WOLF_INFO("sensor state: ", state_sensor);
// store previous number of kf, lmks, and factors
auto n_prev_kf = problem->getTrajectory()->size();
@@ -301,7 +304,7 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
auto n_prev_fac = fac_list.size();
// Things to check
- bool any_active_track = _quality_th <= 1 and i >= _track_length - 1;
+ bool any_active_track = _quality_th <= 1 and i >= _track_length;
bool should_emplace_KF =
(t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt > _time_span and
any_active_track);
@@ -389,10 +392,6 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), n_prev_lmk);
}
- // step with random movement
- t += dt;
- randomStep();
-
// solve
if (should_emplace_factors)
{
@@ -414,6 +413,10 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
}
}
+
+ // step with random movement
+ t += dt;
+ randomStep();
}
}
@@ -439,7 +442,7 @@ void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite&
}
// / TESTS //////////////////////////////////////////
-TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_id)
{
testConfiguration(2, // int dim
false, // bool orientation
@@ -448,46 +451,34 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
1e6, // double dist_th
6, // int track_length
4.5 * dt, // double time_span
- true); // bool init_landmarks
+ true); // bool init_landmarks & loop closure
}
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_type)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_nothing)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_type)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e6, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_mixed)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_mixed)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ true); // bool init_landmarks & loop closure
+}
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_id)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -496,10 +487,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_type)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -508,10 +499,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -520,10 +511,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -532,10 +523,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_id)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_id)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -544,10 +535,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 6, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_type)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_type)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -556,10 +547,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 6, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_nothing)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -568,10 +559,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 6, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_mixed)
// {
// testConfiguration(2, // int dim
// false, // bool orientation
@@ -580,10 +571,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 6, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_id)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -592,10 +583,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 3, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_type)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -604,22 +595,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 3, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks_nothing)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 3, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_mixed)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -628,10 +607,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 3, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_id)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -640,10 +619,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_type)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -652,10 +631,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_nothing)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -664,10 +643,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_mixed)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -676,10 +655,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_id)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_id)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -688,10 +667,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 0, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_type)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_type)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -700,10 +679,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 0, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_nothing)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -712,10 +691,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 0, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_mixed)
// {
// testConfiguration(2, // int dim
// true, // bool orientation
@@ -724,10 +703,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 0, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_id)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -736,10 +715,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 7, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_type)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -748,22 +727,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 7, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks_nothing)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 7, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_mixed)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -772,10 +739,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 7, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_id)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -784,10 +751,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 53, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_type)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -796,10 +763,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 53, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_nothing)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -808,10 +775,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 53, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_mixed)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -820,10 +787,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 53, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_id)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_id)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -832,10 +799,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_type)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_type)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -844,10 +811,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_nothing)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -856,10 +823,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_mixed)
// {
// testConfiguration(3, // int dim
// false, // bool orientation
@@ -868,10 +835,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 2, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_id)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -880,10 +847,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_type)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -892,22 +859,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks_nothing)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 1, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_mixed)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -916,10 +871,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 1, // int track_length
// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
+// true); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_id)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_id)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -928,10 +883,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 4, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_type)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_type)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -940,10 +895,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 4, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_nothing)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -952,10 +907,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 4, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_mixed)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -964,10 +919,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 4, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_id)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_id)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -976,10 +931,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 5, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_type)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_type)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -988,10 +943,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 5, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_nothing)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_nothing)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -1000,10 +955,10 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 5, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_mixed)
+// TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_mixed)
// {
// testConfiguration(3, // int dim
// true, // bool orientation
@@ -1012,7 +967,7 @@ TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
// 1e6, // double dist_th
// 5, // int track_length
// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
+// false); // bool init_landmarks & loop closure
// }
int main(int argc, char** argv)