diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e62e51d0acafc4efed5b0d4eff606ec508531779..885c1f53a456deb4b74392d011d560cb7216bafe 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -209,10 +209,6 @@ 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})
-# FactorPose3dWithExtrinsics class test
-wolf_add_gtest(gtest_factor_pose_3d_with_extrinsics gtest_factor_pose_3d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
-
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
@@ -245,6 +241,11 @@ target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
+# FactorPose3dWithExtrinsics class test
+wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+target_link_libraries(gtest_processor_and_factor_pose_3d_with_extrinsics ${PLUGIN_NAME})
+
+
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy)
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9146cc7e3f49d70a208439c83b709646828d924
--- /dev/null
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -0,0 +1,329 @@
+/**
+ * \file gtest_factor_pose_with_extrinsics.cpp
+ *
+ * Created on: Feb a9, 2020
+ * \author: mfourmy
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/processor/processor_pose.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+const Vector3d zero3 = Vector3d::Zero();
+
+
+
+class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
+{
+ /**
+ Factor graph implemented common for all the tests
+ 2 KF is not enough since the SensorPose extrinsics translation would be unobservable along the first KF1-KF2 transformation axis of rotation
+ (KF1)----|Odom3d|----(KF2)----|Odom3d|----(KF3)
+ | | |
+ | | |
+ |Pose3dWE| |Pose3dWE| |Pose3dWE|
+ \ /
+ \ /
+ \____________(Extrinsics)_______________/
+ */
+
+ // In this class, the poses of the 3 KF are chosen randomly and then measurement data for the factors are
+ // derived from random extrinsics
+ // The problem and Pose Sensor/Processor are implemented
+
+ public:
+
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ SensorBasePtr sensor_pose_;
+ FrameBasePtr KF1_;
+ FrameBasePtr KF2_;
+ FrameBasePtr KF3_;
+
+ Vector7d pose1_;
+ Vector7d pose2_;
+ Vector7d pose3_;
+ Vector7d pose_12_;
+ Vector7d pose_23_;
+ Vector3d b_p_bm_;
+ Quaterniond b_q_m_;
+
+ Vector7d pose1_meas_;
+ Vector7d pose2_meas_;
+ Vector7d pose3_meas_;
+
+ void SetUp() override
+ {
+ // 2 random 3d positions
+ Vector3d w_p_wb1 = Vector3d::Random();
+ Quaterniond w_q_b1 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb2 = Vector3d::Random();
+ Quaterniond w_q_b2 = Quaterniond::UnitRandom();
+ Vector3d w_p_wb3 = Vector3d::Random();
+ Quaterniond w_q_b3 = Quaterniond::UnitRandom();
+
+ // pose vectors
+ pose1_ << w_p_wb1, w_q_b1.coeffs();
+ pose2_ << w_p_wb2, w_q_b2.coeffs();
+ pose3_ << w_p_wb3, w_q_b3.coeffs();
+
+ /////////////////////////////////////////
+ // extrinsics ground truth and mocap data
+ // b_p_bm_ << 0.1,0.2,0.3;
+ b_p_bm_ << Vector3d::Random();
+ b_q_m_ = Quaterniond::UnitRandom();
+
+ Vector3d w_p_wb1_meas = w_p_wb1 + w_q_b1 * b_p_bm_;
+ Vector3d w_p_wb2_meas = w_p_wb2 + w_q_b2 * b_p_bm_;
+ Vector3d w_p_wb3_meas = w_p_wb3 + w_q_b3 * b_p_bm_;
+ Quaterniond w_q_b1_meas = w_q_b1 * b_q_m_;
+ Quaterniond w_q_b2_meas = w_q_b2 * b_q_m_;
+ Quaterniond w_q_b3_meas = w_q_b3 * b_q_m_;
+ pose1_meas_ << w_p_wb1_meas, w_q_b1_meas.coeffs();
+ pose2_meas_ << w_p_wb2_meas, w_q_b2_meas.coeffs();
+ pose3_meas_ << w_p_wb3_meas, w_q_b3_meas.coeffs();
+
+ /////////////////////
+ // relative odom data
+ Vector3d b1_p_b1b2 = w_q_b1.conjugate()*(w_p_wb2 - w_p_wb1);
+ Quaterniond b1_q_b2 = w_q_b1.conjugate()*w_q_b2;
+ pose_12_ << b1_p_b1b2, b1_q_b2.coeffs();
+
+ Vector3d b2_p_b2b3 = w_q_b2.conjugate()*(w_p_wb3 - w_p_wb2);
+ Quaterniond b2_q_b3 = w_q_b2.conjugate()*w_q_b3;
+ pose_23_ << b2_p_b2b3, b2_q_b3.coeffs();
+
+ // Problem and solver_
+ problem_ = Problem::create("PO", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = 1;
+ intr_sp->std_o = 1;
+ Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
+ }
+
+ void TearDown() override{};
+};
+
+class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ ///////////////////
+ // Create factor graph
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // Captures mocap
+ auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1_, 1, sensor_pose_, pose1_meas_, cov6d);
+ auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap1 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap1, fea_mocap1, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap2 = CaptureBase::emplace<CapturePose>(KF2_, 2, sensor_pose_, pose2_meas_, cov6d);
+ auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
+
+ auto cap_mocap3 = CaptureBase::emplace<CapturePose>(KF3_, 3, sensor_pose_, pose3_meas_, cov6d);
+ auto fea_mocap3 = FeatureBase::emplace<FeatureBase>(cap_mocap3, "pose", pose3_meas_, cov6d);
+ FactorPose3dWithExtrinsicsPtr fac_mocap3 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap3, fea_mocap3, nullptr, false, TOP_MOTION);
+
+
+ }
+
+ void TearDown() override{};
+};
+
+
+class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+ problem_->keyFrameCallback(KF1_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF2_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+
+ ///////////////////
+ // Create factor graph
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // process mocap data
+ auto cap_mocap1 = std::make_shared<CapturePose>(1, sensor_pose_, pose1_meas_, cov6d);
+ auto cap_mocap2 = std::make_shared<CapturePose>(2, sensor_pose_, pose2_meas_, cov6d);
+ auto cap_mocap3 = std::make_shared<CapturePose>(3, sensor_pose_, pose3_meas_, cov6d);
+ cap_mocap1->process();
+ cap_mocap2->process();
+ cap_mocap3->process();
+
+ }
+
+ void TearDown() override{};
+};
+
+
+class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorPose3dWithExtrinsicsBase_Test
+{
+ public:
+
+ void SetUp() override
+ {
+ FactorPose3dWithExtrinsicsBase_Test::SetUp();
+ // Two frames
+ KF1_ = problem_->emplaceFrame(1, pose1_);
+ KF2_ = problem_->emplaceFrame(2, pose2_);
+ KF3_ = problem_->emplaceFrame(3, pose3_);
+
+ Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ ///////////////////
+ // Create factor graph
+ // Odometry capture, feature and factor
+ auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
+ auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
+ FactorRelativePose3dPtr fac_odomA = FactorBase::emplace<FactorRelativePose3d>(fea_odom1, fea_odom1, KF1_, nullptr, false, TOP_MOTION);
+
+ auto cap_odom2 = CaptureBase::emplace<CapturePose>(KF3_, 3, nullptr, pose_23_, cov6d);
+ auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23_, cov6d);
+ FactorRelativePose3dPtr fac_odom2 = FactorBase::emplace<FactorRelativePose3d>(fea_odom2, fea_odom2, KF2_, nullptr, false, TOP_MOTION);
+
+ // process mocap data
+ auto cap_mocap1 = std::make_shared<CapturePose>(1, sensor_pose_, pose1_meas_, cov6d);
+ auto cap_mocap2 = std::make_shared<CapturePose>(2, sensor_pose_, pose2_meas_, cov6d);
+ auto cap_mocap3 = std::make_shared<CapturePose>(3, sensor_pose_, pose3_meas_, cov6d);
+ cap_mocap1->process();
+ cap_mocap2->process();
+ cap_mocap3->process();
+
+ // keyframe callback called after all mocap captures have been processed
+ problem_->keyFrameCallback(KF1_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF2_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ }
+
+ void TearDown() override{};
+};
+
+
+
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
+{
+ // somehow by default the sensor extrinsics is fixed
+ // sensor_pose_->unfixExtrinsics();
+
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ problem_->print(4,1,1,1);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+
+}
+
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve)
+{
+ // somehow by default the sensor extrinsics is fixed
+ sensor_pose_->unfixExtrinsics();
+
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+
+}
+
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve)
+{
+ // somehow by default the sensor extrinsics is fixed
+ sensor_pose_->unfixExtrinsics();
+
+ problem_->perturb();
+
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+
+ SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
+ ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6);
+ ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+}
+
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_pose.cpp
similarity index 100%
rename from test/gtest_factor_pose_3d_with_extrinsics.cpp
rename to test/gtest_processor_pose.cpp