diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 12ba01777409f49bb84afea831d368d0bb59dd0f..d1243c205eaf2cf8711cd6012f5c5dbdc20b710b 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,3 +1,10 @@
+workflow:
+ rules:
+ - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
+ - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
+ when: never
+ - if: '$CI_COMMIT_BRANCH'
+
stages:
- license
- build_and_test
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 685499123554ca81439d4b6c9ee70575bd656186..01bb44f1c27e19e25777a4a6978c47a6c835bd04 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -108,12 +108,13 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
# ============ HEADERS ============
SET(HDRS_CAPTURE
include/${PROJECT_NAME}/capture/capture_base.h
+ include/${PROJECT_NAME}/capture/capture_diff_drive.h
+ include/${PROJECT_NAME}/capture/capture_landmarks_external.h
include/${PROJECT_NAME}/capture/capture_motion.h
include/${PROJECT_NAME}/capture/capture_odom_2d.h
include/${PROJECT_NAME}/capture/capture_odom_3d.h
include/${PROJECT_NAME}/capture/capture_pose.h
include/${PROJECT_NAME}/capture/capture_void.h
- include/${PROJECT_NAME}/capture/capture_diff_drive.h
)
SET(HDRS_COMMON
include/${PROJECT_NAME}/common/factory.h
@@ -133,11 +134,15 @@ SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_pose_2d.h
include/${PROJECT_NAME}/factor/factor_pose_3d.h
include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
+ include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
- include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_2d.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_3d.h
+ include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
@@ -185,6 +190,7 @@ SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/processor_pose.h
include/${PROJECT_NAME}/processor/processor_tracker.h
include/${PROJECT_NAME}/processor/processor_tracker_feature.h
+ include/${PROJECT_NAME}/processor/processor_tracker_feature_landmark_external.h
include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
include/${PROJECT_NAME}/processor/track_matrix.h
)
@@ -244,12 +250,13 @@ SET(HDRS_YAML
# ============ SOURCES ============
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
+ src/capture/capture_diff_drive.cpp
+ src/capture/capture_landmarks_external.cpp
src/capture/capture_motion.cpp
src/capture/capture_odom_2d.cpp
src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
src/capture/capture_void.cpp
- src/capture/capture_diff_drive.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
@@ -294,6 +301,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
+ src/processor/processor_tracker_feature_landmark_external.cpp
src/processor/processor_tracker_landmark.cpp
src/processor/track_matrix.cpp
)
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..27af193e1f73de46ec3f934c0f89b0ff0f2a9750
--- /dev/null
+++ b/include/core/capture/capture_landmarks_external.h
@@ -0,0 +1,61 @@
+//--------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--------
+#pragma once
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+
+namespace wolf {
+
+struct LandmarkDetection
+{
+ int id; // id of landmark
+ Eigen::VectorXd measure; // either pose or position
+ Eigen::MatrixXd covariance; // covariance of the measure
+ double quality; // [0, 1] quality of the detection
+};
+
+WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
+
+//class CaptureLandmarksExternal
+class CaptureLandmarksExternal : public CaptureBase
+{
+ protected:
+ std::vector<LandmarkDetection> detections_;
+
+ public:
+
+ 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<double>& _qualities = {});
+
+ ~CaptureLandmarksExternal() override;
+
+ std::vector<LandmarkDetection> getDetections() const {return detections_;};
+
+ 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_2d.h b/include/core/factor/factor_relative_pose_2d.h
index 4a608feb01c29b413b19608987767853eecf5407..d156213cd912cb8b82bef159804e184cac5018f6 100644
--- a/include/core/factor/factor_relative_pose_2d.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -142,11 +142,11 @@ class FactorRelativePose2d : public FactorAnalytic
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const override;
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::MatrixXd>& jacobians,
- const std::vector<bool>& _compute_jacobian) const override;
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
@@ -175,8 +175,8 @@ inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
@@ -211,8 +211,8 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::MatrixXd>& jacobians,
- const std::vector<bool>& _compute_jacobian) const
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index b0eea0f2b8f91f00a9a2bc8a7bda6ce1be66dc45..f0f70a5c15341c48f03e1b36ce86eb6e1e1415db 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -65,6 +65,8 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
+
+ Eigen::Vector3d residual() const;
};
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
@@ -132,31 +134,49 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
- T o_ref = _o_ref[0];
- T o_target = _o_target[0];
- T o_sensor = _o_sensor[0];
+ const T& o_ref = _o_ref[0];
+ const T& o_target = _o_target[0];
+ const T& o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
- // Expected measurement
- // r1_p_r1_s1 = ps
- // r2_p_r2_s2 = ps
- // r1_R_s1 = R(os)
- // r2_R_s2 = R(os)
- // w_R_r1 = R(o1)
- // w_R_r2 = R(o2)
- // ----------------------------------------
-
- // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
- // s1_R_r1*r1_R_w*w_p_r1_w +
- // s1_R_r1*r1_R_w*w_p_w_r2 +
- // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
-
- // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
-
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
- expected_measurement(2) = o_target - o_ref;
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ // Expected measurement
+ // r1_p_s1 = p_sensor
+ // r2_p_s2 = p_sensor
+ // r1_R_s1 = R(o_sensor)
+ // r2_R_s2 = R(o_sensor)
+ // w_p_r1 = p_ref
+ // w_p_r2 = p_target
+ // w_R_r1 = R(o_ref)
+ // w_R_r2 = R(o_target)
+ // ----------------------------------------
+ // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 + w_R_r2*r2_p_s2 - w_p_r1) - r1_p_s1)
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
+ expected_measurement(2) = o_target - o_ref;
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ // Expected measurement
+ // r1_p_s1 = p_sensor
+ // r2_p_s2 = p_sensor
+ // r1_R_s1 = R(o_sensor)
+ // r2_R_s2 = R(o_sensor)
+ // w_p_r1 = p_ref
+ // w_p_r2 = p_target
+ // w_R_r1 = R(o_ref)
+ // w_R_r2 = R(o_target)
+ // ----------------------------------------
+ // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
+ expected_measurement(2) = o_target - o_ref - o_sensor;
+ }
// Error
er = expected_measurement - getMeasurement().cast<T>();
@@ -186,6 +206,34 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
return true;
}
+inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
+{
+ Eigen::Vector3d res;
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
+ p_sensor = getCapture()->getSensorP()->getState();
+ o_sensor = getCapture()->getSensorO()->getState();
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ p_ref = getFrameOther()->getP()->getState();
+ o_ref = getFrameOther()->getO()->getState();
+ p_target = getCapture()->getFrame()->getP()->getState();
+ o_target = getCapture()->getFrame()->getO()->getState();
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ p_ref = getCapture()->getFrame()->getP()->getState();
+ o_ref = getCapture()->getFrame()->getO()->getState();
+ p_target = getLandmarkOther()->getP()->getState();
+ o_target = getLandmarkOther()->getO()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+ return res;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index 6003c446732399f1abe6c4127dfb257543c82c2c..4ea300d5c13d35aed9ff1c60ba57c822cd4386c6 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * factor_relative_pose_3d.h
- *
- * Created on: Oct 7, 2016
- * Author: jsola
- */
#ifndef FACTOR_RELATIVE_POSE_3D_H_
#define FACTOR_RELATIVE_POSE_3D_H_
@@ -48,24 +42,31 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
+ FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
~FactorRelativePose3d() override = default;
template<typename T>
- bool operator ()(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
+ bool operator ()(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
T* _residuals) const;
template<typename T>
- bool expectation(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
+ bool expectation(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const;
- Eigen::VectorXd expectation() const;
+ Eigen::Vector7d expectation() const;
private:
template<typename T>
@@ -118,39 +119,64 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
+inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ _lmk_ptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _lmk_ptr->getP(), // landmark P
+ _lmk_ptr->getO(), // landmark Q
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO()) // current frame Q
+{
+ MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
template<typename T>
-inline bool FactorRelativePose3d::expectation(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
- T* _expectation_dp,
- T* _expectation_dq) const
+inline bool FactorRelativePose3d::expectation(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
- Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
- Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
- Eigen::Map<const Eigen::Matrix<T,3,1> > p_past(_p_past);
- Eigen::Map<const Eigen::Quaternion<T> > q_past(_q_past);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
-// std::cout << "p_current: " << p_current(0) << " "
-// << p_current(1) << " "
-// << p_current(2) << "\n";
-// std::cout << "q_current: " << q_current.coeffs()(0) << " "
-// << q_current.coeffs()(1) << " "
-// << q_current.coeffs()(2) << " "
-// << q_current.coeffs()(3) << "\n";
-// std::cout << "p_past: " << p_past(0) << " "
-// << p_past(1) << " "
-// << p_past(2) << "\n";
-// std::cout << "q_past: " << q_past.coeffs()(0) << " "
-// << q_past.coeffs()(1) << " "
-// << q_past.coeffs()(2) << " "
-// << q_past.coeffs()(3) << "\n";
+// std::cout << "p_target: " << p_target(0) << " "
+// << p_target(1) << " "
+// << p_target(2) << "\n";
+// std::cout << "q_target: " << q_target.coeffs()(0) << " "
+// << q_target.coeffs()(1) << " "
+// << q_target.coeffs()(2) << " "
+// << q_target.coeffs()(3) << "\n";
+// std::cout << "p_ref: " << p_ref(0) << " "
+// << p_ref(1) << " "
+// << p_ref(2) << "\n";
+// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
+// << q_ref.coeffs()(1) << " "
+// << q_ref.coeffs()(2) << " "
+// << q_ref.coeffs()(3) << "\n";
// estimate motion increment, dp, dq;
- expectation_dp = q_past.conjugate() * (p_current - p_past);
- expectation_dq = q_past.conjugate() * q_current;
+ expectation_dp = q_ref.conjugate() * (p_target - p_ref);
+ expectation_dq = q_ref.conjugate() * q_target;
// std::cout << "exp_p: " << expectation_dp(0) << " "
// << expectation_dp(1) << " "
@@ -163,34 +189,38 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_current,
return true;
}
-inline Eigen::VectorXd FactorRelativePose3d::expectation() const
+inline Eigen::Vector7d FactorRelativePose3d::expectation() const
{
- Eigen::VectorXd exp(7);
+ Eigen::Vector7d exp;
auto frm_current = getFeature()->getFrame();
auto frm_past = getFrameOther();
- const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
- const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
- const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
- const Eigen::VectorXd frame_past_ori = frm_past->getO()->getState();
-
-// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
-// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
-
- expectation(frame_current_pos.data(),
- frame_current_ori.data(),
- frame_past_pos.data(),
- frame_past_ori.data(),
+ auto lmk = getLandmarkOther();
+
+ const Eigen::VectorXd target_pos = ( frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
+ const Eigen::VectorXd target_ori = ( frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
+ const Eigen::VectorXd ref_pos = ( frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState());
+ const Eigen::VectorXd ref_ori = ( frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState());
+
+// std::cout << "target_pos: " << target_pos.transpose() << std::endl;
+// std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl;
+// std::cout << "ref_pos: " << ref_pos.transpose() << std::endl;
+// std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl;
+
+ expectation(target_pos.data(),
+ target_ori.data(),
+ ref_pos.data(),
+ ref_ori.data(),
exp.data(),
exp.data()+3);
return exp;
}
template<typename T>
-inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
- T* _residuals) const
+inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
+ T* _residuals) const
{
/* Residual expression
@@ -232,7 +262,7 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
- expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
+ expectation(_p_target, _q_target, _p_ref, _q_ref, expected.data(), expected.data()+3);
// measured motion increment, dp_m, dq_m
// Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
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 19245f6bcf630f737e4a01aa78b4a11030d8264a..66468bbcafb2b4480df5077ca6b2a16d6ff61839 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -58,7 +58,7 @@ class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativeP
/** \brief Class Destructor
*/
- ~FactorRelativePose3dWithExtrinsics() override;
+ ~FactorRelativePose3dWithExtrinsics() override = default;
/** brief : compute the residual from the state blocks being iterated by the solver.
**/
@@ -116,6 +116,8 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
_feature_ptr->getCapture()->getSensorP(),
_feature_ptr->getCapture()->getSensorO())
{
+ assert(_feature_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_feature_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
}
inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
@@ -143,45 +145,59 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
{
}
-inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
-{
- //
-}
-
template<typename T>
inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
- const T* const _o_ref,
- const T* const _p_target,
- const T* const _o_target,
- const T* const _p_sensor,
- const T* const _o_sensor,
- T* _residuals) const
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
{
// Maps
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_sensor);
- Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_sensor);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
+ Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_target);
- Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_t(_o_target);
Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
+ // WOLF_INFO("p_sensor: ", p_r_s.transpose());
+ // WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
+ // WOLF_INFO("p_ref: ", p_w_r.transpose());
+ // WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose());
+ // WOLF_INFO("p_target: ", p_w_t.transpose());
+ // WOLF_INFO("o_target: ", q_w_t.coeffs().transpose());
+
// Expected measurement
- Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate();
- Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l;
- Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l);
+ Eigen::Quaternion<T> expected_q;
+ Eigen::Matrix<T,3,1> expected_p;
+ // 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;
+ }
+ // 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;
+ }
// Measurement
- Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>());
+ Eigen::Vector3d p_meas(getMeasurement().head<3>());
Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 );
Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
//Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
// Error
Eigen::Matrix<T, 6, 1> err;
- err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l);
- //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l); // true error function for which the covariance should be computed
- err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
+ // err.head(3) = q_l_c_meas * (p_meas.cast<T>() - expected_p);
+ err.head(3) = (p_meas - expected_p);
+ err.tail(3) = wolf::log_q(q_l_c_meas * expected_q); // true error function for which the covariance should be computed
+ //err.tail(3) = T(2)*(q_l_c_meas * expected_q).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
@@ -192,15 +208,27 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
{
Eigen::Vector6d res;
- double * p_camera, * o_camera, * p_ref, * o_ref, * p_target, * o_target;
- p_camera = getCapture()->getSensorP()->getState().data();
- o_camera = getCapture()->getSensorO()->getState().data();
- p_ref = getCapture()->getFrame()->getP()->getState().data();
- o_ref = getCapture()->getFrame()->getO()->getState().data();
- p_target = getLandmarkOther()->getP()->getState().data();
- o_target = getLandmarkOther()->getO()->getState().data();
-
- operator() (p_camera, o_camera, p_ref, o_ref, p_target, o_target, res.data());
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
+ p_sensor = getCapture()->getSensorP()->getState();
+ o_sensor = getCapture()->getSensorO()->getState();
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ p_ref = getFrameOther()->getP()->getState();
+ o_ref = getFrameOther()->getO()->getState();
+ p_target = getCapture()->getFrame()->getP()->getState();
+ o_target = getCapture()->getFrame()->getO()->getState();
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ p_ref = getCapture()->getFrame()->getP()->getState();
+ o_ref = getCapture()->getFrame()->getO()->getState();
+ p_target = getLandmarkOther()->getP()->getState();
+ o_target = getLandmarkOther()->getO()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
diff --git a/include/core/factor/factor_relative_position_2d.h b/include/core/factor/factor_relative_position_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a2c7a0e52a7b58f8cda42586c56abff31793e79
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d.h
@@ -0,0 +1,258 @@
+//--------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--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_analytic.h"
+#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
+#include <Eigen/StdVector>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition2d);
+
+//class
+class FactorRelativePosition2d : public FactorAnalytic
+{
+ public:
+
+ /** \brief Constructor of category FAC_FRAME
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP())
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_FEATURE
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP() )
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_LANDMARK
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP())
+ {
+ //
+ }
+
+ ~FactorRelativePosition2d() override = default;
+
+ /** \brief Returns the factor residual size
+ **/
+ unsigned int getSize() const override
+ {
+ return 2;
+ }
+
+ /** \brief Returns the residual evaluated in the states provided
+ *
+ * Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
+ **/
+ Eigen::VectorXd evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+ /** \brief Returns the jacobians evaluated in the states provided
+ *
+ * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ *
+ **/
+ void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+};
+
+/// IMPLEMENTATION ///
+
+inline Eigen::VectorXd FactorRelativePosition2d::evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+ Eigen::VectorXd residual(2);
+ Eigen::VectorXd expected_measurement(2);
+ // Expected measurement
+ Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
+ expected_measurement = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
+ // Residual
+ residual = expected_measurement - getMeasurement();
+ residual = getMeasurementSquareRootInformationUpper() * residual;
+ return residual;
+}
+
+inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 3);
+ assert(_st_vector.size() == 3);
+ assert(_compute_jacobian.size() == 3);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
+ (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
+ }
+}
+
+inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 3);
+ assert(_st_vector.size() == 3);
+ assert(_compute_jacobian.size() == 3);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
+ (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
+ }
+}
+
+inline void FactorRelativePosition2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+{
+ assert(jacobians.size() == 3);
+
+ jacobians[0] = (Eigen::MatrixXd(2,2) <<
+ -cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
+
+ jacobians[1] = (Eigen::MatrixXd(2,1) <<
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ + (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
+ - (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0))).finished();
+
+ jacobians[2] = (Eigen::MatrixXd(2,2) <<
+ cos(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_2d_with_extrinsics.h b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd36867f4a93e08fe671f0491a07276ed00b6624
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
@@ -0,0 +1,128 @@
+//--------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--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition2dWithExtrinsics);
+
+//class
+class FactorRelativePosition2dWithExtrinsics : public FactorAutodiff<FactorRelativePosition2dWithExtrinsics, 2, 2, 1, 2, 2, 1>
+{
+ public:
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorRelativePosition2dWithExtrinsics() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const;
+};
+
+inline FactorRelativePosition2dWithExtrinsics::FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+}
+
+template<typename T>
+inline bool FactorRelativePosition2dWithExtrinsics::operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
+{
+ // MAPS
+ Eigen::Map<Eigen::Matrix<T,2,1> > res(_residuals);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
+ T o_ref = _o_ref[0];
+ T o_sensor = _o_sensor[0];
+
+ // Expected measurement
+ Eigen::Matrix<T, 2, 1> expected_measurement = Eigen::Rotation2D<T>(-o_sensor) *
+ (-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (p_target - p_ref));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper() * (expected_measurement - getMeasurement());
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXs J(3,6);
+// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
+// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_3d.h b/include/core/factor/factor_relative_position_3d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d76dcb3406059f37218d0ace894ec1b3f9bc474
--- /dev/null
+++ b/include/core/factor/factor_relative_position_3d.h
@@ -0,0 +1,154 @@
+//--------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--------
+
+#pragma once
+
+#include "core/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition3d);
+
+//class
+class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d,3,3,4,3>
+{
+ public:
+ FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
+ FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorRelativePosition3d() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _q_ref,
+ const T* const _p_target,
+ T* _residuals) const;
+
+ private:
+ template<typename T>
+ void printRes(const Eigen::Matrix<T, 3, 1>& r) const;
+};
+
+template<typename T>
+inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r) const
+{
+ std::cout << "Jet residual = " << std::endl;
+ std::cout << r(0) << std::endl;
+ std::cout << r(1) << std::endl;
+ std::cout << r(2) << std::endl;
+}
+
+template<>
+inline void FactorRelativePosition3d::printRes (const Eigen::Matrix<double,3,1> & r) const
+{
+ std::cout << "Scalar residual = " << std::endl;
+ std::cout << r.transpose() << std::endl;
+}
+
+inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ _frame_past_ptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO(), // past frame Q
+ _ftr_current_ptr->getFrame()->getP()) // current frame P
+{
+ MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
+inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ _lmk_ptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(), // frame P
+ _ftr_current_ptr->getFrame()->getO(), // frame Q
+ _lmk_ptr->getP()) // landmark P
+{
+ MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
+template<typename T>
+inline bool FactorRelativePosition3d::operator ()(const T* const _p_ref,
+ const T* const _q_ref,
+ const T* const _p_target,
+ T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref);
+ Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals);
+
+// std::cout << "p_ref: " << p_ref(0) << " "
+// << p_ref(1) << " "
+// << p_ref(2) << "\n";
+// std::cout << "q_ref: " << q_ref.coeffs()(0) << " "
+// << q_ref.coeffs()(1) << " "
+// << q_ref.coeffs()(2) << " "
+// << q_ref.coeffs()(3) << "\n";
+// std::cout << "p_target: " << p_target(0) << " "
+// << p_target(1) << " "
+// << p_target(2) << "\n";
+
+ residuals = getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref)));
+
+ return true;
+}
+
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_3d_with_extrinsics.h b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..f892ac25a4adcb6c31f43f76b4fc3022a62b56ae
--- /dev/null
+++ b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
@@ -0,0 +1,149 @@
+//--------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--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition3dWithExtrinsics);
+
+//class
+class FactorRelativePosition3dWithExtrinsics : public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4>
+{
+ public:
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorRelativePosition3dWithExtrinsics() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const;
+
+ Eigen::Vector3d residual() const;
+ double cost() const;
+};
+
+inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition3dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+{
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+}
+
+template<typename T>
+inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _p_ref,
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _p_sensor,
+ const T* const _o_sensor,
+ T* _residuals) const
+{
+ // Maps
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor);
+ Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
+ Eigen::Map<Eigen::Matrix<T,3,1>> residuals(_residuals);
+
+ // WOLF_INFO("p_sensor: ", p_r_s.transpose());
+ // WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose());
+ // WOLF_INFO("p_ref: ", p_w_r.transpose());
+ // WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose());
+ // WOLF_INFO("p_target: ", p_w_t.transpose());
+
+ // Expected measurement
+ Eigen::Matrix<T,3,1> expected_p;
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
+
+ // Residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (getMeasurement() - expected_p);
+
+ return true;
+}
+
+inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const
+{
+ Eigen::Vector3d res;
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target;
+ p_sensor = getCapture()->getSensorP()->getState();
+ o_sensor = getCapture()->getSensorO()->getState();
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ p_ref = getFrameOther()->getP()->getState();
+ o_ref = getFrameOther()->getO()->getState();
+ p_target = getCapture()->getFrame()->getP()->getState();
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ p_ref = getCapture()->getFrame()->getP()->getState();
+ o_ref = getCapture()->getFrame()->getO()->getState();
+ p_target = getLandmarkOther()->getP()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+ return res;
+}
+
+inline double FactorRelativePosition3dWithExtrinsics::cost() const
+{
+ return residual().squaredNorm();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index af09f86002c697ad026ad9942d218026808555c4..da7113fa583bd7838bda5ee9a96c610731814dcd 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -113,7 +113,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
-
+ LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
+ LandmarkBasePtr getLandmark(const unsigned int& _id);
+
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 95631a8a6fb82ac22595b64d3e1d5b4fe7849cb4..39f67c0358fc1f3c127adc1ec96844d005011588 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -320,6 +320,13 @@ inline void compose(const VectorComposite& _x1,
_c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
+inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
+{
+ VectorComposite c("PO", {2,1});
+ compose(x1, x2, c);
+ return c;
+}
+
inline void compose(const VectorComposite& _x1,
const VectorComposite& _x2,
VectorComposite& _c,
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 46500c6e225e5f2f07928c2656d950a1462749ad..d2de44d0b38a58d5313a8f07574755e4866260d6 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -62,6 +62,12 @@ struct PriorOptions
};
WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
+struct Reference
+{
+ StateBlockPtr p_ptr;
+ StateBlockPtr o_ptr;
+};
+
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
@@ -90,6 +96,8 @@ class Problem : public std::enable_shared_from_this<Problem>
VectorComposite transformation_;
mutable std::mutex mut_transform_;
+ Reference local_reference_;
+
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
@@ -105,6 +113,10 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
SizeEigen getDim() const;
const StateStructure& getFrameStructure() const;
+ StateBlockPtr getLocalReferenceP();
+ StateBlockConstPtr getLocalReferenceP() const;
+ StateBlockPtr getLocalReferenceO();
+ StateBlockConstPtr getLocalReferenceO() const;
protected:
void appendToStructure(const StateStructure& _structure);
@@ -453,6 +465,26 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
+inline StateBlockPtr Problem::getLocalReferenceP()
+{
+ return local_reference_.p_ptr;
+}
+
+inline StateBlockConstPtr Problem::getLocalReferenceP() const
+{
+ return local_reference_.p_ptr;
+}
+
+inline StateBlockPtr Problem::getLocalReferenceO()
+{
+ return local_reference_.o_ptr;
+}
+
+inline StateBlockConstPtr Problem::getLocalReferenceO() const
+{
+ return local_reference_.o_ptr;
+}
+
inline TreeManagerBaseConstPtr Problem::getTreeManager() const
{
return tree_manager_;
diff --git a/include/core/processor/processor_tracker_feature_landmark_external.h b/include/core/processor/processor_tracker_feature_landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..b16168db2d60f618401a09848de6fb368f711db3
--- /dev/null
+++ b/include/core/processor/processor_tracker_feature_landmark_external.h
@@ -0,0 +1,176 @@
+//--------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--------
+
+#pragma once
+
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker_feature.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
+
+struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
+{
+ double filter_quality_th; ///< min quality to consider the detection
+ double filter_dist_th; ///< for considering tracked detection: distance threshold to previous 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
+
+ ParamsProcessorTrackerFeatureLandmarkExternal() = default;
+ ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
+ const wolf::ParamsServer & _server):
+ ParamsProcessorTrackerFeature(_unique_name, _server)
+ {
+ filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter_quality_th");
+ filter_dist_th = _server.getParam<double> (prefix + _unique_name + "/filter_dist_th");
+ filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
+ time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureLandmarkExternal);
+
+//Class
+class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
+{
+ public:
+ ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tracker_feature);
+ ~ProcessorTrackerFeatureLandmarkExternal() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureLandmarkExternal, ParamsProcessorTrackerFeatureLandmarkExternal);
+
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
+ std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
+
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * \return the number of features tracked
+ */
+ unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) override;
+
+ /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
+ * \param _last_feature input feature in last capture tracked
+ * \param _incoming_feature input/output feature in incoming capture to be corrected
+ * \return false if the the process discards the correspondence with origin's feature
+ */
+ bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) override;
+
+ /** \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 Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
+ */
+ unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr) override;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * extract the detections and fill detections_incoming_ (FeaturePtrList)
+ */
+ void preProcess() override;
+
+ /** 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;
+};
+
+inline ProcessorTrackerFeatureLandmarkExternal::ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tfle) :
+ ProcessorTrackerFeature("ProcessorTrackerFeatureLandmarkExternal", "PO", 0, _params_tfle),
+ params_tfle_(_params_tfle)
+{
+ //
+}
+
+inline ProcessorTrackerFeatureLandmarkExternal::~ProcessorTrackerFeatureLandmarkExternal()
+{
+ //
+}
+
+inline bool ProcessorTrackerFeatureLandmarkExternal::correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature)
+{
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index bbccccf950aa02f499d215a21f5488a724330435..ab109ffdbf4302cfeb517be5e184ae3499d77fa2 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -88,7 +88,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* - createLandmark() : creates a Landmark using a new Feature <=== IMPLEMENT
* - findLandmarks() : find the new Landmarks again in \b incoming <=== IMPLEMENT
* - establishFactors() : which calls the pure virtual:
- * - createFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
+ * - emplaceFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
*
* Should you need extra functionality for your derived types, you can overload these two methods,
*
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index b89b9102ab64073b729744ec5d6c7d4d8098b143..9c46dd20c96535308e5f851e218641498787a631 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -19,16 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file utils_gtest.h
- * \brief Some utils for gtest
- * \author Jeremie Deray
- * Created on: 26/09/2016
- * Eigen macros extension by: Joan Sola on 26/04/2017
- */
-
-#ifndef WOLF_UTILS_GTEST_H
-#define WOLF_UTILS_GTEST_H
+#pragma once
#include <gtest/gtest.h>
@@ -144,24 +135,58 @@ TEST(Test, Foo)
}, \
C_expect, C_actual);
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 3 and rhs.size() == 3); \
+ Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 3 and rhs.size() == 3); \
+ Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-} // namespace testing
+#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
-#endif /* WOLF_UTILS_GTEST_H */
+} // namespace testing
\ No newline at end of file
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..154dcc83f488b39a91c69f0906a0f59e2fdd56d7
--- /dev/null
+++ b/src/capture/capture_landmarks_external.cpp
@@ -0,0 +1,53 @@
+//--------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/capture/capture_landmarks_external.h"
+
+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<double>& _qualities) :
+ CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+{
+ 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(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
+}
+
+CaptureLandmarksExternal::~CaptureLandmarksExternal()
+{
+ //
+}
+
+void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
+{
+ detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
+}
+
+} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea..0c002934af723fd242babeb22cabd4de15823e9f 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -65,21 +65,46 @@ void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
landmark_list_.remove(_landmark_ptr);
}
-void MapBase::load(const std::string& _map_file_dot_yaml)
+LandmarkBaseConstPtr MapBase::getLandmark(const unsigned int& _id) const
{
- YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBaseConstPtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
+
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
+
+ return (*lmk_it);
+}
+
+LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
+{
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBasePtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
- unsigned int nlandmarks = map["nlandmarks"].as<unsigned int>();
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
- assert(nlandmarks == map["landmarks"].size() && "Number of landmarks in map file does not match!");
+ return (*lmk_it);
+}
- for (unsigned int i = 0; i < nlandmarks; i++)
+void MapBase::load(const std::string& _map_file_dot_yaml)
+{
+ YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+
+ for (unsigned int i = 0; i < map["landmarks"].size(); i++)
{
YAML::Node lmk_node = map["landmarks"][i];
LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
lmk_ptr->link(shared_from_this());
}
-
}
void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
@@ -87,10 +112,8 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
YAML::Emitter emitter;
emitter << YAML::BeginMap;
- emitter << "map name" << _map_name;
- emitter << "date-time" << dateTimeNow(); // Get date and time for archiving purposes
-
- emitter << "nlandmarks" << getLandmarkList().size();
+ emitter << "map_name" << _map_name;
+ emitter << "date_time" << dateTimeNow(); // Get date and time for archiving purposes
emitter << "landmarks" << YAML::BeginSeq;
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index d55a2666cc00b3c8cdc53854ab43f7903f0b9d6e..c269ff41137f33cb6cfd14158b54228346782f28 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -200,11 +200,11 @@ void ProcessorTrackerFeature::establishFactors()
auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
- assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()");
+ assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) && "not emplaced factor returned by emplaceFactor()");
- WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ WOLF_DEBUG_COND(fac_ptr, "New factor: track: " , feature_in_last->trackId(),
+ " origin: " , feature_in_origin->id() ,
+ " from last: " , feature_in_last->id() );
}
}
diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50a1730c2d3bf08c7020022448913ca7c857cf3c
--- /dev/null
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -0,0 +1,399 @@
+//--------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/processor/processor_tracker_feature_landmark_external.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/feature/feature_base.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"
+#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/math/SE2.h"
+#include "core/math/SE3.h"
+
+using namespace Eigen;
+
+namespace wolf
+{
+
+void ProcessorTrackerFeatureLandmarkExternal::preProcess()
+{
+ assert(unmatched_detections_incoming_.empty());
+
+ auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
+ if (not cap_landmarks)
+ throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+
+ auto landmark_detections = cap_landmarks->getDetections();
+ std::set<int> ids;
+ for (auto detection : landmark_detections)
+ {
+ WOLF_WARN_COND(ids.count(detection.id), "ProcessorTrackerFeatureLandmarkExternal::preProcess: detection with repeated id, discarding...");
+
+ if (detection.quality < params_tfle_->filter_quality_th
+ or ids.count(detection.id))
+ continue;
+
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
+ "FeatureLandmarkExternal",
+ detection.measure,
+ detection.covariance);
+ ftr->setLandmarkId(detection.id);
+
+ if (detection.id != -1 and detection.id != 0)
+ ids.insert(detection.id);
+
+ unmatched_detections_incoming_.insert(ftr);
+ }
+}
+
+unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
+{
+ if (_capture != incoming_ptr_)
+ throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures should be called with incoming capture");
+ if (not _features_in.empty() and _features_in.front()->getCapture() != last_ptr_)
+ throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures should be called with features in not from last");
+
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+ auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+
+ // Track features given by ProcessorTrackerFeature
+ for (auto feat_in : _features_in)
+ {
+ for (auto feat_candidate : unmatched_detections_incoming_)
+ {
+ if (feat_candidate->landmarkId() == feat_in->landmarkId() and
+ detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
+ {
+ // add match
+ _features_out.push_back(feat_candidate);
+ _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+
+ // remove from unmatched
+ unmatched_detections_incoming_.erase(feat_candidate);
+ break;
+ }
+ }
+ }
+
+ // Track features in last not matched yet
+ auto feat_last_it = unmatched_detections_last_.begin();
+ while (feat_last_it != unmatched_detections_last_.end())
+ {
+ auto feat_in = *feat_last_it;
+ bool found = false;
+ for (auto feat_candidate : unmatched_detections_incoming_)
+ {
+ if (feat_candidate->landmarkId() == feat_in->landmarkId() and
+ detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
+ {
+ // add track
+ track_matrix_.newTrack(feat_in);
+
+ // add match
+ _features_out.push_back(feat_candidate);
+ _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+
+ // remove from unmatched
+ unmatched_detections_incoming_.erase(feat_candidate);
+ found = true;
+ break;
+ }
+ }
+ if (found)
+ feat_last_it = unmatched_detections_last_.erase(feat_last_it);
+ else
+ feat_last_it++;
+ }
+
+ return _features_out.size();
+}
+
+double ProcessorTrackerFeatureLandmarkExternal::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 (getProblem()->getDim() == 2)
+ return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
+ else
+ return (_ftr1->getMeasurement().head<3>() - _ftr2->getMeasurement().head<3>()).norm();
+ }
+ else
+ {
+ 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') + Eigen::Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ auto p2 = pose_s2.at('P') + Eigen::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') + Eigen::Quaterniond(Eigen::Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ auto p2 = pose_s2.at('P') + Eigen::Quaterniond(Eigen::Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+ return (p1-p2).norm();
+ }
+ }
+}
+
+bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
+{
+ auto matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
+
+ // no tracks longer than filter_track_length_th
+ auto n_tracks = 0;
+ for (auto match : matches_origin_last)
+ {
+ if (track_matrix_.track(match.first).size() >= params_tfle_->filter_track_length_th)
+ n_tracks++;
+ }
+ WOLF_INFO("Nbr. of active feature tracks: " , n_tracks );
+ if (n_tracks == 0)
+ return false;
+
+ bool vote = false;// = n_tracks < params_tracker_feature_->min_features_for_keyframe;
+
+ if (origin_ptr_)
+ {
+ WOLF_INFO("Time span since last frame: " ,
+ last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() ,
+ " (should be greater than ",
+ params_tfle_->time_span,
+ ")");
+ vote = vote or last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
+ }
+
+ WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
+
+ return vote;
+}
+
+unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+{
+ // NOT NECESSARY SINCE ALL FEATURES ARE TRACKED IN trackFeatures(...)
+ return 0;
+}
+
+FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr)
+{
+ assert(_feature_ptr);
+ assert(_feature_other_ptr);
+ assert(_feature_ptr->getCapture());
+ assert(_feature_ptr->getCapture()->getFrame());
+
+ assert(getProblem());
+ assert(getProblem()->getMap());
+
+ // Check track length
+ if (params_tfle_->filter_track_length_th > 1)
+ {
+ auto snapshot = track_matrix_.snapshot(_feature_ptr->getCapture());
+ const auto& it = std::find_if(snapshot.begin(), snapshot.end(),
+ [_feature_ptr](const std::pair<SizeStd, FeatureBasePtr>& pair)
+ {
+ return pair.second == _feature_ptr;
+ });
+ assert(it != snapshot.end());
+ if (track_matrix_.track(it->first).size() < params_tfle_->filter_track_length_th)
+ return nullptr;
+ }
+
+ // Get landmark
+ LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(_feature_ptr->landmarkId());
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 2)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
+ double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature_ptr->landmarkId());
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature_ptr->getMeasurement().size() == 3)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
+ double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
+ lmk->setId(_feature_ptr->landmarkId());
+ }
+ // no orientation, add it
+ else if (not lmk->getO())
+ {
+ double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0);
+
+ double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
+
+ lmk->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 3)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature_ptr->landmarkId());
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7)
+ {
+ // no landmark, create it
+ if (not lmk)
+ {
+ Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature_ptr->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>());
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
+ lmk->setId(_feature_ptr->landmarkId());
+ }
+ // no orientation, add it
+ else if (not lmk->getO())
+ {
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState()));
+
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
+
+ lmk->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
+ }
+
+ // emplace factor
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
+ _feature_ptr,
+ lmk,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ else
+ throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::emplaceFactor unknown case");
+
+ // not reachable
+ return nullptr;
+}
+
+void ProcessorTrackerFeatureLandmarkExternal::advanceDerived()
+{
+ ProcessorTrackerFeature::advanceDerived();
+
+ unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
+}
+void ProcessorTrackerFeatureLandmarkExternal::resetDerived()
+{
+ ProcessorTrackerFeature::resetDerived();
+
+ unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerFeatureLandmarkExternal)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerFeatureLandmarkExternal)
+} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6fa50395a7ee2af50d290b32e543b18bac534869..261438924ee686d0799f738278b5f1186630a48e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -173,6 +173,18 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
# FactorRelativePose3dWithExtrinsics class test
wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+# FactorRelativePosition2d class test
+wolf_add_gtest(gtest_factor_relative_position_2d gtest_factor_relative_position_2d.cpp)
+
+# FactorRelativePosition2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_position_2d_with_extrinsics gtest_factor_relative_position_2d_with_extrinsics.cpp)
+
+# FactorRelativePosition3d class test
+wolf_add_gtest(gtest_factor_relative_position_3d gtest_factor_relative_position_3d.cpp)
+
+# FactorRelativePosition3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_position_3d_with_extrinsics gtest_factor_relative_position_3d_with_extrinsics.cpp)
+
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
@@ -185,6 +197,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_SE3.cpp b/test/gtest_SE3.cpp
index a876103795f7afe0401c4a5db48290dd95c3775c..614d6f7f4cc04430d4a595209a94039bbbacd5cb 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -19,19 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_SE3.cpp
- *
- * Created on: Feb 2, 2019
- * \author: jsola
- */
-
#include "core/math/SE3.h"
#include "core/utils/utils_gtest.h"
-
-
using namespace Eigen;
using namespace wolf;
using namespace SE3;
@@ -150,7 +141,6 @@ TEST(SE3, inverseComposite)
VectorComposite pose_vc_out_bis = inverse(pose_vc);
ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8);
ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8);
-
}
TEST(SE3, composeBlocks)
@@ -194,7 +184,6 @@ TEST(SE3, composeEigenVectors)
compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
-
Vector7d x1; x1 << p1, q1.coeffs();
Vector7d x2; x2 << p2, q2.coeffs();
Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
@@ -287,7 +276,6 @@ TEST(SE3, exp_0_Composite)
ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
-
}
TEST(SE3, plus_0_Composite)
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index cc4030a9e51617399f2b256047f24eb30a93f5df..e317161887bc11dfbb4937b47e9ceb5b4c2e6c5e 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -25,6 +25,8 @@
#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace Eigen;
@@ -32,6 +34,12 @@ using namespace wolf;
using std::cout;
using std::endl;
+
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l;
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -43,93 +51,164 @@ SolverCeres solver(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, Vector3d::Zero());
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
+
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dPtr fac = nullptr;
+void generateRandomProblemFrame()
+{
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Frame 1 pose
+ x_1(2) = pi2pi(x_0(2) + delta(2));
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta.head<2>();
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
+}
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // landmark pose
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta.head<2>();
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
TEST(FactorRelativePose2d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorRelativePose2d, fix_0_solve)
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2d, frame_solve_frame1)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
-
- // x1 groundtruth
- Vector3d x1;
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
-
- // Set states and measurement
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
+ // random setup
+ generateRandomProblemFrame();
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-
- // Fix frm0, perturb frm1
+ // Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
- frm1->perturb(5);
+ frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2d, fix_1_solve)
+TEST(FactorRelativePose2d, frame_solve_frame0)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(1);
- // x1 groundtruth
- Vector3d x1;
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // Set states and measurement
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // Fix frm1, perturb frm0
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
frm1->fix();
- frm0->unfix();
- frm0->perturb(5);
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePose2d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index ee2bc076da1c48876250197c2fa1179ede489306..b5b4c9d0f36318408b0e056a7bdc3b17def589e7 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -25,8 +25,9 @@
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/capture/capture_odom_2d.h"
#include "core/sensor/sensor_odom_2d.h"
-#include "core/processor/processor_odom_2d.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace Eigen;
@@ -36,6 +37,11 @@ using std::endl;
std::string wolf_root = _WOLF_ROOT_DIR;
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l, x_s;
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -44,212 +50,307 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Sensor
-auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
-auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
+auto sensor = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
-// Capture from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
-TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // Frame 1 pose
+ x_1(2) = pi2pi(x_0(2) + delta(2));
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>()) - Rotation2Dd(x_1(2)) * x_s.head<2>();
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+void generateRandomProblemLandmark()
{
- for (int i = 0; i < 1e2; i++)
- {
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // landmark pose
+ x_l(2) = pi2pi(x_f(2) + x_s(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+////////////// TESTS /////////////////////////////////////////////////////////////////////
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->fix();
- frm1->perturb(5);
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame0)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm0->perturb(1);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+// JV: The position extrinsics in case of pair of frames is not observable
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Perturb frm0, fix the rest
+ // Perturb sensor P, fix the rest
frm1->fix();
- frm0->unfix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->fix();
- frm0->perturb(5);
+ frm0->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ //ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_o)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Perturb sensor P, fix the rest
+ // Perturb landmark, fix the rest
frm1->fix();
- frm0->fix();
- sensor_odom2d->getP()->unfix();
- sensor_odom2d->getO()->fix();
- sensor_odom2d->getP()->perturb(1);
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap1->setData(delta);
- sensor_odom2d->setState(xs);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb sensor O, fix the rest
frm1->fix();
- frm0->fix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->unfix();
- sensor_odom2d->getO()->perturb(.2);
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
- //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+ //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
// remove feature (and factor) for the next loop
- fea1->remove();
+ fea->remove();
}
}
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 9369c090efecad4b74435214230fca2b3b01f959..3b7f25c8fba43f0d56f5146fd2d5168e83733631 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -19,18 +19,14 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_factor_relative_pose_3d.cpp
- *
- * Created on: Mar 30, 2017
- * \author: jsola
- */
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_3d.h"
-#include "core/capture/capture_motion.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
using namespace Eigen;
@@ -38,72 +34,189 @@ using namespace wolf;
using std::cout;
using std::endl;
-Vector7d data2delta(Vector6d _data)
-{
- return (Vector7d() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
-}
+std::string wolf_root = _WOLF_ROOT_DIR;
-// Input odometry data and covariance
-Vector6d data(Vector6d::Random());
-Vector7d delta = data2delta(data);
-Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6d data_cov = data_var.asDiagonal();
+int N = 1e2;
-// perturbated priors
-Vector7d x0 = data2delta(Vector6d::Random()*0.25);
-Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l;
+
+// Input odometry data and covariance
+Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 3);
+
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
-
-// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
-FactorRelativePose3dPtr ctr1 = FactorBase::emplace<FactorRelativePose3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, nullptr, Vector7d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dPtr fac = nullptr;
////////////////////////////////////////////////////////
-TEST(FactorRelativePose3d, check_tree)
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // Random delta
+ delta = Vector7d::Random() * 1e2;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame 0 pose
+ x_0 = Vector7d::Random() * 1e2;
+ x_0.tail<4>().normalize();
+ auto q_0 = Quaterniond(x_0.tail<4>());
+
+ // Frame 1 pose
+ x_1.head<3>() = x_0.head<3>() + q_0 * delta.head<3>();
+ x_1.tail<4>() = (q_0 * q_delta).coeffs();
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose3d, expectation)
+void generateRandomProblemLandmark()
{
- ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
+ // Random delta
+ delta = Vector7d::Random() * 10;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Landmark pose
+ x_l.head<3>() = x_f.head<3>() + q_f * delta.head<3>();
+ x_l.tail<4>() = (q_f * q_delta).coeffs();
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
}
-TEST(FactorRelativePose3d, fix_0_solve)
+TEST(FactorRelativePose3d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3d, frame_solve_frame1)
{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
- // Fix frame 0, perturb frm1
- frm0->fix();
- frm1->unfix();
- frm1->setState(x1);
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(1);
- // solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST(FactorRelativePose3d, fix_1_solve)
+TEST(FactorRelativePose3d, frame_solve_frame0)
{
- // Fix frame 1, perturb frm0
- frm0->unfix();
- frm1->fix();
- frm0->setState(x0);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // solve for frm0
- std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6);
+TEST(FactorRelativePose3d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ frm1->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index f4755e11139f07ba42d537e0fc4e070c300cf9a3..2e99b1d2b765998d11c7eba96f7241e29977a7c4 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -19,308 +19,353 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
-#include <core/utils/utils_gtest.h>
-
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-
using namespace Eigen;
using namespace wolf;
-using std::static_pointer_cast;
-
-
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
-class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
- public:
- Vector3d pos_camera, pos_robot, pos_landmark;
- Vector3d euler_camera, euler_robot, euler_landmark;
- Quaterniond quat_camera, quat_robot, quat_landmark;
- Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
- Vector7d pose_camera, pose_robot, pose_landmark;
-
- ProblemPtr problem;
- SolverCeresPtr solver;
-
- SensorBasePtr S;
- FrameBasePtr F1;
- CapturePosePtr c1;
- FeaturePosePtr f1;
- LandmarkBasePtr lmk1;
- FactorRelativePose3dWithExtrinsicsPtr fac;
-
- Eigen::Matrix6d meas_cov;
-
- void SetUp() override
- {
- // configuration
-
- /* We have three poses to take into account:
- * - pose of the sensor (extrinsincs)
- * - pose of the landmark
- * - pose of the robot (Keyframe)
- *
- * The measurement provides the pose of the landmark wrt sensor's current pose in the world.
- * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics.
- *
- * The robot has a sensor looking forward
- * S: pos = (0,0,0), ori = (0, 0, 0)
- *
- * There is a point at the origin
- * P: pos = (0,0,0)
- *
- * Therefore, P projects exactly at the origin of the sensor,
- * f: p = (0,0)
- *
- * We form a Wolf tree with 1 frames F1, 1 capture C1,
- * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1:
- *
- * Frame F1, Capture C1, feature f1, landmark l1, constraint c1
- *
- * The frame pose F1, and the sensor pose S
- * in the robot frame are variables subject to optimization
- *
- * We perform a number of tests based on this configuration.*/
-
-
- // sensor is at origin of robot and looking forward
- // robot is at (0,0,0)
- // landmark is right in front of sensor. Its orientation wrt sensor is identity.
- pos_camera << 0,0,0;
- pos_robot << 0,0,0; //robot is at origin
- pos_landmark << 0,1,0;
- euler_camera << 0,0,0;
- //euler_camera << -M_PI_2, 0, -M_PI_2;
- euler_robot << 0,0,0;
- euler_landmark << 0,0,0;
- quat_camera = e2q(euler_camera);
- quat_robot = e2q(euler_robot);
- quat_landmark = e2q(euler_landmark);
- vquat_camera = quat_camera.coeffs();
- vquat_robot = quat_robot.coeffs();
- vquat_landmark = quat_landmark.coeffs();
- pose_camera << pos_camera, vquat_camera;
- pose_robot << pos_robot, vquat_robot;
- pose_landmark << pos_landmark, vquat_landmark;
-
- // Build problem
- problem = Problem::create("PO", 3);
- solver = std::make_shared<SolverCeres>(problem);
-
- // Create sensor to be able to initialize (a camera for instance)
- S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose",
- std::make_shared<StatePoint3d>(pos_camera, true),
- std::make_shared<StateQuaternion>(vquat_camera, true),
- std::make_shared<StateParams4>(Vector4d::Zero(), false), // fixed
- Vector1d::Zero());
-
- // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
- // S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera);
- // camera = std::static_pointer_cast<SensorCamera>(S);
-
-
- // F1 is be origin KF
- VectorComposite x0(pose_robot, "PO", {3,4});
- VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
- F1 = problem->setPriorFactor(x0, s0, 0.0);
-
-
- // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world)
- meas_cov = Eigen::Matrix6d::Identity();
- meas_cov.topLeftCorner(3,3) *= 1e-2;
- meas_cov.bottomRightCorner(3,3) *= 1e-3;
-
- //emplace feature and landmark
- c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
- f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
- lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
- std::make_shared<StatePoint3d>(pos_landmark),
- std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
- }
-};
-
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
-{
- auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
- lmk1,
- nullptr,
- false);
+using std::cout;
+using std::endl;
- ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
-}
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l, x_s;
+
+// Input odometry data and covariance
+Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
-/////////////////////////////////////////////
-// Tree not ok with this gtest implementation
-/////////////////////////////////////////////
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor, Vector7d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem->check(1));
-
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
- ASSERT_TRUE(problem->check(1));
+ // Random delta
+ delta = Vector7d::Random() * 1e2;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame 0 pose
+ x_0 = Vector7d::Random() * 1e2;
+ x_0.tail<4>().normalize();
+ auto q_0 = Quaterniond(x_0.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 1e2;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Frame 1 pose
+ auto q_1 = q_0 * q_s * q_delta * q_s.conjugate();
+ x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 * x_s.head<3>();
+ x_1.tail<4>() = q_1.coeffs();
+
+ // WOLF_INFO("x_0: ", x_0.transpose());
+ // WOLF_INFO("x_s: ", x_s.transpose());
+ // WOLF_INFO("delta: ", delta.transpose());
+ // WOLF_INFO("x_1: ", x_1.transpose());
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
+void generateRandomProblemLandmark()
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix F1, perturbate state
- F1->unfix();
- F1->getP()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ // Random delta
+ delta = Vector7d::Random() * 10;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 10;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Landmark pose
+ x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>());
+ x_l.tail<4>() = (q_f * q_s * q_delta).coeffs();
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, check_tree)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix F1, perturbate state
- F1->unfix();
- F1->getO()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
+ ASSERT_TRUE(problem_ptr->check(0));
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix lmk1, perturbate state
- lmk1->unfix();
- lmk1->getP()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm0->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
+// JV: The position extrinsics in case of pair of frames is not observable
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ //ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- // unfix F1, perturbate state
- lmk1->unfix();
- lmk1->getO()->perturb();
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
{
- // Change setup
- Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
- Quaterniond q_w_r, q_r_c, q_c_l, q_w_l;
- p_w_r << 1, 2, 3;
- p_r_c << 4, 5, 6;
- p_c_l << 7, 8, 9;
- q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize();
- q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize();
- q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize();
-
- q_w_l = q_w_r * q_r_c * q_c_l;
- p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l);
-
- // Change feature (remove and emplace)
- Vector7d meas;
- meas << p_c_l, q_c_l.coeffs();
- f1->remove();
- f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov));
-
- // emplace factor
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // Change Landmark
- lmk1->getP()->setState(p_w_l);
- lmk1->getO()->setState(q_w_l.coeffs());
- ASSERT_TRUE(lmk1->getP()->stateUpdated());
- ASSERT_TRUE(lmk1->getO()->stateUpdated());
-
- // Change Frame
- F1->getP()->setState(p_w_r);
- F1->getO()->setState(q_w_r.coeffs());
- F1->fix();
- ASSERT_TRUE(F1->getP()->stateUpdated());
- ASSERT_TRUE(F1->getO()->stateUpdated());
-
- // Change sensor extrinsics
- S->getP()->setState(p_r_c);
- S->getO()->setState(q_r_c.coeffs());
- ASSERT_TRUE(S->getP()->stateUpdated());
- ASSERT_TRUE(S->getO()->stateUpdated());
-
- Vector7d t_w_r, t_w_l;
- t_w_r << p_w_r, q_w_r.coeffs();
- t_w_l << p_w_l, q_w_l.coeffs();
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6);
-
- // unfix LMK, perturbate state
- lmk1->unfix();
- lmk1->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e91d0fb436646ea92401d64e5bde466e5989c6d9
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -0,0 +1,268 @@
+//--------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/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_2d.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1;//1e2
+
+// Vectors
+Vector3d x_0, x_f, x_1;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm1, 1, nullptr);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dPtr fac = nullptr;
+
+void generateRandomProblemFrame()
+{
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Frame 1 position
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta;
+
+ // Set states
+ frm0->setState(x_0);
+ frm1->setState(x_1);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureFramePosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, frm0, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // landmark position
+ x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta;
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition2d, frame_solve_frame1_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->getP()->unfix();
+ frm1->getO()->fix();
+ frm1->getP()->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_1, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->getP()->unfix();
+ frm0->getO()->fix();
+ frm0->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->getP()->fix();
+ frm0->getO()->unfix();
+ frm0->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x_0, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->getP()->unfix();
+ frm1->getO()->fix();
+ lmk->fix();
+ frm1->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->getP()->fix();
+ frm1->getO()->unfix();
+ lmk->fix();
+ frm1->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..87bb8f07080f83760be2fe1896904c8f64e2bd92
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -0,0 +1,239 @@
+//--------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/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1;//1e2
+
+// Vectors
+Vector3d x_f, x_s;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm, 1, sensor);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // landmark position
+ x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta);
+
+ // Set states
+ frm->setState(x_f);
+ lmk->setState(x_l);
+ sensor->setState(x_s);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk->getStateVector(), x_l, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->getP()->unfix();
+ frm->getO()->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->getP()->fix();
+ frm->getO()->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb sensor P, fix the rest
+ frm->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb sensor O, fix the rest
+ frm->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE2d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e928a49baf2f61b7c23eb663830a7a0d63c78a47
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -0,0 +1,280 @@
+//--------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/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d x_f;
+Vector3d delta1, delta2, delta3, x_1, x_2, x_3;
+
+// Input odometry data and covariance
+Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmarks
+LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+// Captures
+CaptureBasePtr cap1, cap2, cap3;
+// Features
+FeatureBasePtr fea1, fea2, fea3;
+// Factors
+FactorRelativePosition3dPtr fac1, fac2, fac3;
+
+////////////////////////////////////////////////////////
+void generateRandomProblemFrame()
+{
+ // Random delta
+ delta1 = Vector3d::Random() * 1e2;
+ delta2 = Vector3d::Random() * 1e2;
+ delta3 = Vector3d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 1e2;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Frames position
+ x_1 = x_f.head<3>() + q_f * delta1;
+ x_2 = x_f.head<3>() + q_f * delta2;
+ x_3 = x_f.head<3>() + q_f * delta3;
+
+ // Set states
+ frm->setState(x_f);
+ frm1->getP()->setState(x_1);
+ frm2->getP()->setState(x_2);
+ frm3->getP()->setState(x_3);
+
+ // capture & feature & factor with delta measurementCaptureBase(const std::string& _type,
+ cap1 = CaptureBase::emplace<CaptureBase>(frm1, "CaptureFramePosition3d", 1);
+ cap2 = CaptureBase::emplace<CaptureBase>(frm2, "CaptureFramePosition3d", 2);
+ cap3 = CaptureBase::emplace<CaptureBase>(frm3, "CaptureFramePosition3d", 3);
+ fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureFramePosition3d", delta1, data_cov);
+ fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureFramePosition3d", delta2, data_cov);
+ fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureFramePosition3d", delta3, data_cov);
+ fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, frm, nullptr, false, TOP_MOTION);
+ fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, frm, nullptr, false, TOP_MOTION);
+ fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, frm, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+ // Random delta
+ delta1 = Vector3d::Random() * 1e2;
+ delta2 = Vector3d::Random() * 1e2;
+ delta3 = Vector3d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 1e2;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Landmarks positions
+ x_1 = x_f.head<3>() + q_f * delta1;
+ x_2 = x_f.head<3>() + q_f * delta2;
+ x_3 = x_f.head<3>() + q_f * delta3;
+
+ // Set states
+ frm->setState(x_f);
+ lmk1->setState(x_1);
+ lmk2->setState(x_2);
+ lmk3->setState(x_3);
+
+ // feature & factor with delta measurement
+ cap1 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+ cap2 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+ cap3 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1);
+ fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPosition3d", delta1, data_cov);
+ fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureLandmarkPosition3d", delta2, data_cov);
+ fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureLandmarkPosition3d", delta3, data_cov);
+ fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, lmk1, nullptr, false, TOP_LMK);
+ fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, lmk2, nullptr, false, TOP_LMK);
+ fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, lmk3, nullptr, false, TOP_LMK);
+}
+
+TEST(FactorRelativePosition3d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition3d, frame_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm, fix the rest
+ frm->unfix();
+ frm1->fix();
+ frm2->fix();
+ frm3->fix();
+ frm->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3d, frame_solve_frames)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm->fix();
+ frm1->unfix();
+ frm2->unfix();
+ frm3->unfix();
+ frm1->getP()->perturb(1);
+ frm2->getP()->perturb(1);
+ frm3->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("P"), x_1, 1e-3);
+ ASSERT_MATRIX_APPROX(frm2->getStateVector("P"), x_2, 1e-3);
+ ASSERT_MATRIX_APPROX(frm3->getStateVector("P"), x_3, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition3d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->unfix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ frm->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3d, landmark_solve_lmks)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk1->unfix();
+ lmk2->unfix();
+ lmk3->unfix();
+ lmk1->perturb(1);
+ lmk2->perturb(1);
+ lmk3->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk1->getStateVector("P"), x_1, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk2->getStateVector("P"), x_2, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk3->getStateVector("P"), x_3, 1e-3);
+
+ // remove captures (and features and factors) for the next loop
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3917e5d85f021c0f950b18fa7ab4d191fb835379
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -0,0 +1,267 @@
+//--------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/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1e2;
+
+// Vectors
+Vector7d x_f, x_s;
+Vector3d delta1, delta2, delta3, x_l1, x_l2, x_l3;
+
+// Input odometry data and covariance
+Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmarks
+LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureOdom3d>(frm, 1, sensor, Vector3d::Zero(), data_cov);
+// Features
+FeatureBasePtr fea1, fea2, fea3;
+// Factors
+FactorRelativePosition3dWithExtrinsicsPtr fac1, fac2, fac3;
+
+void generateRandomProblemLandmark()
+{
+ // Random deltas
+ delta1 = Vector3d::Random() * 10;
+ delta2 = Vector3d::Random() * 10;
+ delta3 = Vector3d::Random() * 10;
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 10;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Landmarks poses
+ x_l1 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta1);
+ x_l2 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta2);
+ x_l3 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta3);
+
+ // Set states
+ frm->setState(x_f);
+ lmk1->setState(x_l1);
+ lmk2->setState(x_l2);
+ lmk3->setState(x_l3);
+ sensor->setState(x_s);
+
+ // features & factors with deltas measurements
+ fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta1, data_cov);
+ fea2 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta2, data_cov);
+ fea3 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta3, data_cov);
+ fac1 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1, fea1, lmk1, nullptr, false, TOP_LMK);
+ fac2 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2, fea2, lmk2, nullptr, false, TOP_LMK);
+ fac3 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3, fea3, lmk3, nullptr, false, TOP_LMK);
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_lmks)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk1->unfix();
+ lmk2->unfix();
+ lmk3->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk1->perturb(1);
+ lmk2->perturb(1);
+ lmk3->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_MATRIX_APPROX(lmk1->getStateVector(), x_l1, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk2->getStateVector(), x_l2, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk3->getStateVector(), x_l3, 1e-3);
+
+ // remove features (and factors) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm->unfix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm->fix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS);
+ ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm->fix();
+ lmk1->fix();
+ lmk2->fix();
+ lmk3->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 9ca7544ee6c9f28013f4037642de45504eca0b92..f0957a5abd90436d96d6e3a4e2788ad2d5c54faa 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -40,6 +40,9 @@
#include <iostream>
using namespace wolf;
+std::string wolf_root = _WOLF_ROOT_DIR;
+std::string map_path = wolf_root + "/test/yaml/maps";
+
TEST(MapYaml, save_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
@@ -62,9 +65,6 @@ TEST(MapYaml, save_2d)
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, o2_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, o3_sb);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
// Check Problem functions
std::string filename = map_path + "/map_2d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
@@ -80,89 +80,54 @@ TEST(MapYaml, load_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
- // Check Problem functions
- std::string filename = map_path + "/map_2d_problem.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->loadMap(filename);
-
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+ std::vector<std::string> maps{"map_2d_map", "map_2d_problem", "map_2d_manual"};
- for (auto lmk : problem->getMap()->getLandmarkList())
+ for (auto map : maps)
{
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- }
- else if (lmk->id() == 2)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- }
- }
-
- // empty the map
- {
- auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list)
- lmk->remove();
- }
- ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+ std::string filename = map_path + "/" + map + ".yaml";
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->loadMap(filename);
- // Check Map functions
- filename = map_path + "/map_2d_map.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->getMap()->load(filename);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
+ for (auto lmk : problem->getMap()->getLandmarkList())
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ }
}
- else if (lmk->id() == 2)
+ // empty the map
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list)
+ lmk->remove();
}
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
}
}
+
TEST(MapYaml, save_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
@@ -185,9 +150,6 @@ TEST(MapYaml, save_3d)
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p2_sb, q2_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", p3_sb, q3_sb);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
// Check Problem functions
std::string filename = map_path + "/map_3d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
@@ -203,99 +165,60 @@ TEST(MapYaml, load_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
- // Check Problem functions
- std::string filename = map_path + "/map_3d_problem.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->loadMap(filename);
-
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
-
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- }
- else if (lmk->id() == 2)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
- }
- else if (lmk->id() == 3)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
- }
- }
+ std::vector<std::string> maps{"map_3d_map", "map_3d_problem", "map_3d_manual"};
- // empty the map
+ for (auto map : maps)
{
- auto lmk_list = problem->getMap()->getLandmarkList();
- for (auto lmk : lmk_list)
- lmk->remove();
- }
- ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
+ std::string filename = map_path + "/" + map + ".yaml";
- // Check Map functions
- filename = map_path + "/map_3d_map.yaml";
- std::cout << "Loading map to file: " << filename << std::endl;
- problem->getMap()->load(filename);
+ std::cout << "Loading map to file: " << filename << std::endl;
+ problem->loadMap(filename);
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
- for (auto lmk : problem->getMap()->getLandmarkList())
- {
- if (lmk->id() == 1)
- {
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- }
- else if (lmk->id() == 2)
+ for (auto lmk : problem->getMap()->getLandmarkList())
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
- ASSERT_FALSE(lmk->getP()->isFixed());
- ASSERT_FALSE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ if (lmk->id() == 1)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() == nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ }
+ else if (lmk->id() == 2)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
+ ASSERT_FALSE(lmk->getP()->isFixed());
+ ASSERT_FALSE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
+ else if (lmk->id() == 3)
+ {
+ ASSERT_TRUE(lmk->getP() != nullptr);
+ ASSERT_TRUE(lmk->getO() != nullptr);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
+ ASSERT_TRUE(lmk->getP()->isFixed());
+ ASSERT_TRUE(lmk->getO()->isFixed());
+ ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ }
}
- else if (lmk->id() == 3)
+
+ // empty the map
{
- ASSERT_TRUE(lmk->getP() != nullptr);
- ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
- ASSERT_TRUE(lmk->getP()->isFixed());
- ASSERT_TRUE(lmk->getO()->isFixed());
- ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
+ auto lmk_list = problem->getMap()->getLandmarkList();
+ for (auto lmk : lmk_list)
+ lmk->remove();
}
+ ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
}
}
TEST(MapYaml, remove_map_files)
{
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string map_path = wolf_root + "/test/yaml";
-
std::string filename = map_path + "/map_2d_problem.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
filename = map_path + "/map_2d_map.yaml";
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..a82e385f35739d3065749cdfc67b43b07c71589d
--- /dev/null
+++ b/test/gtest_processor_tracker_feature_landmark_external.cpp
@@ -0,0 +1,524 @@
+//--------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/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/processor/processor_odom_3d.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/ceres_wrapper/solver_ceres.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;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor, sensor_odom;
+ ProcessorTrackerFeatureLandmarkExternalPtr 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;
+};
+
+void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
+ bool _orientation,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _add_landmarks)
+{
+ dim = _dim;
+ orientation = _orientation;
+ t = TimeStamp(0);
+ dt = 0.1;
+
+ problem = Problem::create("PO", dim);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // Sensors
+ 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);
+ sensor_odom = SensorBase::emplace<SensorOdom2d>(problem->getHardware(),
+ Vector3d::Zero(),
+ ParamsSensorOdom2d());
+
+ }
+ else
+ {
+ 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());
+ }
+
+ // Processors
+ 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;
+ params->filter_quality_th = _quality_th;
+ params->filter_dist_th = _dist_th;
+ params->filter_track_length_th = _track_length_th;
+ params->time_span = _time_span;
+ processor = ProcessorBase::emplace<ProcessorTrackerFeatureLandmarkExternal>(sensor, params);
+
+ if (dim == 2)
+ {
+ 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));
+ }
+ else
+ {
+ 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));
+ }
+
+ // Emplace frame
+ 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);
+
+ // Emplace 3 random landmarks
+ for (auto i = 0; i < 3; i++)
+ {
+ 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);
+ landmarks.push_back(lmk);
+ state_landmarks.push_back(lmk->getState());
+ }
+
+ // Store groundtruth poses
+ state_robot = F0->getState("PO");
+ state_sensor = sensor->getState("PO");
+}
+
+void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
+{
+ // compute delta
+ VectorXd delta;
+ MatrixXd delta_cov;
+ if (dim == 2)
+ {
+ delta = Vector2d::Random() * 0.1;
+ delta_cov = Matrix2d::Identity() * 0.1;
+ }
+ else
+ {
+ delta = Vector7d::Random() * 0.1;
+ delta.tail<4>().normalize();
+ delta_cov = Matrix6d::Identity() * 0.1;
+ }
+
+ // Process odometry
+ auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, delta_cov, nullptr);
+ cap_odom->process();
+
+ // update groundtruth pose with random movement
+ state_robot = processor_motion->getState("PO");
+}
+
+CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
+{
+ // Detections
+ auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
+ for (auto i = 0; i < landmarks.size(); i++)
+ {
+ // compute detection
+ 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'));
+ if (orientation)
+ meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
+ }
+ else
+ {
+ 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'));
+ if (orientation)
+ 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);
+
+ // quality
+ double quality = (double) i / (double) (landmarks.size()-1); // increasing from 0 to 1
+
+ // add detection
+ cap->addDetection(landmarks.at(i)->id(), meas, cov, quality );
+ }
+
+ return cap;
+}
+
+void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
+ bool orientation,
+ double quality_th,
+ double dist_th,
+ int track_length,
+ double time_span,
+ bool add_landmarks)
+{
+ initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);
+
+ ASSERT_TRUE(problem->check());
+
+ for (auto i = 0; i<10; i++)
+ {
+ WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
+
+ // detection of landmarks
+ auto cap = computeCaptureLandmarks();
+ ASSERT_TRUE(problem->check());
+
+ // process detections
+ cap->process();
+ ASSERT_TRUE(problem->check());
+ 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);
+
+ if (should_emplace_KF)
+ {
+ // voted for keyframe
+ ASSERT_TRUE(problem->getTrajectory()->size() > 1);
+
+ // emplaced factors
+ ASSERT_FALSE(fac_list.empty());
+
+ // factors' type
+ if (should_emplace_KF)
+ for (auto fac : fac_list)
+ {
+ if (fac->getProcessor() == processor)
+ {
+ ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
+ (orientation ? "Pose" : "Position") +
+ (dim == 2 ? "2d" : "3d") +
+ "WithExtrinsics");
+ }
+ }
+ // landmarks created by processor
+ if (not add_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)));
+ 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());
+ 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);
+ }
+
+ // step with random movement
+ t+=dt;
+ randomStep();
+
+ // solve
+ if (should_emplace_KF)
+ {
+ // perturb landmarks
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ lmk->perturb();
+
+ // fix frame and extrinsics
+ sensor->fix();
+ problem->getLastFrame()->fix();
+
+ // solve
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ // check values
+ //assertVectorComposite(sensor->getState("PO"), state_sensor);
+ //assertVectorComposite(problem->getState("PO"), state_robot);
+ for (auto lmk_map : problem->getMap()->getLandmarkList())
+ {
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ }
+ }
+ }
+}
+
+void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
+{
+ if (v1.includesStructure("PO") and v2.includesStructure("PO"))
+ {
+ if (dim == 2)
+ {
+ ASSERT_POSE2d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ else
+ {
+ ASSERT_POSE3d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ }
+ else if (v1.includesStructure("P") and v2.includesStructure("P"))
+ {
+ ASSERT_MATRIX_APPROX(v1.vector("P"), v2.vector("P"), Constants::EPS);
+ }
+ else
+ throw std::runtime_error("wrong vector composite");
+}
+
+// / TESTS //////////////////////////////////////////
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
+{
+ 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
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
+{
+ 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
+}
+
+TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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(ProcessorTrackerFeatureLandmarkExternalTest, 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)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/yaml/maps/map_2d_manual.yaml b/test/yaml/maps/map_2d_manual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4a2276ad153b4adfbae3679d42b1aba801f15bb3
--- /dev/null
+++ b/test/yaml/maps/map_2d_manual.yaml
@@ -0,0 +1,13 @@
+map_name: 2d map saved from Map::save()
+date_time: 21/09/2022 at 12:00:59
+useless_parameter: [1, 6, 3]
+landmarks:
+ -
+ id: 1
+ type: LandmarkBase
+ position: [1, 2]
+ position fixed: false
+ transformable: true
+ another_useless_param: "wtf"
+ - {id: 2, type: LandmarkBase, position: [3, 4], position fixed: false, orientation: [2], orientation fixed: false, transformable: true}
+ - {id: 3, type: LandmarkBase, position: [5, 6], position fixed: true, orientation: [3], orientation fixed: true, transformable: true}
\ No newline at end of file
diff --git a/test/yaml/maps/map_3d_manual.yaml b/test/yaml/maps/map_3d_manual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4b66ed90f51224d03b70cf484ac333da74e777f4
--- /dev/null
+++ b/test/yaml/maps/map_3d_manual.yaml
@@ -0,0 +1,25 @@
+#map_name: 3d map saved from Problem::saveMap()
+#date_time: 21/09/2022 at 12:00:59
+landmarks:
+ -
+ id: 10
+ type: LandmarkBase
+ position: [1, 2, 3]
+ position fixed: false
+ transformable: true
+ -
+ id: 11
+ type: LandmarkBase
+ position: [4, 5, 6]
+ position fixed: false
+ orientation: [0, 1, 0, 0]
+ orientation fixed: false
+ transformable: true
+ -
+ id: 12
+ type: LandmarkBase
+ position: [7, 8, 9]
+ position fixed: true
+ orientation: [0, 0, 1, 0]
+ orientation fixed: true
+ transformable: true
\ No newline at end of file