From 836e61bed93f8344c8bab868baff2b7d87ff69f7 Mon Sep 17 00:00:00 2001
From: joan vallve <jvallve@iri.upc.edu>
Date: Thu, 1 Feb 2024 17:58:17 +0100
Subject: [PATCH] developing processorLandmarkExternal
---
CMakeLists.txt | 2 +
.../core/feature/feature_landmark_external.h | 63 ++
.../processor/processor_landmark_external.h | 218 ++--
src/feature/feature_landmark_external.cpp | 62 +
src/processor/processor_landmark_external.cpp | 434 ++++---
test/gtest_processor_landmark_external.cpp | 1008 ++++++++++++-----
6 files changed, 1279 insertions(+), 508 deletions(-)
create mode 100644 include/core/feature/feature_landmark_external.h
create mode 100644 src/feature/feature_landmark_external.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 94873905c..8f7b6c613 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -152,6 +152,7 @@ SET(HDRS_FACTOR
SET(HDRS_FEATURE
include/${PROJECT_NAME}/feature/feature_base.h
include/${PROJECT_NAME}/feature/feature_diff_drive.h
+ include/${PROJECT_NAME}/feature/feature_landmark_external.h
include/${PROJECT_NAME}/feature/feature_match.h
include/${PROJECT_NAME}/feature/feature_motion.h
include/${PROJECT_NAME}/feature/feature_odom_2d.h
@@ -273,6 +274,7 @@ SET(SRCS_FACTOR
SET(SRCS_FEATURE
src/feature/feature_base.cpp
src/feature/feature_diff_drive.cpp
+ src/feature/feature_landmark_external.cpp
src/feature/feature_motion.cpp
src/feature/feature_odom_2d.cpp
src/feature/feature_pose.cpp
diff --git a/include/core/feature/feature_landmark_external.h b/include/core/feature/feature_landmark_external.h
new file mode 100644
index 000000000..c7a907d1e
--- /dev/null
+++ b/include/core/feature/feature_landmark_external.h
@@ -0,0 +1,63 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+// Wolf includes
+#include "core/feature/feature_base.h"
+
+// std includes
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FeatureLandmarkExternal);
+
+// class FeatureLandmarkExternal
+class FeatureLandmarkExternal : public FeatureBase
+{
+ protected:
+ int external_id_; ///< The id of the landmark assigned by an external classifier
+ int external_type_; ///< The type of the landmark assigned by an external classifier
+
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _external_id the id assigned by an external tracker (-1 for not tracked)
+ * \param _external_type the type index of the landmark detection (-1 for not classified)
+ *
+ */
+ FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_covariance,
+ const int& _external_id = -1,
+ const int& _external_type = -1);
+
+ ~FeatureLandmarkExternal() override;
+
+ int getExternalType() const;
+ void setExternalType(const int& _external_type);
+ int getExternalId() const;
+ void setExternalId(const int& _external_id);
+};
+
+} // namespace wolf
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index 692eadd1d..8fd3f5417 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -33,121 +33,131 @@ 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
- 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 use_orientation; ///< use orientation measure or not when emplacing factors
+ double match_dist_th; ///< for considering tracked detection: distance threshold to previous detection
+ 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_loops_when_ids_match; ///< emplace loop closure factors if external ID matches
ParamsProcessorLandmarkExternal() = default;
- ParamsProcessorLandmarkExternal(std::string _unique_name,
- const wolf::ParamsServer & _server):
- ParamsProcessorTracker(_unique_name, _server)
+ 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");
- new_features_for_keyframe = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
+ 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");
+ 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_loops_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/close_loops_when_ids_match");
}
};
WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
-//Class
+// Class
class ProcessorLandmarkExternal : public ProcessorTracker
{
- public:
- ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
- ~ProcessorLandmarkExternal() override;
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
-
- void configure(SensorBasePtr _sensor) override { };
-
- protected:
-
- ParamsProcessorLandmarkExternalPtr params_tfle_;
- TrackMatrix track_matrix_;
- std::set<SizeStd> lmks_ids_origin_;
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
- */
- void preProcess() override;
-
-
- /** \brief Process known Features
- * \return The number of successful matches.
- *
- * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
- */
- unsigned int processKnown() override;
-
- /**\brief Process new Features
- *
- */
- unsigned int processNew(const int& _max_features) override { return 0;};
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- bool voteForKeyFrame() const override;
-
- /** \brief Emplaces a new factor for each known feature in Capture \b last
- */
- void establishFactors() override;
-
- /** \brief Emplaces a new factor
- * \param _feature feature pointer
- * \param _landmark pointer to the landmark
- */
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
-
- /** \brief Emplaces a landmark or modifies (if needed) a landmark
- * \param _feature_ptr pointer to the Feature
- */
- LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Modifies (if needed) a landmark
- * \param _landmark pointer to the landmark
- * \param _feature pointer to the Feature.
- */
- void modifyLandmark(LandmarkBasePtr _landmark,
- FeatureBasePtr _feature);
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * It does nothing for now
- */
- void postProcess() override {};
-
- void advanceDerived() override;
- void resetDerived() override;
-
- double detectionDistance(FeatureBasePtr _ftr1,
- FeatureBasePtr _ftr2,
- const VectorComposite& _pose1,
- const VectorComposite& _pose2,
- const VectorComposite& _pose_sen) const;
+ public:
+ ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
+ ~ProcessorLandmarkExternal() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
+
+ void configure(SensorBasePtr _sensor) override{};
+
+ protected:
+ ParamsProcessorLandmarkExternalPtr params_tfle_;
+ TrackMatrix track_matrix_;
+ std::set<SizeStd> lmks_ids_origin_;
+ std::map<int, SizeStd> external_id_to_landmark_id_;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
+ */
+ void preProcess() override;
+
+ /** \brief Process known Features
+ * \return The number of successful matches.
+ *
+ * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in
+ * incoming
+ */
+ unsigned int processKnown() override;
+
+ /**\brief Process new Features
+ *
+ */
+ unsigned int processNew(const int& _max_features) override
+ {
+ return 0;
+ };
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Emplaces a new factor for each known feature in Capture \b last
+ */
+ void establishFactors() override;
+
+ /** \brief Emplaces a new factor
+ * \param _feature feature pointer
+ * \param _landmark pointer to the landmark
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
+
+ /** \brief Emplaces a landmark or modifies (if needed) a landmark
+ * \param _feature_ptr pointer to the Feature
+ */
+ LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+
+ /** \brief Modifies (if needed) a landmark
+ * \param _landmark pointer to the landmark
+ * \param _feature pointer to the Feature.
+ */
+ void modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature);
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * It does nothing for now
+ */
+ void postProcess() override{};
+
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ double detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const;
+
+ SizeStd getAvailableLandmarkId() const;
};
-inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
- ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
- params_tfle_(_params_tfle),
- lmks_ids_origin_()
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
+ : ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
+ params_tfle_(_params_tfle),
+ lmks_ids_origin_()
{
//
}
@@ -157,4 +167,4 @@ inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
//
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/feature/feature_landmark_external.cpp b/src/feature/feature_landmark_external.cpp
new file mode 100644
index 000000000..686d1261d
--- /dev/null
+++ b/src/feature/feature_landmark_external.cpp
@@ -0,0 +1,62 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/feature/feature_landmark_external.h"
+
+namespace wolf
+{
+
+FeatureLandmarkExternal::FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _meas_covariance,
+ const int& _external_id,
+ const int& _external_type)
+ : FeatureBase("FeatureLandmarkExternal", _measurement, _meas_covariance),
+ external_id_(_external_id),
+ external_type_(_external_type)
+{
+}
+
+FeatureLandmarkExternal::~FeatureLandmarkExternal()
+{
+ //
+}
+
+int FeatureLandmarkExternal::getExternalType() const
+{
+ return external_type_;
+}
+
+void FeatureLandmarkExternal::setExternalType(const int& _external_type)
+{
+ external_type_ = _external_type;
+}
+
+int FeatureLandmarkExternal::getExternalId() const
+{
+ return external_id_;
+}
+
+void FeatureLandmarkExternal::setExternalId(const int& _external_id)
+{
+ external_id_ = _external_id;
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index dc8f5b97f..d7bfd41ec 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -22,7 +22,7 @@
#include "core/processor/processor_landmark_external.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/feature/feature_base.h"
+#include "core/feature/feature_landmark_external.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
@@ -43,21 +43,22 @@ void ProcessorLandmarkExternal::preProcess()
{
new_features_incoming_.clear();
- auto dim = getProblem()->getDim();
+ auto dim = getProblem()->getDim();
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks)
- throw std::runtime_error("ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
// Extract features from capture detections
- auto landmark_detections = cap_landmarks->getDetections();
+ auto landmark_detections = cap_landmarks->getDetections();
std::set<int> ids;
for (auto detection : landmark_detections)
{
- WOLF_WARN_COND(ids.count(detection.id), "ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
+ WOLF_WARN_COND(ids.count(detection.id),
+ "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_->filter_quality_th or ids.count(detection.id)) continue;
// measure and covariance
VectorXd meas;
@@ -68,33 +69,29 @@ void ProcessorLandmarkExternal::preProcess()
assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure.head(dim);
- cov = detection.covariance.topLeftCorner(dim,dim);
+ cov = detection.covariance.topLeftCorner(dim, dim);
}
else
{
assert(detection.measure.size() == (dim == 2 ? 3 : 7));
- assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
+ assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and
+ detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure;
cov = detection.covariance;
}
// emplace feature
- FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
- "FeatureLandmarkExternal",
- meas,
- cov);
- ftr->setLandmarkId(detection.id);
-
- if (detection.id != -1 and detection.id != 0)
- ids.insert(detection.id);
-
- //unmatched_detections_incoming_.push_back(ftr);
+ FeatureBasePtr ftr =
+ FeatureBase::emplace<FeatureLandmarkExternal>(cap_landmarks, meas, cov, detection.id, detection.type);
+ if (detection.id >= 0) ids.insert(detection.id);
// add new feature
new_features_incoming_.push_back(ftr);
}
- WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ", new_features_incoming_.size(), " new features");
+ WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ",
+ new_features_incoming_.size(),
+ " features in incoming capture");
}
unsigned int ProcessorLandmarkExternal::processKnown()
@@ -112,24 +109,141 @@ unsigned int ProcessorLandmarkExternal::processKnown()
for (auto feat_last : known_features_last_)
{
+ auto feat_lmk_last = std::static_pointer_cast<FeatureLandmarkExternal>(feat_last);
+ bool matched = false;
+
+ // First we try to match by EXTERNAL_ID (if defined)
+ if (feat_lmk_last->getExternalId() != -1)
+ {
+ 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_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,
+ " matched with feature ",
+ feat_lmk_candidate,
+ " with landmark ID: ",
+ feat_lmk_candidate->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID if candidate has LANDMARK_ID
+ if (feat_lmk_candidate->landmarkId() != 0)
+ feat_lmk_last->setLandmarkId(feat_lmk_candidate->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++;
+ }
+ }
+ if (matched) continue;
+ }
+ // Second we try to match by TYPE (if defined)
+ if (feat_lmk_last->getExternalType() != -1)
+ {
+ auto feat_candidate_it = new_features_incoming_.begin();
+ while (feat_candidate_it != new_features_incoming_.end())
+ {
+ 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,
+ " matched with feature ",
+ feat_lmk_candidate,
+ " with landmark ID: ",
+ feat_lmk_candidate->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID if candidate has LANDMARK_ID
+ if (feat_lmk_candidate->landmarkId() != 0)
+ feat_lmk_last->setLandmarkId(feat_lmk_candidate->landmarkId());
+
+ // set EXTERNAL_ID if candidate has EXTERNAL_ID
+ if (feat_lmk_candidate->getExternalId() != -1)
+ feat_lmk_last->setExternalId(feat_lmk_candidate->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++;
+ }
+ }
+ if (matched) continue;
+ }
+ // Finally, we try to match by distance
auto feat_candidate_it = new_features_incoming_.begin();
while (feat_candidate_it != new_features_incoming_.end())
{
- if ((*feat_candidate_it)->landmarkId() == feat_last->landmarkId() and
- detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < params_tfle_->match_dist_th)
+ auto feat_lmk_candidate = std::static_pointer_cast<FeatureLandmarkExternal>(*feat_candidate_it);
+ if (feat_lmk_candidate->getExternalType() == feat_lmk_last->getExternalType())
{
- // grow track
- track_matrix_.add(feat_last, *feat_candidate_it);
-
- // feature is known
- known_features_incoming_.push_back((*feat_candidate_it));
-
- // remove from unmatched
- feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
- break;
+ // 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,
+ " matched with feature ",
+ feat_lmk_candidate,
+ " with landmark ID: ",
+ feat_lmk_candidate->landmarkId());
+ matched = true;
+
+ // set LANDMARK_ID if candidate has LANDMARK_ID
+ if (feat_lmk_candidate->landmarkId() != 0)
+ feat_lmk_last->setLandmarkId(feat_lmk_candidate->landmarkId());
+
+ // set TYPE if candidate has TYPE
+ if (feat_lmk_candidate->getExternalType() != -1)
+ feat_lmk_last->setExternalType(feat_lmk_candidate->getExternalType());
+
+ // set EXTERNAL_ID if candidate has EXTERNAL_ID
+ if (feat_lmk_candidate->getExternalId() != -1)
+ feat_lmk_last->setExternalId(feat_lmk_candidate->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++;
}
- else
- feat_candidate_it++;
}
}
WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
@@ -139,7 +253,19 @@ unsigned int ProcessorLandmarkExternal::processKnown()
WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
while (not new_features_incoming_.empty())
{
- auto ftr = new_features_incoming_.front();
+ auto ftr = std::static_pointer_cast<FeatureLandmarkExternal>(new_features_incoming_.front());
+
+ // loop closure
+ if (params_tfle_->close_loops_when_ids_match and ftr->getExternalId() != -1 and
+ external_id_to_landmark_id_.count(ftr->getExternalId()) != 0)
+ {
+ WOLF_DEBUG("Feature ",
+ ftr->id(),
+ " set to close loop with landmark ",
+ external_id_to_landmark_id_.at(ftr->getExternalId()));
+
+ ftr->setLandmarkId(external_id_to_landmark_id_.at(ftr->getExternalId()));
+ }
// new track
track_matrix_.newTrack(ftr);
@@ -152,20 +278,23 @@ unsigned int ProcessorLandmarkExternal::processKnown()
}
// check all features have been emplaced
- assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ assert(std::all_of(known_features_incoming_.begin(),
+ known_features_incoming_.end(),
+ [](FeatureBasePtr f) { return f->getCapture() != nullptr; }) &&
"any not linked feature returned by trackFeatures()");
return known_features_incoming_.size();
}
-double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
- FeatureBasePtr _ftr2,
- const VectorComposite& _pose1,
- const VectorComposite& _pose2,
- const VectorComposite& _pose_sen) const
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const
{
// Any not available info of poses, assume identity
- if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or not _pose_sen.includesStructure("PO"))
+ if (not _pose1.includesStructure("PO") or not _pose2.includesStructure("PO") or
+ not _pose_sen.includesStructure("PO"))
{
if (getProblem()->getDim() == 2)
return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
@@ -178,9 +307,9 @@ double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
{
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>();
- return (p1-p2).norm();
+ 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>();
+ return (p1 - p2).norm();
}
else
{
@@ -188,7 +317,7 @@ 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>();
- return (p1-p2).norm();
+ return (p1 - p2).norm();
}
}
}
@@ -197,87 +326,119 @@ bool ProcessorLandmarkExternal::voteForKeyFrame() const
{
auto track_ids_last = track_matrix_.trackIds(last_ptr_);
- WOLF_DEBUG("Active feature tracks: " , track_ids_last.size() );
-
- // no tracks longer than filter_track_length_th
- auto n_tracks = 0;
+ WOLF_DEBUG("Active feature tracks: ", track_ids_last.size());
+
+ // number of tracks longer than filter_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)
{
n_tracks++;
- if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId()))
- n_new_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_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,
+ ")");
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_DEBUG("vote_new_features: ",
+ vote_new_features,
+ " - n_new_tracks = ",
+ n_new_tracks,
+ " (should be equal or bigger than ",
+ params_tfle_->new_features_for_keyframe,
+ ")");
// Sufficient condition: time span
vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
- WOLF_DEBUG("vote_time_span: " , vote_time_span,
- " - time_span = ", last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
- " (should be bigger than ", params_tfle_->time_span, ")");
+ WOLF_DEBUG("vote_time_span: ",
+ vote_time_span,
+ " - time_span = ",
+ last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+ " (should be bigger than ",
+ params_tfle_->time_span,
+ ")");
}
bool vote = vote_min_features and (vote_new_features or vote_time_span);
- WOLF_DEBUG( (vote ? "Vote ": "Do NOT vote ") , "for KF" );
+ WOLF_DEBUG((vote ? "Vote " : "Do NOT vote "), "for KF");
return vote;
}
void ProcessorLandmarkExternal::establishFactors()
{
- if (origin_ptr_ == last_ptr_)
- return;
+ WOLF_DEBUG("establishFactors");
+ // if (origin_ptr_ == last_ptr_) return;
// reset n_tracks_origin_
lmks_ids_origin_.clear();
- // will emplace a factor (and landmark if needed) for each known feature in last with long tracks (params_tfle_->filter_track_length_th)
+ // 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 track_ids_last = track_matrix_.trackIds(last_ptr_);
for (auto track_id : track_ids_last)
{
- // check track length
- if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th)
- continue;
+ 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;
+
+ WOLF_DEBUG("Track long enough: ", track_matrix_.trackSize(track_id));
- auto feature = track_matrix_.feature(track_id, last_ptr_);
+ auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
+
+ // Generate a new LANDMARK_ID if not set
+ WOLF_DEBUG_COND(feature->landmarkId() == 0, "Landmark id not set, getting available id...");
+ if (feature->landmarkId() == 0) // id 0 will not be used
+ {
+ feature->setLandmarkId(getAvailableLandmarkId());
+ }
+ WOLF_DEBUG("Landmark id: ", feature->landmarkId());
+
+ // store correspondence EXTERNAL_ID - LANDMARK_ID
+ if (feature->getExternalId() != -1)
+ external_id_to_landmark_id_[feature->getExternalId()] = feature->landmarkId();
// get landmark
LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
-
- // emplace landmark
- if (not lmk)
- lmk = emplaceLandmark(feature);
- // modify landmark
+ // emplace landmark if doesn't exist
+ WOLF_DEBUG_COND(not lmk, "Landmark does not exist, emplacing...");
+ if (not lmk) lmk = emplaceLandmark(feature);
+
+ // modify landmark (add missing state blocks)
modifyLandmark(lmk, feature);
// emplace factor
auto fac = emplaceFactor(feature, lmk);
+ // TODO: emplace factors for previous tracked features (in keyframes)
+
+ // store tracked landmarks from origin (used by voteForKeyframe())
lmks_ids_origin_.insert(lmk->id());
- if (fac)
- fac_list.push_back(fac);
+ if (fac) fac_list.push_back(fac);
}
WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
@@ -287,51 +448,38 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
{
assert(_feature);
assert(_landmark);
-
+
// 2D - Relative Position
if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
{
- return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
// 2D - Relative Pose
else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
{
- return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ else if (getProblem()->getDim() == 3 and
+ (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
{
- return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
// 3D - Relative Pose
else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
{
- return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature,
- _feature,
- _landmark,
- shared_from_this(),
- params_tfle_->apply_loss_function);
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
+ _feature, _feature, _landmark, shared_from_this(), params_tfle_->apply_loss_function);
}
- else
+ else
throw std::runtime_error("ProcessorLandmarkExternal::emplaceFactor unknown case");
- // not reachable
- return nullptr;
-
+ return nullptr; // not going to happen
}
-
+
LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
{
assert(_feature);
@@ -343,7 +491,8 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
assert(getProblem()->getMap());
if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
- throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
LandmarkBasePtr lmk;
@@ -352,15 +501,13 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
- double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
-
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePosition2d",
- std::make_shared<StatePoint2d>(lmk_p),
- nullptr);
+ lmk = LandmarkBase::emplace<LandmarkBase>(
+ getProblem()->getMap(), "LandmarkRelativePosition2d", std::make_shared<StatePoint2d>(lmk_p), nullptr);
lmk->setId(_feature->landmarkId());
}
// 2D - Relative Pose
@@ -368,11 +515,12 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
{
Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
- double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
- double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
- Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
- double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+ Vector2d lmk_p =
+ frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
"LandmarkRelativePose2d",
@@ -381,30 +529,29 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
lmk->setId(_feature->landmarkId());
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ else if (getProblem()->getDim() == 3 and
+ (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
{
- Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
- Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePosition3d",
- std::make_shared<StatePoint3d>(lmk_p),
- nullptr);
+ lmk = LandmarkBase::emplace<LandmarkBase>(
+ getProblem()->getMap(), "LandmarkRelativePosition3d", std::make_shared<StatePoint3d>(lmk_p), nullptr);
lmk->setId(_feature->landmarkId());
}
// 3D - Relative Pose
else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
{
- Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
- Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
- Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
@@ -413,14 +560,13 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
std::make_shared<StateQuaternion>(lmk_o));
lmk->setId(_feature->landmarkId());
}
- else
+ else
throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
return lmk;
}
-void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
- FeatureBasePtr _feature)
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -428,9 +574,8 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
assert(_feature->getCapture()->getSensorP());
assert(_feature->getCapture()->getSensorO());
assert(getProblem());
-
- if (not _landmark)
- throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
+
+ if (not _landmark) throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
// 2D - Relative Position
if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
@@ -453,7 +598,8 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
}
}
// 3D - Relative Position
- else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ else if (getProblem()->getDim() == 3 and
+ (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
{
// nothing to do (we assume P already in landmark)
return;
@@ -472,9 +618,20 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
_landmark->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
}
}
- else
+ else
throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark unknown case");
-}
+}
+
+SizeStd ProcessorLandmarkExternal::getAvailableLandmarkId() const
+{
+ auto lmk_list = getProblem()->getMap()->getLandmarkList();
+
+ SizeStd max_id = 0;
+ for (auto lmk : lmk_list)
+ if (lmk->id() > max_id) max_id = lmk->id();
+
+ return max_id + 1;
+}
void ProcessorLandmarkExternal::advanceDerived()
{
@@ -487,11 +644,12 @@ void ProcessorLandmarkExternal::resetDerived()
known_features_last_ = std::move(known_features_incoming_);
}
-} // namespace wolf
+} // namespace wolf
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorLandmarkExternal)
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorLandmarkExternal)
-} // namespace wolf
+} // namespace wolf
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index ca466ccc1..d6e50e760 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -44,58 +44,63 @@ using namespace Eigen;
class ProcessorLandmarkExternalTest : public testing::Test
{
- protected:
- int dim;
- bool orientation;
- TimeStamp t;
- double dt;
-
- ProblemPtr problem;
- SolverManagerPtr solver;
- SensorBasePtr sensor, sensor_odom;
- ProcessorLandmarkExternalPtr processor;
- ProcessorMotionPtr processor_motion;
- std::vector<LandmarkBasePtr> landmarks;
-
- // Groundtruth states
- VectorComposite state_robot, state_sensor;
- std::vector<VectorComposite> state_landmarks;
-
- virtual void SetUp() {};
- void initProblem(int _dim,
- bool _orientation,
- double _quality_th,
- double _dist_th,
- unsigned int _track_length_th,
- double _time_span,
- bool _add_landmarks);
- void randomStep();
- CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
- void testConfiguration(int dim,
- bool orientation,
- double quality_th,
- double dist_th,
- int track_length,
- double time_span,
- bool add_landmarks);
- void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
+ protected:
+ int dim;
+ bool orientation;
+ int mode; // 0: external ID, 1: external TYPE, 2: nothing, 3: all mixed
+ TimeStamp t;
+ double dt;
+
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor, sensor_odom;
+ ProcessorLandmarkExternalPtr processor;
+ ProcessorMotionPtr processor_motion;
+ std::vector<LandmarkBasePtr> landmarks;
+
+ // Groundtruth states
+ VectorComposite state_robot, state_sensor;
+ std::vector<VectorComposite> state_landmarks;
+
+ virtual void SetUp(){};
+ void initProblem(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _init_landmarks);
+ void randomStep();
+ CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
+ void testConfiguration(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ int _track_length,
+ double _time_span,
+ bool _init_landmarks);
+ void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
};
-void ProcessorLandmarkExternalTest::initProblem(int _dim,
- bool _orientation,
- double _quality_th,
- double _dist_th,
+void ProcessorLandmarkExternalTest::initProblem(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
unsigned int _track_length_th,
- double _time_span,
- bool _add_landmarks)
+ double _time_span,
+ bool _init_landmarks)
{
- dim = _dim;
+ dim = _dim;
orientation = _orientation;
- t = TimeStamp(0);
- dt = 0.1;
+ mode = _mode;
+ t = TimeStamp(0);
+ dt = 0.1;
problem = Problem::create("PO", dim);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = std::make_shared<SolverCeres>(problem);
// Sensors
if (dim == 2)
@@ -106,58 +111,57 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
std::make_shared<StateAngle>(Vector1d::Random() * M_PI),
nullptr,
2);
- sensor_odom = SensorBase::emplace<SensorOdom2d>(problem->getHardware(),
- Vector3d::Zero(),
- ParamsSensorOdom2d());
-
+ sensor_odom =
+ SensorBase::emplace<SensorOdom2d>(problem->getHardware(), Vector3d::Zero(), ParamsSensorOdom2d());
}
else
{
- sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
std::make_shared<StatePoint3d>(Vector3d::Random()),
std::make_shared<StateQuaternion>(Vector4d::Random().normalized()),
nullptr,
3);
- sensor_odom = SensorBase::emplace<SensorOdom3d>(problem->getHardware(),
- (Vector7d() << 0,0,0,0,0,0,1).finished(),
- ParamsSensorOdom3d());
+ sensor_odom = SensorBase::emplace<SensorOdom3d>(
+ problem->getHardware(), (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), ParamsSensorOdom3d());
}
// 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->min_features_for_keyframe = 1;
- params->new_features_for_keyframe = 1000;
- params->filter_track_length_th = _track_length_th;
- params->time_span = _time_span;
- processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
+ 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->min_features_for_keyframe = 1;
+ params->new_features_for_keyframe = 1000;
+ params->filter_track_length_th = _track_length_th;
+ params->time_span = _time_span;
+ processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
if (dim == 2)
{
- auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
- params_odom->state_getter = true;
+ auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
+ params_odom->state_getter = true;
params_odom->voting_active = false;
- processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom,
- params_odom));
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(
+ ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom, params_odom));
}
else
{
- auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
- params_odom->state_getter = true;
+ auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
+ params_odom->state_getter = true;
params_odom->voting_active = false;
- processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom,
- params_odom));
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(
+ ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom, params_odom));
}
// Emplace frame
- auto F0 = problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished()));
+ auto F0 =
+ problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0, 0, 0, 0, 0, 0, 1).finished()));
F0->fix();
processor->keyFrameCallback(F0);
processor_motion->setOrigin(F0);
@@ -167,21 +171,19 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
{
LandmarkBasePtr lmk;
if (dim == 2)
- lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
- "LandmarkExternal",
- std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
- (orientation ?
- std::make_shared<StateAngle>(Vector1d::Random() * M_PI) :
- nullptr));
-
- else
- lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
- "LandmarkExternal",
- std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
- (orientation ?
- std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) :
- nullptr));
- lmk->setId(i);
+ lmk = LandmarkBase::emplace<LandmarkBase>(
+ _init_landmarks ? problem->getMap() : nullptr,
+ "LandmarkExternal",
+ std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
+ (orientation ? std::make_shared<StateAngle>(Vector1d::Random() * M_PI) : nullptr));
+
+ else
+ lmk = LandmarkBase::emplace<LandmarkBase>(
+ _init_landmarks ? problem->getMap() : nullptr,
+ "LandmarkExternal",
+ std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
+ (orientation ? std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) : nullptr));
+ lmk->setId(i + 1);
landmarks.push_back(lmk);
state_landmarks.push_back(lmk->getState());
}
@@ -198,7 +200,7 @@ void ProcessorLandmarkExternalTest::randomStep()
MatrixXd delta_cov;
if (dim == 2)
{
- delta = Vector2d::Random() * 0.1;
+ delta = Vector2d::Random() * 0.1;
delta_cov = Matrix2d::Identity() * 0.1;
}
else
@@ -223,10 +225,13 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
for (auto i = 0; i < landmarks.size(); i++)
{
// compute detection
- VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
+ VectorXd meas(orientation ? (dim == 2 ? 3 : 7) : (dim == 2 ? 2 : 3));
if (dim == 2)
{
- meas.head(2) = Rotation2Dd(-state_sensor.at('O')(0))*(Rotation2Dd(-state_robot.at('O')(0))*(state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ meas.head(2) =
+ Rotation2Dd(-state_sensor.at('O')(0)) *
+ (Rotation2Dd(-state_robot.at('O')(0)) * (state_landmarks.at(i).at('P') - state_robot.at('P')) -
+ state_sensor.at('P'));
if (orientation)
meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
}
@@ -235,38 +240,51 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
auto q_sensor = Quaterniond(Vector4d(state_sensor.at('O')));
auto q_robot = Quaterniond(Vector4d(state_robot.at('O')));
- meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ meas.head(3) =
+ q_sensor.conjugate() *
+ (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
if (orientation)
- meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O')))).coeffs();
+ meas.tail(4) =
+ (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O'))))
+ .coeffs();
}
// cov
MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
- if (orientation and dim != 2)
- cov = MatrixXd::Identity(6, 6);
+ if (orientation and dim != 2) cov = MatrixXd::Identity(6, 6);
// quality
- double quality = (double) i / (double) (landmarks.size()-1); // increasing from 0 to 1
+ double quality = (double)i / (double)(landmarks.size() - 1); // increasing from 0 to 1
// add detection
- cap->addDetection(landmarks.at(i)->id(), meas, cov, quality );
+ // with ID
+ if (mode == 0 or (mode == 3 and i % 3 == 0)) cap->addDetection(landmarks.at(i)->id(), -1, meas, cov, quality);
+ // with TYPE
+ else if (mode == 1 or (mode == 3 and i % 3 == 1))
+ cap->addDetection(-1, landmarks.at(i)->id(), meas, cov, quality);
+ // with nothing
+ else if (mode == 2 or (mode == 3 and i % 3 == 2))
+ cap->addDetection(-1, -1, meas, cov, quality);
}
return cap;
}
-void ProcessorLandmarkExternalTest::testConfiguration(int dim,
- bool orientation,
- double quality_th,
- double dist_th,
- int track_length,
- double time_span,
- bool add_landmarks)
+void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
+ bool _orientation,
+ int _mode,
+ double _quality_th,
+ double _dist_th,
+ int _track_length,
+ double _time_span,
+ bool _init_landmarks)
{
- initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);
+ initProblem(_dim, _orientation, _mode, _quality_th, _dist_th, _track_length, _time_span, _init_landmarks);
+
+ auto n_keyframes = problem->getTrajectory()->size();
ASSERT_TRUE(problem->check());
- for (auto i = 0; i<10; i++)
+ for (auto i = 0; i < 10; i++)
{
WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
@@ -277,73 +295,90 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
// process detections
cap->process();
ASSERT_TRUE(problem->check());
- problem->print(4,1,1,1);
+ problem->print(4, 1, 1, 1);
// CHECKS
FactorBasePtrList fac_list;
problem->getTrajectory()->getFactorList(fac_list);
// should emplace KF in last?
- bool any_active_track = quality_th <=1 and i >= track_length-1;
- bool should_emplace_KF = t-dt > time_span and any_active_track;
- WOLF_INFO("should emplace KF: ", should_emplace_KF, " t-dt = ", t-dt, " time_span = ", time_span, " track_length-1 = ", track_length-1);
-
+ bool any_active_track = _quality_th <= 1 and i >= _track_length - 1;
+ bool should_emplace_KF = (t - dt > _time_span and any_active_track);
+ bool should_emplace_factors = i == 1 or should_emplace_KF;
+ WOLF_INFO("should emplace KF: ",
+ should_emplace_KF,
+ "should emplace factors: ",
+ should_emplace_factors,
+ " i = ",
+ i,
+ " t-dt = ",
+ t - dt,
+ " time_span = ",
+ _time_span,
+ " track_length-1 = ",
+ _track_length - 1);
+
+ // Check voted&emplaced a keyframe
if (should_emplace_KF)
- {
- // voted for keyframe
- ASSERT_TRUE(problem->getTrajectory()->size() > 1);
-
- // emplaced factors
+ {
+ ASSERT_EQ(problem->getTrajectory()->size(), n_keyframes + 1);
+ n_keyframes++;
+ }
+ else
+ {
+ ASSERT_EQ(problem->getTrajectory()->size(), n_keyframes);
+ }
+
+ // Check emplaced factors and landmarks
+ if (should_emplace_factors)
+ {
ASSERT_FALSE(fac_list.empty());
-
+
// factors' type
for (auto fac : fac_list)
{
if (fac->getProcessor() == processor)
{
- ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
- (orientation ? "Pose" : "Position") +
- (dim == 2 ? "2d" : "3d") +
- "WithExtrinsics");
+ ASSERT_EQ(fac->getType(),
+ std::string("FactorRelative") + (orientation ? "Pose" : "Position") +
+ (dim == 2 ? "2d" : "3d") + "WithExtrinsics");
}
}
// landmarks created by processor
- if (not add_landmarks)
+ if (not _init_landmarks)
{
auto landmarks_map = problem->getMap()->getLandmarkList();
- // amount of landmarks due to quality filtering of detections
- auto n_landmarks = (quality_th <= 0 ? 3 : (quality_th <= 0.5 ? 2 : (quality_th <= 1 ? 1 : 0)));
+ // amount of landmarks according to quality filtering of detections
+ auto n_landmarks = (_quality_th <= 0 ? 3 : (_quality_th <= 0.5 ? 2 : (_quality_th <= 1 ? 1 : 0)));
ASSERT_EQ(landmarks_map.size(), n_landmarks);
// check state correctly initialized
for (auto lmk_map : landmarks_map)
{
ASSERT_TRUE(lmk_map->id() < landmarks.size());
- ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
+ ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id() - 1)->id());
assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
}
}
}
else
{
- // didn't vote for keyframe
- ASSERT_FALSE(problem->getTrajectory()->size() > 1);
// no factors emplaced
ASSERT_TRUE(fac_list.empty());
+
// no landmarks created yet (if not added by the test)
- ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not _init_landmarks);
}
// step with random movement
- t+=dt;
+ t += dt;
randomStep();
// solve
- if (should_emplace_KF)
+ if (should_emplace_factors)
{
// perturb landmarks
- for (auto lmk : problem->getMap()->getLandmarkList())
- lmk->perturb();
+ for (auto lmk : problem->getMap()->getLandmarkList()) lmk->perturb();
// fix frame and extrinsics
sensor->fix();
@@ -353,9 +388,7 @@ void ProcessorLandmarkExternalTest::testConfiguration(int dim,
auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report);
- // check values
- //assertVectorComposite(sensor->getState("PO"), state_sensor);
- //assertVectorComposite(problem->getState("PO"), state_robot);
+ // check values
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
@@ -386,141 +419,584 @@ void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite&
}
// / TESTS //////////////////////////////////////////
-TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks)
-{
- testConfiguration(2, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 6, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks)
-{
- testConfiguration(2, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 2, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality)
-{
- testConfiguration(2, //int dim
- false, //bool orientation
- 0.3, //double quality_th
- 1e6, //double dist_th
- 6, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks)
-{
- testConfiguration(2, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 3, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks)
-{
- testConfiguration(2, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 1, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality)
-{
- testConfiguration(2, //int dim
- true, //bool orientation
- 0.3, //double quality_th
- 1e6, //double dist_th
- 0, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks)
+TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks_id)
{
- testConfiguration(3, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 7, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
+ testConfiguration(2, // 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_3d_no_lmks)
+// 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(3, //int dim
- false, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 53, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
+ 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_3d_no_lmks_quality)
-{
- testConfiguration(3, //int dim
- false, //bool orientation
- 0.3, //double quality_th
- 1e6, //double dist_th
- 2, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks)
-{
- testConfiguration(3, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 1, //int track_length
- 4.5*dt, //double time_span
- true); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks)
-{
- testConfiguration(3, //int dim
- true, //bool orientation
- 0, //double quality_th
- 1e6, //double dist_th
- 4, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality)
-{
- testConfiguration(3, //int dim
- true, //bool orientation
- 0.2, //double quality_th
- 1e6, //double dist_th
- 5, //int track_length
- 4.5*dt, //double time_span
- false); //bool add_landmarks
-}
-
-
-int main(int argc, char **argv)
+// 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
+// }
+
+int main(int argc, char** argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
--
GitLab