FactorPose3dWithExtrinsics gtest implemented and working with 3 KFs

include/core/factor/factor_pose_3d_with_extrinsics.h

@@ -76,12 +76,6 @@ inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p,
     err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm);
     err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>());
 
-    std::cout << "\n\n\n\nFAC" << std::endl;
-    std::cout << "w_p_wm: " <<  w_p_wm.transpose() << std::endl;
-    std::cout << "(w_p_wb + w_q_b*b_p_bm): " <<  (w_p_wb + w_q_b*b_p_bm).transpose() << std::endl;
-    std::cout << "b_p_bm: " <<  b_p_bm.transpose() << std::endl;
-    std::cout << "err: " <<  err.transpose() << std::endl;
-
     // Residuals
     Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals);
     res = getMeasurementSquareRootInformationUpper().cast<T>() * err;

test/gtest_factor_pose_3d_with_extrinsics.cpp

@@ -7,10 +7,12 @@
 
 #include "core/utils/utils_gtest.h"
 #include "core/ceres_wrapper/solver_ceres.h"
-#include "core/factor/factor_relative_pose_3d.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_pose.h"
 #include "core/sensor/sensor_pose.h"
-#include "core/capture/capture_odom_3d.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"
 
 
@@ -21,111 +23,145 @@ using std::endl;
 
 const Vector3d zero3 = Vector3d::Zero();
 
