Fixed processor inertial kinematics gtest

test/gtest_processor_force_torque_preint.cpp

@@ -285,14 +285,6 @@ public:
     }
 
     void buildDataVectors(){
-        std::cout << "\n\nOOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << "OOOOOOOOOOOOOOOO" << std::endl;
-        std::cout << pbc_ << std::endl;
         pbc_meas_ = pbc_ + bias_pbc_;
 
         acc_gyr_meas_ << acc_meas_, w_meas_;

test/gtest_processor_inertial_kinematics.cpp

@@ -18,6 +18,7 @@
 #include <core/ceres_wrapper/ceres_manager.h>
 #include <core/math/rotations.h>
 #include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
 #include <core/feature/feature_base.h>
 #include <core/factor/factor_block_absolute.h>
 
@@ -39,6 +40,9 @@ using namespace wolf;
 using namespace Eigen;
 using namespace std;
 
+const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
+const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
+
 
 class FactorInertialKinematics_2KF : public testing::Test
 {
@@ -67,9 +71,7 @@ class FactorInertialKinematics_2KF : public testing::Test
 
         //===================================================== INITIALIZATION
         x_origin_.resize(19);
-        Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
-        x_origin_ << zero3, 0,0,0,1, zero3, zero3, zero3, zero3;
-        //P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+        x_origin_ << ZERO3, 0,0,0,1, ZERO3, ZERO3, ZERO3, ZERO3;
         P_origin_ = pow(1e-3, 2) * Eigen::MatrixXd::Identity(18,18);
         t_.set(0.0);
 
@@ -79,29 +81,49 @@ class FactorInertialKinematics_2KF : public testing::Test
         intr_ik->std_vbc = 0.1;
         VectorXd extr;  // default size for dynamic vector is 0-0 -> what we need
         SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
-        // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr,  bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml");  // TODO: does not work!
+        // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr,  bodydynamics_root_dir + "/test/sensor_inertial_kinematics.yaml");  // TODO: does not work!
         sen_ikin_ = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
 
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu.yaml");
+        SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root_dir + "/test/sensor_imu.yaml");
         sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
-        ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu.yaml");
+        ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
         // auto processor_imu = std::static_pointer_cast<ProcessorImu>(proc);
 
 
 
         ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
         params_ik->sensor_angvel_name = "SenImu";
-        params_ik->std_bp_drift = 0.01;
+        params_ik->std_bp_drift = 0.0000001;
         ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
-        // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
+        // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
         // auto proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
 
         // Set origin of the problem
-        KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
+        // KF0_ = problem_->setPriorFactor(x_origin_, P_origin_, t_, 0.005);
+        KF0_ = problem_->emplaceFrame(KEY, x_origin_, t_);
+
+        ///////////////////////////////////////////////////
+        // Prior pose factor
+        CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF0_, t_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
+        pose_prior_capture->emplaceFeatureAndFactor();
+
+        ///////////////////////////////////////////////////
+        // Prior velocity factor
+        CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
+        FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.segment<3>(7), P_origin_.block<3, 3>(6, 6));
+        FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
         // Set origin of processor Imu
         std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_);
+
+        ////////////////////////////////////////////
+        // Add absolute factor on Imu biases to simulate a fix()
+        Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
+        CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t_);
+        FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
+        FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
+
     }
 
     virtual void TearDown(){}
@@ -132,18 +154,18 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
     // sen_imu_->getIntrinsic()->fix();
 
     // T0
+    auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin_, meas_ikin0, Iq, Lq);
+    sen_ikin_->process(CIKin0);
     CaptureImuPtr CImu0 = std::make_shared<CaptureImu>(t0, sen_imu_, acc_gyr_meas, acc_gyr_cov);
     sen_imu_->process(CImu0);
-    CImu0->getSensorIntrinsic()->fix();
 
-    auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin_, meas_ikin0, Iq, Lq);
-    sen_ikin_->process(CIKin0);
+
     CIKin0->getSensorIntrinsic()->fix();
 
     // T1
     CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas, acc_gyr_cov);
     sen_imu_->process(CImu1);
-    CImu1->getSensorIntrinsic()->fix();
+    // CImu1->getSensorIntrinsic()->fix();
 
     auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin0, Iq, Lq);
     sen_ikin_->process(CIKin1);
@@ -152,7 +174,7 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
 
     // T2
     CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(t2, sen_imu_, acc_gyr_meas, acc_gyr_cov);
-    CImu2->getSensorIntrinsic()->fix();
+    // CImu2->getSensorIntrinsic()->fix();
     sen_imu_->process(CImu2);
 
     auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin0, Iq, Lq);
@@ -161,23 +183,36 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
 
     // T3
     CaptureImuPtr CImu3 = std::make_shared<CaptureImu>(t3, sen_imu_, acc_gyr_meas, acc_gyr_cov);
-    CImu3->getSensorIntrinsic()->fix();
+    // CImu3->getSensorIntrinsic()->fix();
     sen_imu_->process(CImu3);
 
     CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin0, Iq, Lq);
     sen_ikin_->process(CIKin3);
     CIKin3->getSensorIntrinsic()->fix();
 
+    KF0_->perturb();
 
-    KF0_->getStateBlock("C")->setState(Eigen::Vector3d::Random());
-    KF0_->getStateBlock("D")->setState(Eigen::Vector3d::Random());
-    KF0_->getStateBlock("L")->setState(Eigen::Vector3d::Random());
+    std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+    std::cout << report << std::endl;
+    problem_->print(4,true,true,true);
 
+    Vector3d pdiff; pdiff << 4.5, 0, 0;
+    Vector3d vdiff; vdiff << 3, 0, 0;
 
-    problem_->print(4,true,true,true);
-    // std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
+    FrameBasePtr KF1 = problem_->getTrajectory()->getLastKeyFrame();
+
+
+    ASSERT_MATRIX_APPROX(KF0_->getStateBlock("P")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF0_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF0_->getStateBlock("C")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF0_->getStateBlock("D")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF0_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
+    ASSERT_MATRIX_APPROX(KF1->getStateBlock("P")->getState(), pdiff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1->getStateBlock("V")->getState(), vdiff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1->getStateBlock("C")->getState(), pdiff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1->getStateBlock("D")->getState(), vdiff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1->getStateBlock("L")->getState(), ZERO3, 1e-6);
 }