diff --git a/include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h b/include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h
deleted file mode 100644
index 6f5d3c1bf252e4c801506f18c962f0bcae3e6e7f..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h
+++ /dev/null
@@ -1,163 +0,0 @@
-#ifndef _FACTOR_APRILTAG_H_
-#define _FACTOR_APRILTAG_H_
-
-//Wolf includes
-#include "core/common/wolf.h"
-#include "core/math/rotations.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/sensor/sensor_base.h"
-#include "core/landmark/landmark_base.h"
-#include "core/feature/feature_base.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(FactorKfLmkPose3dWithExtrinsics);
-
-class FactorKfLmkPose3dWithExtrinsics : public FactorAutodiff<FactorKfLmkPose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4>
-{
- public:
-
- /** \brief Class constructor
- */
- FactorKfLmkPose3dWithExtrinsics(
- const SensorBasePtr& _sensor_ptr,
- const FrameBasePtr& _frame_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const FeatureBasePtr& _feature_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status);
-
- /** \brief Class Destructor
- */
- ~FactorKfLmkPose3dWithExtrinsics() override;
-
- /** brief : compute the residual from the state blocks being iterated by the solver.
- **/
- template<typename T>
- bool operator ()( const T* const _p_camera,
- const T* const _o_camera,
- const T* const _p_keyframe,
- const T* const _o_keyframe,
- const T* const _p_landmark,
- const T* const _o_landmark,
- T* _residuals) const;
-
- Eigen::Vector6d residual() const;
- double cost() const;
-
- // print function only for double (not Jet)
- template<typename T, int Rows, int Cols>
- void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const
- {
- // jet prints nothing
- }
- template<int Rows, int Cols>
- void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const
- {
- // double prints stuff
- WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
- }
-};
-
-} // namespace wolf
-
-// Include here all headers for this class
-//#include <YOUR_HEADERS.h>
-
-namespace wolf
-{
-
-FactorKfLmkPose3dWithExtrinsics::FactorKfLmkPose3dWithExtrinsics(
- const SensorBasePtr& _sensor_ptr,
- const FrameBasePtr& _frame_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const FeatureBasePtr& _feature_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
- TOP_LMK,
- _feature_ptr,
- nullptr,
- nullptr,
- nullptr,
- _landmark_other_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _sensor_ptr->getP(), _sensor_ptr->getO(),
- _frame_ptr->getP(), _frame_ptr->getO(),
- _landmark_other_ptr->getP(), _landmark_other_ptr->getO()
- )
-{
-
-
-}
-
-/** \brief Class Destructor
- */
-FactorKfLmkPose3dWithExtrinsics::~FactorKfLmkPose3dWithExtrinsics()
-{
- //
-}
-
-template<typename T> bool FactorKfLmkPose3dWithExtrinsics::operator ()( const T* const _p_camera, const T* const _o_camera, const T* const _p_keyframe, const T* const _o_keyframe, const T* const _p_landmark, const T* const _o_landmark, T* _residuals) const
-{
- // Maps
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_camera);
- Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_camera);
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_keyframe);
- Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_keyframe);
- Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_landmark);
- Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_landmark);
- Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
-
- // 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);
-
- // Measurement
- Eigen::Vector3d p_c_l_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 )
-
- // Residual
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
-
- return true;
-}
-
-Eigen::Vector6d FactorKfLmkPose3dWithExtrinsics::residual() const
-{
- Eigen::Vector6d res;
- double * p_camera, * o_camera, * p_frame, * o_frame, * p_tag, * o_tag;
- p_camera = getCapture()->getSensorP()->getState().data();
- o_camera = getCapture()->getSensorO()->getState().data();
- p_frame = getCapture()->getFrame()->getP()->getState().data();
- o_frame = getCapture()->getFrame()->getO()->getState().data();
- p_tag = getLandmarkOther()->getP()->getState().data();
- o_tag = getLandmarkOther()->getO()->getState().data();
-
- operator() (p_camera, o_camera, p_frame, o_frame, p_tag, o_tag, res.data());
-
- return res;
-}
-
-double FactorKfLmkPose3dWithExtrinsics::cost() const
-{
- return residual().squaredNorm();
-}
-
-} // namespace wolf
-
-#endif /* _CONSTRAINT_AUTODIFF_APRILTAG_H_ */
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 1d7e85804d2e1e94eeca7623f382e0ef19f05dbd..54a6acbc8f00b24217168344b4b95302798607b3 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -16,61 +16,104 @@ WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
{
public:
+ /** \brief Class constructor Frame-Frame
+ */
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
- _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(),
- _ftr_ptr->getFrame()->getO(),
- _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!");
- //
- }
+ FactorStatus _status = FAC_ACTIVE);
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePose2dWithExtrinsics(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);
~FactorRelativePose2dWithExtrinsics() override = default;
template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
- const T* const _o2,
- const T* const _sp,
- const T* const _so,
+ bool 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;
};
+FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _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(),
+ _ftr_ptr->getFrame()->getO(),
+ _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!");
+ //
+}
+
+FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ _lmk_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _lmk_other_ptr->getO(),
+ _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 FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _p2,
- const T* const _o2,
- const T* const _ps,
- const T* const _os,
+inline bool FactorRelativePose2dWithExtrinsics::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
{
// MAPS
Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
- Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
- Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
- Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
- T o1 = _o1[0];
- T o2 = _o2[0];
- T os = _os[0];
+ 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];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
@@ -91,8 +134,8 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
// 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>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
- expected_measurement(2) = o2 - o1;
+ 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;
// 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
new file mode 100644
index 0000000000000000000000000000000000000000..dec430a9547d8c15edd32f06f7ef38f7c86aa89b
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -0,0 +1,194 @@
+#ifndef _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
+#define _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/math/rotations.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/sensor/sensor_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose3dWithExtrinsics);
+
+class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativePose3dWithExtrinsics, 6, 3, 4, 3, 4, 3, 4>
+{
+ public:
+
+ /** \brief Class constructor Frame-Frame
+ */
+ FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top = TOP_LMK,
+ FactorStatus _status = FAC_ACTIVE);
+
+ /** \brief Class Destructor
+ */
+ ~FactorRelativePose3dWithExtrinsics() override;
+
+ /** brief : compute the residual from the state blocks being iterated by the solver.
+ **/
+ template<typename T>
+ bool 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;
+
+ Eigen::Vector6d residual() const;
+ double cost() const;
+
+ // print function only for double (not Jet)
+ template<typename T, int Rows, int Cols>
+ void print(int kf, int lmk, const std::string s, const Eigen::Matrix<T, Rows, Cols> _M) const
+ {
+ // jet prints nothing
+ }
+ template<int Rows, int Cols>
+ void print(int kf, int lmk, const std::string s, const Eigen::Matrix<double, Rows, Cols> _M) const
+ {
+ // double prints stuff
+ WOLF_TRACE("KF", kf, " L", lmk, "; ", s, _M);
+ }
+};
+
+} // namespace wolf
+
+namespace wolf
+{
+
+FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
+ _top,
+ _feature_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _feature_ptr->getFrame()->getP(),
+ _feature_ptr->getFrame()->getO(),
+ _feature_ptr->getCapture()->getSensorP(),
+ _feature_ptr->getCapture()->getSensorO())
+{
+}
+
+FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
+ _top,
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feature_ptr->getFrame()->getP(),
+ _feature_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP(),
+ _landmark_other_ptr->getO(),
+ _feature_ptr->getCapture()->getSensorP(),
+ _feature_ptr->getCapture()->getSensorO())
+{
+}
+
+FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
+{
+ //
+}
+
+template<typename T>
+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
+{
+ // 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_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<Eigen::Matrix<T,6,1>> residuals(_residuals);
+
+ // 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);
+
+ // Measurement
+ Eigen::Vector3d p_c_l_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 )
+
+ // Residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+ return true;
+}
+
+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());
+
+ return res;
+}
+
+double FactorRelativePose3dWithExtrinsics::cost() const
+{
+ return residual().squaredNorm();
+}
+
+} // namespace wolf
+
+#endif /* _FACTOR_TELATIVE_POSE_3D_WITH_EXTRINSICS */
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index df8050d2ad6e8dd9da6d0bab197cc762d29543be..4988d2a44860219f3cbf87d7331a01deaa057a0d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -213,9 +213,9 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM
wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
-# FactorRelativePose3d class test
-wolf_add_gtest(gtest_factor_kf_lmk_pose_3d_with_extrinsics gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_kf_lmk_pose_3d_with_extrinsics ${PLUGIN_NAME})
+# FactorRelativePose3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${PLUGIN_NAME})
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
diff --git a/test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
similarity index 65%
rename from test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp
rename to test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 481eae173729b619154b9710dd0287af6ae6325b..84a4e373be68d3d1b6fe35f2f4f3e4d908564e4f 100644
--- a/test/gtest_factor_kf_lmk_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -1,3 +1,4 @@
+#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
#include <core/utils/utils_gtest.h>
#include "core/common/wolf.h"
@@ -7,7 +8,6 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_pose.h"
#include "core/feature/feature_pose.h"
-#include "core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h"
@@ -18,7 +18,7 @@ using std::static_pointer_cast;
// Use the following in case you want to initialize tests with predefines variables or methods.
-class FactorKfLmkPose3dWithExtrinsics_class : public testing::Test{
+class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
public:
Vector3d pos_camera, pos_robot, pos_landmark;
Vector3d euler_camera, euler_robot, euler_landmark;
@@ -34,7 +34,7 @@ class FactorKfLmkPose3dWithExtrinsics_class : public testing::Test{
CapturePosePtr c1;
FeaturePosePtr f1;
LandmarkBasePtr lmk1;
- FactorKfLmkPose3dWithExtrinsicsPtr fac;
+ FactorRelativePose3dWithExtrinsicsPtr fac;
Eigen::Matrix6d meas_cov;
@@ -127,47 +127,36 @@ class FactorKfLmkPose3dWithExtrinsics_class : public testing::Test{
};
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Constructor)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
{
- FactorKfLmkPose3dWithExtrinsicsPtr factor = std::make_shared<FactorKfLmkPose3dWithExtrinsics>(
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE
- );
-
- ASSERT_TRUE(factor->getType() == "FactorKfLmkPose3dWithExtrinsics");
+ auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
+ lmk1,
+ nullptr,
+ false);
+
+ ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
}
/////////////////////////////////////////////
// Tree not ok with this gtest implementation
/////////////////////////////////////////////
-// TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_tree)
-// {
-// auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
-// S,
-// F1,
-// lmk1,
-// f1,
-// nullptr,
-// false,
-// FAC_ACTIVE);
-// ASSERT_TRUE(problem->check(1));
-// }
-
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_P_perturbated)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+ ASSERT_TRUE(problem->check(1));
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
{
- auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE);
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
// unfix F1, perturbate state
F1->unfix();
@@ -177,16 +166,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_P_perturbated)
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
}
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_O_perturbated)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
{
- auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE);
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
// unfix F1, perturbate state
F1->unfix();
@@ -196,16 +182,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_F1_O_perturbated)
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
}
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_initialization)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
{
- auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE);
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6);
ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
@@ -213,16 +196,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, Check_initialization)
}
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_P_perturbated)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
{
- auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE);
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
// unfix lmk1, perturbate state
lmk1->unfix();
@@ -233,16 +213,13 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_P_perturbated)
ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
}
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_O_perturbated)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
{
- auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE);
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
// unfix F1, perturbate state
lmk1->unfix();
@@ -254,7 +231,7 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_O_perturbated)
}
-TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_PO_perturbated)
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
{
// Change setup
Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
@@ -276,14 +253,11 @@ TEST_F(FactorKfLmkPose3dWithExtrinsics_class, solve_L1_PO_perturbated)
f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov));
// emplace factor
- auto factor = FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(f1,
- S,
- F1,
- lmk1,
- f1,
- nullptr,
- false,
- FAC_ACTIVE);
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
// Change Landmark
lmk1->getP()->setState(p_w_l);