-
-// 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_wb1 = Vector3d::Zero();
-// Quaterniond w_q_b1 = Quaterniond::Identity();
-// Vector3d w_p_wb2 = Vector3d::Ones();
-// Quaterniond w_q_b2 = Quaterniond::Identity();
-// Quaterniond w_q_b2(0,0,0,1);
-// Quaterniond w_q_b2 = Quaterniond::UnitRandom();
-
-// pose vectors
-Vector7d pose1((Vector7d() << w_p_wb1, w_q_b1.coeffs()).finished()); 
-Vector7d pose2((Vector7d() << w_p_wb2, w_q_b2.coeffs()).finished()); 
-// unitary covariance
-Matrix6d cov_odom = pow(0.01,2)*Matrix6d::Identity(); 
-Matrix6d cov_mocap = pow(0.0001,2)*Matrix6d::Identity(); 
-
-/////////////////////////////////////////
-// extrinsics ground truth and mocap data
-// Vector3d b_p_bm = Vector3d::Zero();
-// Vector3d b_p_bm = 0.1*Vector3d::Ones();
-Vector3d b_p_bm((Vector3d() << 0.1,0,0).finished());
-// Vector3d b_p_bm = Vector3d::Random();
-// Quaterniond b_q_m = Quaterniond::Identity();
-Quaterniond b_q_m(0,0,0,1);
-// Quaterniond 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; 
-Quaterniond w_q_b1_meas = w_q_b1 * b_q_m;
-Quaterniond w_q_b2_meas = w_q_b2 * b_q_m;
-Vector7d pose1_meas((Vector7d() << w_p_wb1_meas, w_q_b1_meas.coeffs()).finished());
-Vector7d pose2_meas((Vector7d() << w_p_wb2_meas, w_q_b2_meas.coeffs()).finished());
-
-/////////////////////
-// 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;
-Vector7d pose_12((Vector7d() << b1_p_b1b2, b1_q_b2.coeffs()).finished());
-
-// Problem and solver
-ProblemPtr problem = Problem::create("PO", 3);
-SolverCeres solver(problem);
-
-// pose sensor and proc (to get extrinsics in the prb)
-auto intr_sp = std::make_shared<ParamsSensorPose>();
-Vector7d extr = (Vector7d() << b_p_bm, b_q_m.coeffs()).finished();
-auto 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);
-
-// Two frames
-FrameBasePtr KF1 = problem->emplaceFrame(1, pose1);
-FrameBasePtr KF2 = problem->emplaceFrame(2, pose2);
-
-
-///////////////////
-// Create factor graph
-// Odometry capture, feature and factor
-auto cap_odom = CaptureBase::emplace<CapturePose>(KF2, 2, nullptr, pose_12, cov_odom);
-auto fea_odom = FeatureBase::emplace<FeatureBase>(cap_odom, "odom", pose_12, cov_odom);
-FactorRelativePose3dPtr fac_odom = FactorBase::emplace<FactorRelativePose3d>(fea_odom, fea_odom, KF1, nullptr, false, TOP_MOTION);
-
-// Captures mocap
-auto cap_mocap1 = CaptureBase::emplace<CapturePose>(KF1, 1, sensor_pose, pose1_meas, cov_mocap);
-auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1_meas, cov_mocap);
-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, cov_mocap);
-auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2_meas, cov_mocap);
-FactorPose3dWithExtrinsicsPtr fac_mocap2 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap2, fea_mocap2, nullptr, false, TOP_MOTION);
-
-
-
-TEST(FactorPose3dWithExtrinsics, check_tree)
+/**
+Factor graph implemented for the test
+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)_______________/
+*/
+
+class FactorPose3dWithExtrinsics_Test : public testing::Test
+{
+    public:
+
+        ProblemPtr problem_;
+        SolverCeresPtr solver_;
+        SensorBasePtr sensor_pose_;
+        FrameBasePtr KF1_;
+        FrameBasePtr KF2_;
+        FrameBasePtr KF3_;
+
+        Vector7d pose1_;
+        Vector7d pose2_;
+        Vector7d pose3_;
+        Vector3d b_p_bm_;
+        Quaterniond b_q_m_;
+
+    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(); 
+        // unitary covariance
+        Matrix6d cov_odom = pow(0.1,2)*Matrix6d::Identity(); 
+        Matrix6d cov_mocap = pow(0.001,2)*Matrix6d::Identity(); 
+
+        /////////////////////////////////////////
+        // extrinsics ground truth and mocap data
+        b_p_bm_ << 0.1,0.2,0.3;
+        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_;
+        Vector7d pose1__meas; pose1__meas << w_p_wb1_meas, w_q_b1_meas.coeffs();
+        Vector7d pose2__meas; pose2__meas << w_p_wb2_meas, w_q_b2_meas.coeffs();
+        Vector7d pose3__meas; 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;
+        Vector7d pose_12; 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;
+        Vector7d pose_23; 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>();
+        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);
+
+        // Two frames
+        KF1_ = problem_->emplaceFrame(1, pose1_);
+        KF2_ = problem_->emplaceFrame(2, pose2_);
+        KF3_ = problem_->emplaceFrame(3, pose3_);
+
+        ///////////////////
+        // Create factor graph
+        // Odometry capture, feature and factor
+        auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12, cov_odom);
+        auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12, cov_odom);
+        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, cov_odom);
+        auto fea_odom2 = FeatureBase::emplace<FeatureBase>(cap_odom2, "odom", pose_23, cov_odom);
+        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, cov_mocap);
+        auto fea_mocap1 = FeatureBase::emplace<FeatureBase>(cap_mocap1, "pose", pose1__meas, cov_mocap);
+        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, cov_mocap);
+        auto fea_mocap2 = FeatureBase::emplace<FeatureBase>(cap_mocap2, "pose", pose2__meas, cov_mocap);
+        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, cov_mocap);
+        auto fea_mocap3 = FeatureBase::emplace<FeatureBase>(cap_mocap3, "pose", pose3__meas, cov_mocap);
+        FactorPose3dWithExtrinsicsPtr fac_mocap3 = FactorBase::emplace<FactorPose3dWithExtrinsics>(fea_mocap3, fea_mocap3, nullptr, false, TOP_MOTION);
+
+    }
+
+    void TearDown() override{};
+};
+
+
+TEST_F(FactorPose3dWithExtrinsics_Test, check_tree)
 {
-    ASSERT_TRUE(problem->check(0));
+    ASSERT_TRUE(problem_->check(0));
 }
 
-TEST(FactorPose3dWithExtrinsics, solve)
+TEST_F(FactorPose3dWithExtrinsics_Test, solve)
 {
+    // somehow by default the sensor extrinsics is fixed
+    sensor_pose_->unfixExtrinsics();
 
-    // Fix frame 0, perturb frm1
-    sensor_pose->unfixExtrinsics();
-    // sensor_pose->fixExtrinsics();
-    // frm0->setState(x0);
-
-
-    problem->print(4, true, true, true);
-    problem->perturb();
-
-    std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+    problem_->perturb();
 
-    std::cout << report << std::endl;
+    std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
 
-    problem->print(4, true, true, true);
+    // problem_->print(4,1,1,1);
 
-    // ASSERT_MATRIX_APPROX(KF1->getStateVector(), pose1, 1e-6);
-    ASSERT_MATRIX_APPROX(KF2->getStateVector(), pose2, 1e-6);
-    ASSERT_MATRIX_APPROX(sensor_pose->getP()->getState(), b_p_bm, 1e-6);
-    ASSERT_MATRIX_APPROX(sensor_pose->getO()->getState(), b_q_m.coeffs(), 1e-6);
+    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);
 
 }