Gtest for factor and processor pose 3d implemented

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)

test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp

+/**
+ * \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