diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index e08694d11432395c9fd983c84e7aaa8d9dbd315a..611abc0f187fd5bd6058be070173d08f4611290d 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -23,6 +23,7 @@ stages:
- fi
- export WOLF_CORE_BRANCH=$CI_COMMIT_BRANCH
# Print variables
+ - echo $CI_COMMIT_REF_NAME
- echo $CI_COMMIT_BRANCH
- echo $WOLF_IMU_BRANCH
- echo $WOLF_GNSS_BRANCH
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..1544904df32e6dab84353dd424b10461f3e03b3c 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;
@@ -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()
@@ -98,154 +98,278 @@ 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_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");
- // 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_INFO("Tracking feature last: ",
+ feat_lmk_last->id(),
+ " - ID: ",
+ feat_lmk_last->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_last->getExternalType(),
+ " meas: ",
+ feat_lmk_last->getMeasurement().transpose());
+
+ // auto last_global_meas = (dim_ == 2 ? Eigen::VectorXd(pose_last.at('P') +
+ // Rotation2Dd(pose_last.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_last->getMeasurement().head<2>()))
+ // : Eigen::VectorXd(pose_last.at('P') +
+ // Quaterniond(Vector4d(pose_last.at('O'))) *
+ // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
+ // feat_lmk_last->getMeasurement().head<3>())));
+ // WOLF_INFO("pose_last: ", pose_last);
+ // WOLF_INFO("pose_sen: ", pose_sen);
+ // auto last_global_meas =
+ // pose_last.at('P') +
+ // Rotation2Dd(pose_last.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) * feat_lmk_last->getMeasurement().head<2>());
+ // WOLF_INFO("Last global meas: ", last_global_meas.transpose());
+
+ // 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_INFO("Feature incoming candidate (by ID): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // auto global_meas =
+ // (dim_ == 2
+ // ? Eigen::VectorXd(pose_incoming.at('P') +
+ // Rotation2Dd(pose_incoming.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_incoming->getMeasurement().head<2>()))
+ // : Eigen::VectorXd(
+ // pose_incoming.at('P') +
+ // Quaterniond(Vector4d(pose_incoming.at('O'))) *
+ // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
+ // feat_lmk_incoming->getMeasurement().head<3>())));
+ // auto global_meas =
+ // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_incoming->getMeasurement().head<2>());
+ // WOLF_INFO("pose_incoming: ", pose_incoming);
+ // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
+
+ // 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_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
+ 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_INFO("Feature incoming candidate (by TYPE): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // auto global_meas =
+ // (dim_ == 2
+ // ? Eigen::VectorXd(pose_incoming.at('P') +
+ // Rotation2Dd(pose_incoming.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_incoming->getMeasurement().head<2>()))
+ // : Eigen::VectorXd(
+ // pose_incoming.at('P') +
+ // Quaterniond(Vector4d(pose_incoming.at('O'))) *
+ // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
+ // feat_lmk_incoming->getMeasurement().head<3>())));
+ // auto global_meas =
+ // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_incoming->getMeasurement().head<2>());
+ // WOLF_INFO("pose_incoming: ", pose_incoming);
+ // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
+
+ // 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_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
+ 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_INFO("Feature incoming candidate (by distance): ",
+ feat_lmk_incoming->id(),
+ " - ID: ",
+ feat_lmk_incoming->getExternalId(),
+ " - TYPE: ",
+ feat_lmk_incoming->getExternalType(),
+ " meas: ",
+ feat_lmk_incoming->getMeasurement().transpose());
+
+ // auto global_meas =
+ // (dim_ == 2
+ // ? Eigen::VectorXd(pose_incoming.at('P') +
+ // Rotation2Dd(pose_incoming.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_incoming->getMeasurement().head<2>()))
+ // : Eigen::VectorXd(
+ // pose_incoming.at('P') +
+ // Quaterniond(Vector4d(pose_incoming.at('O'))) *
+ // (pose_sen.at('P') + Quaterniond(Vector4d(pose_sen.at('O'))) *
+ // feat_lmk_incoming->getMeasurement().head<3>())));
+ // auto global_meas =
+ // pose_incoming.at('P') + Rotation2Dd(pose_incoming.at('O')(0)) *
+ // (pose_sen.at('P') + Rotation2Dd(pose_sen.at('O')(0)) *
+ // feat_lmk_incoming->getMeasurement().head<2>());
+ // WOLF_INFO("pose_incoming: ", pose_incoming);
+ // WOLF_INFO("Incoming global meas: ", global_meas.transpose());
+
+ // 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_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
+ 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_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());
@@ -291,159 +415,231 @@ 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>();
+ VectorComposite pose_s1 = SE2::compose(_pose1, _pose_sen);
+ VectorComposite pose_s2 = SE2::compose(_pose2, _pose_sen);
+
+ Eigen::Vector2d p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ Eigen::Vector2d p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+
return (p1 - p2).norm();
}
else
{
- auto pose_s1 = SE3::compose(_pose1, _pose_sen);
- 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>();
+ VectorComposite pose_s1 = SE3::compose(_pose1, _pose_sen);
+ VectorComposite pose_s2 = SE3::compose(_pose2, _pose_sen);
+ Eigen::Vector3d p1 =
+ pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ Eigen::Vector3d p2 =
+ pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+
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);
+ 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);
+ 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();
+ }
+}
+
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 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++;
}
}
// 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_->filter_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_
- 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;
+ auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
- WOLF_DEBUG("Track long enough: ", track_matrix_.trackSize(track_id));
+ WOLF_INFO("Feature ",
+ feature->id(),
+ ": ID: ",
+ feature->landmarkId(),
+ " EXTERNAL_ID: ",
+ feature->getExternalId(),
+ " TYPE: ",
+ feature->getExternalType());
- auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
+ // 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;
// Landmark match
LandmarkExternalPtr lmk;
if (feature->landmarkId() == 0) // landmark id 0 never used
{
- 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
+ WOLF_INFO("Searching Loop closure 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_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))
+ }
}
- // loop closure with ID
- if (params_tfle_->close_loop_when_id_match and feature->getExternalId() != -1)
+ // By TYPE
+ WOLF_INFO("Searching Loop closure 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_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)
+ if (feature->getExternalId() != -1) lmk->setExternalId(feature->getExternalId());
}
// Emplace new landmark (loop closure not found or not enabled)
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());
- // TODO: check all features do not disagree
+ WOLF_INFO("Setting landmark id in feature ",
+ feat_pair.second->id(),
+ " landmark_id: ",
+ feat_pair.second->landmarkId());
}
}
- // 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
@@ -453,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)
@@ -509,7 +702,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/CMakeLists.txt b/test/CMakeLists.txt
index 505e9ca54b9c1512711d0c1db5b99a900524db7c..1709df19170f9110e8e2d180c7d27597a6becc64 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -204,7 +204,7 @@ wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
# ProcessorLandmarkExternal class test
-# wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
+wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index 77995d6a50f136c17dc305db655758b550f29ec4..8336e31a65f58be3c5a832c0f31b63f12ca03ff9 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -93,6 +93,12 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
double _time_span,
bool _init_landmarks)
{
+ // INCOMPATIBLE OPTIONS
+ if (_init_landmarks and _mode == 2)
+ throw std::runtime_error("Landmarks initialized with mode 2 (no id no type), impossible to close loops");
+ if (_init_landmarks and _mode == 4)
+ throw std::runtime_error("Landmarks initialized with mode 4 (changing), impossible to close loops");
+
dim = _dim;
orientation = _orientation;
mode = _mode;
@@ -129,19 +135,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)
@@ -171,20 +178,21 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Emplace 3 random landmarks
for (auto i = 0; i < 3; i++)
{
+ bool init_landmark = _init_landmarks and not (mode == 3 and i % 3 == 2);
LandmarkExternalPtr lmk;
if (dim == 2)
lmk = LandmarkBase::emplace<LandmarkExternal>(
- _init_landmarks ? problem->getMap() : nullptr,
- i + 1,
+ init_landmark ? problem->getMap() : nullptr,
i + 1,
+ 3 * i + 10,
std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
(orientation ? std::make_shared<StateAngle>(Vector1d::Random() * M_PI) : nullptr));
else
lmk = LandmarkBase::emplace<LandmarkExternal>(
- _init_landmarks ? problem->getMap() : nullptr,
- i + 1,
+ init_landmark ? problem->getMap() : nullptr,
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);
@@ -252,8 +260,8 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
.coeffs();
}
// cov
- MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
- if (orientation and dim != 2) cov = MatrixXd::Identity(6, 6);
+ MatrixXd cov = 0.01 * MatrixXd::Identity(meas.size(), meas.size());
+ if (orientation and dim != 2) cov = 0.01 * MatrixXd::Identity(6, 6);
// quality
double quality = (double)i / (double)(landmarks.size() - 1); // increasing from 0 to 1
@@ -262,10 +270,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 +300,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 +311,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);
@@ -375,8 +385,12 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
for (auto lmk_map : landmarks_map)
{
auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map);
- ASSERT_EQ(lmk_ext->getExternalId(), landmarks.at(lmk_ext->getExternalId() - 1)->getExternalId());
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
+ if (lmk_ext->getExternalId() != -1)
+ {
+ ASSERT_EQ(lmk_ext->getExternalId(),
+ landmarks.at(lmk_ext->getExternalId() - 1)->getExternalId());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
+ }
}
}
}
@@ -389,10 +403,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)
{
@@ -411,9 +421,14 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map);
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
+ if (lmk_ext->getExternalId() != -1)
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
}
}
+
+ // step with random movement
+ t += dt;
+ randomStep();
}
}
@@ -438,582 +453,635 @@ void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite&
throw std::runtime_error("wrong vector composite");
}
-// / TESTS //////////////////////////////////////////
-TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
+/// TESTS //////////////////////////////////////////
+
+//-------------------- Position in 2D --------------------
+TEST_F(ProcessorLandmarkExternalTest, P_2d_loop_closure_id)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // 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_loop_closure_type)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // 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_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_id)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_type)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_nothing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_mixed)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_changing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_id)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_type)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_nothing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_mixed)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_2d_quality_changing)
+{
+ testConfiguration(2, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position & Orientation in 2D --------------------
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_loop_closure_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // 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, PO_2d_loop_closure_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // 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, PO_2d_loop_closure_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // 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, PO_2d_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_nothing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_mixed)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_changing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_id)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_type)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_nothing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_mixed)
{
testConfiguration(2, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_2d_quality_changing)
+{
+ testConfiguration(2, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position in 3D --------------------
+TEST_F(ProcessorLandmarkExternalTest, P_3d_loop_closure_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // 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_3d_loop_closure_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // 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_3d_loop_closure_mixed)
+{
+ testConfiguration(3, // 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_3d_id)
+{
+ testConfiguration(3, // int dim
false, // bool orientation
0, // 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_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_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_no_lmks_id)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_type)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_nothing)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_mixed)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_id)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 0, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_type)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 1, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_nothing)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 2, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality_mixed)
-// {
-// testConfiguration(2, // int dim
-// false, // bool orientation
-// 3, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 6, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks_id)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 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_type)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 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
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks_mixed)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 3, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_id)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 1, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_type)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 1, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_nothing)
-// {
-// testConfiguration(2, // 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
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_mixed)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 1, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_id)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 0, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 0, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_type)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 1, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 0, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_nothing)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 2, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 0, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality_mixed)
-// {
-// testConfiguration(2, // int dim
-// true, // bool orientation
-// 3, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 0, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks_id)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 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_type)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 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
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks_mixed)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 7, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_id)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 53, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_type)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 53, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_nothing)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 53, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_mixed)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 53, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_id)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 0, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_type)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 1, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_nothing)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 2, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality_mixed)
-// {
-// testConfiguration(3, // int dim
-// false, // bool orientation
-// 3, // int mode
-// 0.3, // double quality_th
-// 1e6, // double dist_th
-// 2, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks_id)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 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_type)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 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
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks_mixed)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 1, // int track_length
-// 4.5 * dt, // double time_span
-// true); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_id)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 0, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 4, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_type)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 1, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 4, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_nothing)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 2, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 4, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_mixed)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 3, // int mode
-// 0, // double quality_th
-// 1e6, // double dist_th
-// 4, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_id)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 0, // int mode
-// 0.2, // double quality_th
-// 1e6, // double dist_th
-// 5, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_type)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 1, // int mode
-// 0.2, // double quality_th
-// 1e6, // double dist_th
-// 5, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_nothing)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 2, // int mode
-// 0.2, // double quality_th
-// 1e6, // double dist_th
-// 5, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
-
-// TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality_mixed)
-// {
-// testConfiguration(3, // int dim
-// true, // bool orientation
-// 3, // int mode
-// 0.2, // double quality_th
-// 1e6, // double dist_th
-// 5, // int track_length
-// 4.5 * dt, // double time_span
-// false); // bool init_landmarks
-// }
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_nothing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_changing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_id)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_type)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_nothing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_mixed)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, P_3d_quality_changing)
+{
+ testConfiguration(3, // int dim
+ false, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+//-------------------- Position & Orientation in 3D --------------------
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_loop_closure_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // 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, PO_3d_loop_closure_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // 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, PO_3d_loop_closure_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // 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, PO_3d_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_nothing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_changing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0, // double quality_th
+ 1e-2, // double dist_th
+ 2, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_id)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 0, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_type)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 1, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_nothing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 2, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_mixed)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 3, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
+
+TEST_F(ProcessorLandmarkExternalTest, PO_3d_quality_changing)
+{
+ testConfiguration(3, // int dim
+ true, // bool orientation
+ 4, // int mode
+ 0.3, // double quality_th
+ 1e-2, // double dist_th
+ 6, // int track_length
+ 4.5 * dt, // double time_span
+ false); // bool init_landmarks & loop closure
+}
int main(int argc, char** argv)
{