diff --git a/CMakeLists.txt b/CMakeLists.txt
index 55c694583426d7ae5178cfa8328836779a1f3d6e..c94b33bfb04c9751c727f62bb16557cd1ef8f508 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -159,6 +159,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
include/core/factor/factor_relative_pose_3d.h
include/core/factor/factor_pose_3d_with_extrinsics.h
+ include/core/factor/factor_relative_pose_3d_with_extrinsics.h
include/core/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
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 2247a898bb4d5030ed1b07ef6422d72a9ca968e8..61962bb3cdd1b29b76e48999868c76d85392a002 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -37,61 +37,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) :
+ FactorAutodiff("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) :
+ FactorAutodiff("FactorRelativePose2dWithExtrinsics",
+ _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(),
+ _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
@@ -112,8 +155,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..5f065f0bae985d849cdd6702ce823ef80bfc31d3
--- /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("FactorRelativePose3dWithExtrinsics",
+ _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("FactorRelativePose3dWithExtrinsics",
+ _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 668be04a81a6dcdba8bfc793b28f663ac6b2aeb0..9479b901549a8777580ab7455507a4f9a83a9567 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -218,6 +218,10 @@ 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})
+# 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})
+
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3b57c7b8b71d69e3454fe84a4ca5ca0a5b17076a
--- /dev/null
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -0,0 +1,304 @@
+#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/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<StateBlock>(pos_camera, true),
+ std::make_shared<StateQuaternion>(vquat_camera, true),
+ std::make_shared<StateBlock>(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, 0.1);
+
+
+ // 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<StateBlock>(pos_landmark),
+ std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
+ }
+};
+
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
+{
+ auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
+ lmk1,
+ nullptr,
+ false);
+
+ ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
+}
+
+/////////////////////////////////////////////
+// Tree not ok with this gtest implementation
+/////////////////////////////////////////////
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
+{
+ ASSERT_TRUE(problem->check(1));
+
+ 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<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);
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
+{
+ 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);
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
+{
+ 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);
+ ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
+{
+ 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);
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
+{
+ auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
+ f1,
+ lmk1,
+ nullptr,
+ false);
+
+ // unfix F1, perturbate state
+ lmk1->unfix();
+ lmk1->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_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6);
+
+}
+
+TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
+{
+ // 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);
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+