diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
index f5c9ad4632d0c704b73c5273c2d9ba3ee7930d0d..27af193e1f73de46ec3f934c0f89b0ff0f2a9750 100644
--- a/include/core/capture/capture_landmarks_external.h
+++ b/include/core/capture/capture_landmarks_external.h
@@ -28,9 +28,10 @@ namespace wolf {
struct LandmarkDetection
{
+ int id; // id of landmark
Eigen::VectorXd measure; // either pose or position
Eigen::MatrixXd covariance; // covariance of the measure
- int id; // id of landmark
+ double quality; // [0, 1] quality of the detection
};
WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
@@ -39,21 +40,22 @@ WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
class CaptureLandmarksExternal : public CaptureBase
{
protected:
- std::list<LandmarkDetection> detections_;
+ std::vector<LandmarkDetection> detections_;
public:
CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
- const std::vector<Eigen::VectorXd>& _detections,
- const std::vector<Eigen::MatrixXd>& _covs,
- const std::vector<int>& _ids);
+ const std::vector<int>& _ids = {},
+ const std::vector<Eigen::VectorXd>& _detections = {},
+ const std::vector<Eigen::MatrixXd>& _covs = {},
+ const std::vector<double>& _qualities = {});
~CaptureLandmarksExternal() override;
- std::list<LandmarkDetection> getDetections() const {return detections_;};
+ std::vector<LandmarkDetection> getDetections() const {return detections_;};
- void addDetection(const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const int& _id);
+ void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality);
};
} //namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
index 0204203f330403b3821dcc784706989b4f7c1c08..680c619c3622092e4e0631fb6c74c594794ce70d 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -179,14 +179,14 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
// FRAME CASE
if (not getFrameOtherList().empty())
{
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - p_w_r) - p_r_s);
expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t * q_r_s;
- expected_p = q_r_s.conjugate() * q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - (p_w_r + q_w_r * p_r_s));
}
// LANDMARK CASE
else if (not getLandmarkOtherList().empty())
{
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t;
- expected_p = q_r_s.conjugate() * q_w_r.conjugate() * (p_w_t - (p_w_r + q_w_r * p_r_s));
}
// Measurement
diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h
index 9e3173a37d124e18f108818581b8e1fc1d504d0c..578181788acdc0b74cf938d8e1b2e4dd0ce27d55 100644
--- a/include/core/processor/processor_tracker_feature_landmark_external.h
+++ b/include/core/processor/processor_tracker_feature_landmark_external.h
@@ -32,14 +32,16 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
{
- unsigned int min_track_length; ///< length of the track necessary to consider the detection
+ unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
+ double filter_quality_th; ///< min quality to consider the detection
ParamsProcessorTrackerFeatureLandmarkExternal() = default;
ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
const wolf::ParamsServer & _server):
ParamsProcessorTrackerFeature(_unique_name, _server)
{
- min_track_length = _server.getParam<unsigned int>(prefix + _unique_name + "/min_track_length");
+ filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
+ filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter_quality_th");
}
};
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
index ebf00489363b359f08e9ec818395948f559623ad..154dcc83f488b39a91c69f0906a0f59e2fdd56d7 100644
--- a/src/capture/capture_landmarks_external.cpp
+++ b/src/capture/capture_landmarks_external.cpp
@@ -25,16 +25,19 @@ namespace wolf{
CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids,
const std::vector<Eigen::VectorXd>& _detections,
const std::vector<Eigen::MatrixXd>& _covs,
- const std::vector<int>& _ids) :
+ const std::vector<double>& _qualities) :
CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
{
- if (_detections.size() != _covs.size() or _detections.size() != _ids.size())
- throw std::runtime_error("CaptureLandmarksExternal constructor: '_detections', '_covs' and '_ids' should have the same size.");
+ if (_ids.size() != _detections.size() or
+ _ids.size() != _covs.size() or
+ _ids.size() != _qualities.size())
+ throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
for (auto i = 0; i < _detections.size(); i++)
- addDetection(_detections.at(i), _covs.at(i), _ids.at(i));
+ addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
}
CaptureLandmarksExternal::~CaptureLandmarksExternal()
@@ -42,9 +45,9 @@ CaptureLandmarksExternal::~CaptureLandmarksExternal()
//
}
-void CaptureLandmarksExternal::addDetection(const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const int& _id)
+void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
{
- detections_.push_back(LandmarkDetection{_detection, _cov, _id});
+ detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
}
} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 1aaf3bc58cca2d9157c998e5fd1d5d0c1cf90e56..73af9e2ed1d3f9fbbdc32240eb2ef83d9b07ff46 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -191,6 +191,9 @@ wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
+# ProcessorTrackerFeatureLandmarkExternal class test
+wolf_add_gtest(gtest_processor_tracker_feature_landmark_external gtest_processor_tracker_feature_landmark_external.cpp)
+
# ProcessorFixedWingModel class test
wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_tracker_feature_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..944d0cd90300fc9a85b4a3c6c78a2ec58969ba24
--- /dev/null
+++ b/test/gtest_processor_tracker_feature_landmark_external.cpp
@@ -0,0 +1,273 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 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/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_quaternion.h"
+
+#include "core/processor/processor_tracker_feature_landmark_external.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
+{
+ protected:
+ int dim;
+ bool orientation;
+ TimeStamp t;
+ double dt;
+
+ ProblemPtr problem;
+ SensorBasePtr sensor;
+ ProcessorTrackerFeatureLandmarkExternalPtr processor;
+ std::vector<LandmarkBasePtr> landmarks;
+
+ // Groundtruth poses
+ VectorXd p_robot, o_robot, p_sensor, o_sensor;
+
+ virtual void SetUp() {};
+ void initProblem(int _dim, bool _orientation);
+ CaptureLandmarksExternalPtr randomStep();
+ void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id) const;
+};
+
+void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, bool _orientation)
+{
+ dim = _dim;
+ orientation = _orientation;
+ t = TimeStamp(0);
+ dt = 0.1;
+
+ problem = Problem::create("PO", dim);
+
+ if (dim == 2)
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StatePoint2d>(Vector2d::Random()),
+ std::make_shared<StateAngle>(Vector1d::Random() * M_PI),
+ nullptr,
+ 2);
+ else
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StatePoint3d>(Vector3d::Random()),
+ std::make_shared<StateQuaternion>(Vector4d::Random().normalized()),
+ nullptr,
+ 3);
+
+
+ // Emplace processor
+ ParamsProcessorTrackerFeatureLandmarkExternalPtr params = std::make_shared<ParamsProcessorTrackerFeatureLandmarkExternal>();
+ params->time_tolerance = dt/2;
+ params->min_features_for_keyframe = 0;
+ params->max_new_features = -1;
+ params->voting_active = true;
+ params->apply_loss_function = false;
+ processor = ProcessorBase::emplace<ProcessorTrackerFeatureLandmarkExternal>(sensor, params);
+
+ // Emplace frame
+ auto F0 = problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished()));
+ F0->fix();
+ problem->keyFrameCallback(F0, nullptr);
+
+ // Emplace 3 random landmarks
+ for (auto i = 0; i < 3; i++)
+ {
+ LandmarkBasePtr lmk;
+ if (dim == 2)
+ lmk = LandmarkBase::emplace<LandmarkBase>(problem->getMap(),
+ "LandmarkExternal",
+ std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
+ (orientation ?
+ std::make_shared<StateAngle>(Vector1d::Random() * M_PI) :
+ nullptr));
+
+ else
+ lmk = LandmarkBase::emplace<LandmarkBase>(problem->getMap(),
+ "LandmarkExternal",
+ std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
+ (orientation ?
+ std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) :
+ nullptr));
+ lmk->setId(i);
+ landmarks.push_back(lmk);
+ }
+
+ // Store groundtruth poses
+ p_robot = F0->getP()->getState();
+ o_robot = F0->getO()->getState();
+ p_sensor = sensor->getP()->getState();
+ o_sensor = sensor->getO()->getState();
+}
+
+CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
+{
+ // update groundtruth pose with random movement
+ p_robot += VectorXd::Random(dim) * 0.1;
+ if (dim == 2)
+ o_robot(0) = pi2pi(o_robot(0) + Vector1d::Random()(0) * 0.1);
+ else
+ o_robot = (o_robot + Vector4d::Random() * 0.1).normalized();
+
+ // Detections
+ auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
+ VectorXd p_lmk, o_lmk;
+ for (auto i = 0; i < landmarks.size(); i++)
+ {
+ p_lmk = landmarks.at(i)->getP()->getState();
+ if (orientation)
+ o_lmk = landmarks.at(i)->getO()->getState();
+
+ // compute detection
+ VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
+ if (dim == 2)
+ {
+ meas.head(2) = Rotation2Dd(-o_sensor(0))*(Rotation2Dd(-o_robot(0))*(p_lmk - p_robot) - p_sensor);
+ if (orientation)
+ meas(2) = o_lmk(0) - p_robot(0);
+ }
+ else
+ {
+ auto q_sensor = Quaterniond(Vector4d(o_sensor));
+ auto q_robot = Quaterniond(Vector4d(o_robot));
+
+ meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (p_lmk - p_robot) - p_sensor);
+ if (orientation)
+ meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(o_lmk))).coeffs();
+ }
+ MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
+ if (orientation and dim != 2)
+ cov = MatrixXd::Identity(6, 6);
+
+ // add detection
+ cap->addDetection(meas, cov, landmarks.at(i)->id());
+ }
+
+ return cap;
+}
+
+void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id) const
+{
+ VectorXd detection;
+ MatrixXd cov;
+ if (orientation)
+ {
+ if (dim == 2)
+ {
+ detection = Vector3d::Random()*5;
+ detection(2) = pi2pi(detection(2));
+ cov = Matrix3d::Identity()*0.1;
+ }
+ else
+ {
+ detection = Vector7d::Random()*5;
+ detection.tail<4>().normalize();
+ cov = Matrix6d::Identity()*0.1;
+ }
+ }
+ else
+ {
+ if (dim == 2)
+ {
+ detection = Vector2d::Random()*5;
+ cov = Matrix2d::Identity()*0.1;
+ }
+ else
+ {
+ detection = Vector3d::Random()*5;
+ cov = Matrix3d::Identity()*0.1;
+ }
+ }
+ _cap->addDetection(detection, cov, _id);
+}
+
+// TESTS /////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
+{
+ initProblem(2, false);
+ ASSERT_TRUE(problem->check());
+
+ for (auto i = 0; i<10; i++)
+ {
+ auto cap = randomStep();
+ ASSERT_TRUE(problem->check());
+
+ cap->process();
+ ASSERT_TRUE(problem->check());
+
+ t+=dt;
+ }
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_2d)
+{
+ initProblem(2, true);
+ ASSERT_TRUE(problem->check());
+
+ auto cap = randomStep();
+ ASSERT_TRUE(problem->check());
+
+ cap->process();
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_3d)
+{
+ initProblem(3, false);
+ ASSERT_TRUE(problem->check());
+
+ auto cap = randomStep();
+ ASSERT_TRUE(problem->check());
+
+ cap->process();
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_3d)
+{
+ initProblem(3, true);
+ ASSERT_TRUE(problem->check());
+
+ auto cap = randomStep();
+ ASSERT_TRUE(problem->check());
+
+ cap->process();
+ ASSERT_TRUE(problem->check());
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}