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..fb89f046ed7d967cc428bb8355dc4cf760da40b8 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -132,31 +132,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;
+ }
// Error
er = expected_measurement - getMeasurement().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..a72d9683a8f9818e6dc242a67e2770a29fa1692e 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -150,38 +150,57 @@ 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_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t * q_r_s;
+ expected_p = q_r_s.conjugate() * q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - (p_w_r + q_w_r * p_r_s));
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t;
+ expected_p = q_r_s.conjugate() * q_w_r.conjugate() * (p_w_t - (p_w_r + q_w_r * p_r_s));
+ }
// Measurement
- 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 +211,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_camera, o_camera, p_ref, o_ref, p_target, o_target;
+ p_camera = getCapture()->getSensorP()->getState();
+ o_camera = 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_camera.data(), o_camera.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..8c52033c6add806a4d809857477063558e85317f
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d.h
@@ -0,0 +1,260 @@
+//--------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
+
+#endif
diff --git a/include/core/factor/factor_relative_position_2d_with_extrinsics copy.h b/include/core/factor/factor_relative_position_2d_with_extrinsics copy.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e62f6e0079852d738930493b23112c9ded14df5
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d_with_extrinsics copy.h
@@ -0,0 +1,130 @@
+//--------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
+
+#endif
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/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index b89b9102ab64073b729744ec5d6c7d4d8098b143..905f25c8b26a3fa1caa79cb6e72be4f6d07cfcc1 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::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
+ return lhs.angularDistance(rhs) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
+ return lhs.angularDistance(rhs) < precision; \
+ }, \
+ C_expect, C_actual);
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#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 EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#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/map/map_base.cpp b/src/map/map_base.cpp
index 66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea..b629aba0805d22efc6b23b72e0057ffcc6a30643 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -69,17 +69,12 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
{
YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
- unsigned int nlandmarks = map["nlandmarks"].as<unsigned int>();
-
- assert(nlandmarks == map["landmarks"].size() && "Number of landmarks in map file does not match!");
-
- for (unsigned int i = 0; i < nlandmarks; i++)
+ 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 +82,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/test/CMakeLists.txt b/test/CMakeLists.txt
index 6fa50395a7ee2af50d290b32e543b18bac534869..812ee1f8e4233825bb78aae2029cbd349c34afc1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -172,6 +172,7 @@ 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)
+wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics_new gtest_factor_relative_pose_3d_with_extrinsics_new.cpp)
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index ee2bc076da1c48876250197c2fa1179ede489306..76744a478fddc50146f3bea9b3598803cad93b46 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,8 @@ using std::endl;
std::string wolf_root = _WOLF_ROOT_DIR;
+int N = 1;//1e2
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -45,13 +48,18 @@ 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>());
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, Vector3d::Zero() );
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, Vector3d::Zero() );
-// Capture from frm1 to frm0
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
+
+// Capture
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
TEST(FactorRelativePose2dWithExtrinsics, check_tree)
@@ -59,9 +67,10 @@ TEST(FactorRelativePose2dWithExtrinsics, check_tree)
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
// Random delta
Vector3d delta = Vector3d::Random() * 10;
@@ -88,7 +97,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
// 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
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
// Perturb frm1, fix the rest
frm0->fix();
@@ -107,9 +116,9 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
}
}
-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;
@@ -136,7 +145,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
// 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
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
// Perturb frm0, fix the rest
frm1->fix();
@@ -155,9 +164,9 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
// Random delta
Vector3d delta = Vector3d::Random() * 10;
@@ -184,7 +193,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
// 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
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
// Perturb sensor P, fix the rest
frm1->fix();
@@ -203,9 +212,9 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_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;
@@ -232,7 +241,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
// 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
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
// Perturb sensor O, fix the rest
frm1->fix();
@@ -253,6 +262,201 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
}
}
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random frame pose
+ Vector3d x_f = Vector3d::Random() * 10;
+ pi2pi(x_f(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // landmark pose
+ Vector3d x_l;
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ lmk->perturb(5);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(lmk->getStateVector(), x_l, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random frame pose
+ Vector3d x_f = Vector3d::Random() * 10;
+ pi2pi(x_f(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // landmark pose
+ Vector3d x_l;
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm1->perturb(5);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x_f, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random frame pose
+ Vector3d x_f = Vector3d::Random() * 10;
+ pi2pi(x_f(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // landmark pose
+ Vector3d x_l;
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor_odom2d->getP()->unfix();
+ sensor_odom2d->getO()->fix();
+ sensor_odom2d->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random frame pose
+ Vector3d x_f = Vector3d::Random() * 10;
+ pi2pi(x_f(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // landmark pose
+ Vector3d x_l;
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f);
+ lmk->setState(x_l);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, lmk, nullptr, false, TOP_LMK); // create and add
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->unfix();
+ sensor_odom2d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b83aa4a8d014415fbd80405c47197e6eddaaba5
--- /dev/null
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics_new.cpp
@@ -0,0 +1,377 @@
+ //--------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_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"
+
+
+ using namespace Eigen;
+ using namespace wolf;
+ using std::cout;
+ using std::endl;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ int N = 1;//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_odom3d = 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()));
+
+ // Capture
+ auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor_odom3d, Vector7d::Zero(), data_cov);
+ // Feature
+ FeatureBasePtr fea = nullptr;
+ // Factor
+ FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
+
+ void generateRandomProblemFrame()
+ {
+ // 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>());
+
+ // Random 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_odom3d->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
+ }
+
+ void generateRandomProblemLandmark()
+ {
+ // 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_odom3d->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(FactorRelativePose3dWithExtrinsics, check_tree)
+ {
+ ASSERT_TRUE(problem_ptr->check(0));
+ }
+
+ // FRAME TO FRAME =========================================================================
+ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
+ {
+ 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_odom3d->getP()->fix();
+ sensor_odom3d->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(frm1->getStateVector(), x_1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+ }
+
+ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
+ {
+ 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_odom3d->getP()->fix();
+ sensor_odom3d->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-6);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+ }
+
+ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
+ {
+ 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_odom3d->getP()->unfix();
+ sensor_odom3d->getO()->fix();
+ WOLF_INFO("sensor P before perturbing: ", sensor_odom3d->getP()->getState().transpose());
+ sensor_odom3d->getP()->perturb(1);
+ WOLF_INFO("sensor P after perturbing: ", sensor_odom3d->getP()->getState().transpose());
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("sensor P after solving: ", sensor_odom3d->getP()->getState().transpose());
+ WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+ }
+
+ 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_odom3d->getP()->fix();
+ sensor_odom3d->getO()->unfix();
+ sensor_odom3d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+ }
+
+ // 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_odom3d->getP()->fix();
+ sensor_odom3d->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-6);
+
+ // 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_odom3d->getP()->fix();
+ sensor_odom3d->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-6);
+
+ // remove feature (and factor) for the next loop
+ fea->remove();
+ }
+ }
+
+ TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
+ {
+ 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_odom3d->getP()->unfix();
+ sensor_odom3d->getO()->fix();
+ sensor_odom3d->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
+
+ // 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_odom3d->getP()->fix();
+ sensor_odom3d->getO()->unfix();
+ sensor_odom3d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom3d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ ASSERT_POSE3d_APPROX(sensor_odom3d->getStateVector(), x_s, 1e-6);
+
+ // 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_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/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