Merge remote-tracking branch 'origin/devel' into 4-migrate-to-state-composites

include/bodydynamics/factor/factor_force_torque.h

@@ -97,6 +97,7 @@ FactorForceTorque::FactorForceTorque(
                             bool                    _apply_loss_function,
                             FactorStatus            _status) :
     FactorAutodiff("FactorForceTorque",
+                   _feat,
                     _frame_other,              // _frame_other_ptr
                     nullptr,                   // _capture_other_ptr
                     nullptr,                   // _feature_other_ptr

include/bodydynamics/factor/factor_force_torque_preint.h

@@ -161,6 +161,7 @@ inline FactorForceTorquePreint::FactorForceTorquePreint(
                             FactorStatus                       _status) :
                 FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>(
                         "FactorForceTorquePreint",
+                        _ftr_ptr,
                         _cap_origin_ptr->getFrame(),
                         _cap_origin_ptr,
                         nullptr,

include/bodydynamics/factor/factor_inertial_kinematics.h

@@ -68,6 +68,7 @@ FactorInertialKinematics::FactorInertialKinematics(
                             bool                    _apply_loss_function,
                             FactorStatus            _status) :
     FactorAutodiff("FactorInertialKinematics",
+                   _feat,
                     nullptr,              // _frame_other_ptr
                     nullptr,              // _capture_other_ptr
                     nullptr,              // _feature_other_ptr

include/bodydynamics/math/force_torque_delta_tools.h

@@ -340,8 +340,8 @@ inline void betweenStates(const MatrixBase<D1>& c1, const MatrixBase<D2>& cd1, c
         MatrixSizeCheck<3, 1>::check(dcd);
         MatrixSizeCheck<3, 1>::check(dLc);
 
-        dc =  q1.conjugate() * ( c2 - c1 - cd1*dt -  0.5*gravity() *dt * dt );
-        dcd = q1.conjugate() * ( cd2 - cd1         -    gravity()*dt );
+        dc =  q1.conjugate() * ( c2 - c1 - cd1*dt - 0.5*gravity() *dt * dt );
+        dcd = q1.conjugate() * ( cd2 - cd1        -   gravity()*dt );
         dLc = q1.conjugate() * ( Lc2 - Lc1);
         dq =  q1.conjugate() *   q2;
 }

src/processor/processor_force_torque_preint.cpp

@@ -174,7 +174,7 @@ void ProcessorForceTorquePreint::computeCurrentDelta(
     bodydynamics::body2delta(body, _dt, std::static_pointer_cast<SensorForceTorque>(getSensor())->getMass(),
                              _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
 
-    Matrix<double, 12, 15> jac_delta_body_reduced = jac_delta_body.leftCols<15>();
+    Matrix<double, 12, 18> jac_delta_body_reduced = jac_delta_body.leftCols<18>();
 
     // compute delta_cov (using uncertainty on f,tau,pbc)
     _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose();  // trivially:  jac_body_delta = Identity

src/processor/processor_inertial_kinematics.cpp

@@ -62,7 +62,6 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
         bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_pack_kf_it->first, buffer_capture_it->first, buffer_pack_kf_it->second->time_tolerance);
         if (time_ok) {
             CaptureBasePtr cap_angvel = buffer_pack_kf_it->second->key_frame->getCaptureOf(sensor_angvel);
-
             auto min_ts = (buffer_pack_kf_it->first < buffer_capture_it->first) ? buffer_pack_kf_it->first : buffer_capture_it->first;
             if (cap_angvel && cap_angvel->getStateBlock("I")){  // TODO: or only cap_angvel?
                 // cast incoming capture to the range-and-bearing type, add it to the keyframe
@@ -80,10 +79,12 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
             else {
                 // if time ok but no capture angvel yet, there is not gonna be any in the next KF of the buffer
                 break;
+                buffer_capture_it++;
+                buffer_pack_kf_it++;
             }
 
             ////////////////
-            // remove everything before (exclusive) this timestamp -> the cap_angvel is yet to come
+            // remove everything before (Inclusive if equal) this timestamp -> the cap_angvel is yet to come
             buffer_pack_kf_.removeUpTo(min_ts);
             buffer_capture_.removeUpTo(min_ts);
         }
@@ -133,7 +134,7 @@ inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureI
         StateBlockPtr sb_pbc1 = _cap_ikin_origin->getSensorIntrinsic();
         StateBlockPtr sb_pbc2 = _cap_ikin->getSensorIntrinsic();
         FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
-            feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin
+                feat_diff, feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin
         );
     }
 

test/gtest_factor_inertial_kinematics.cpp

@@ -76,6 +76,7 @@ class FactorInertialKinematics_1KF : public testing::Test
         Eigen::Matrix3d Qp_, Qv_, Qw_;
         Eigen::Vector3d bias_p_, bias_imu_;
         FeatureInertialKinematicsPtr feat_in_;
+        StateBlockPtr sbbimu_;
 
     virtual void SetUp()
     {
@@ -108,12 +109,12 @@ class FactorInertialKinematics_1KF : public testing::Test
         StateBlockPtr sbd = make_shared<StateBlock>(zero3);
         StateBlockPtr sbL = make_shared<StateBlock>(zero3);
         StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
-        StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu_);
+        sbbimu_ = make_shared<StateBlock>(bias_imu_);
 
         KF0_->addStateBlock("C", sbc, problem_);
         KF0_->addStateBlock("D", sbd, problem_);
         KF0_->addStateBlock("L", sbL, problem_);
-        KF0_->addStateBlock("I", sbbimu, problem_);  // IMU bias
+        KF0_->addStateBlock("I", sbbimu_, problem_);  // IMU bias
 
         // Fix the one we cannot estimate
         KF0_->getStateBlock("I")->fix();  // Imu
@@ -132,12 +133,9 @@ class FactorInertialKinematics_1KF : public testing::Test
         Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
         Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
 
-        // =============== CREATE CAPURE/FEATURE/FACTOR
+        // =============== CREATE CAPURE
         Vector9d meas_ikin0; meas_ikin0 << pBC_meas, vBC_meas, w_meas;
         CIK0_ = CaptureBase::emplace<CaptureInertialKinematics>(KF0_, t_, SIK_, meas_ikin0, Iq, Lq, bias_p_);
-        Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas_ikin0.head<3>(), meas_ikin0.tail<3>(), Iq);
-        feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas_ikin0, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
-        auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu, nullptr, false);
     }
 
     virtual void TearDown(){}
@@ -162,11 +160,10 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
         Eigen::Matrix3d Iq; Iq.setIdentity();
         Eigen::Vector3d Lq = zero3;
 
-        feat_in_->setMeasurement(meas);
+        // =============== CREATE FEATURE/FACTOR
         Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
-        feat_in_->setMeasurementCovariance(Q_ikin_err);
-        feat_in_->setBIq(Iq);
-        feat_in_->setBLcm(Lq);
+        feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+        auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
     }
 };
 
@@ -188,11 +185,10 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K
         Eigen::Matrix3d Iq; Iq.setIdentity();
         Eigen::Vector3d Lq = zero3;
 
-        feat_in_->setMeasurement(meas);
+        // =============== CREATE FEATURE/FACTOR
         Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
-        feat_in_->setMeasurementCovariance(Q_ikin_err);
-        feat_in_->setBIq(Iq);
-        feat_in_->setBLcm(Lq);
+        feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+        auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
     }
 };
 
@@ -215,11 +211,10 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K
         Eigen::Matrix3d Iq; Iq.setIdentity();
         Eigen::Vector3d Lq = zero3;
 
-        feat_in_->setMeasurement(meas);
+        // =============== CREATE FEATURE/FACTOR
         Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
-        feat_in_->setMeasurementCovariance(Q_ikin_err);
-        feat_in_->setBIq(Iq);
-        feat_in_->setBLcm(Lq);
+        feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+        auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
     }
 };
 
@@ -243,11 +238,10 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
         Eigen::Matrix3d Iq; Iq.setIdentity();
         Eigen::Vector3d Lq = zero3;
 
-        feat_in_->setMeasurement(meas);
+        // =============== CREATE FEATURE/FACTOR
         Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
-        feat_in_->setMeasurementCovariance(Q_ikin_err);
-        feat_in_->setBIq(Iq);
-        feat_in_->setBLcm(Lq);
+        feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+        auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
     }
 };
 
@@ -356,7 +350,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
 //        CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0_, "CaptureInertialKinematics", t_, nullptr);
 //        auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
 //        feat_in_ = static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a segfault
-//        // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu, nullptr, false);
+//        // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
 //    }
 //
 //    virtual void TearDown(){}

test/gtest_processor_force_torque_preint.cpp

@@ -121,7 +121,7 @@ public:
         ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
         params_sen_ft->sensor_ikin_name = "SenIK";
         params_sen_ft->sensor_angvel_name = "SenImu";
-        params_sen_ft->time_tolerance = 0.0025;
+        params_sen_ft->time_tolerance = 0.5;
         params_sen_ft->unmeasured_perturbation_std = 1e-4;
         params_sen_ft->max_time_span = 1000;
         params_sen_ft->max_buff_length = 500;
@@ -138,7 +138,7 @@ public:
 };
 
 
-class Cst2KFZeroMotionZeroBias : public ProcessorForceTorquePreintImuOdom3dIkin2KF
+class Cst2KFZeroMotion : public ProcessorForceTorquePreintImuOdom3dIkin2KF
 {
 public:
     FrameBasePtr KF2_;
@@ -161,6 +161,7 @@ public:
     Matrix6d cov_f_;
     Matrix6d cov_tau_;
     Matrix3d cov_pbc_;
+    Matrix3d cov_wb_;
     
     // Aggregated data to construct Captures 
     Vector6d acc_gyr_meas_;
@@ -173,13 +174,21 @@ public:
     Vector3d bias_pbc_;
     Vector7d prev_pose_curr_;
 
+    VectorXd x_origin_;
 
 
-    Cst2KFZeroMotionZeroBias()
+
+    Cst2KFZeroMotion()
     {
         bias_pbc_ = ZERO3;
     }
 
+    virtual void setOriginState()
+    {
+        x_origin_.resize(19);
+        x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
+    }
+
     virtual void SetUp()
     {
         ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp();
@@ -193,8 +202,7 @@ public:
         // We are not using setPrior because of processors initial captures problems so we have to
         // - Manually set problem prior
         // - call setOrigin on processors isMotion
-        VectorXd x_origin_(19);
-        x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
+        setOriginState();
         MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
         KF1_ = problem_->emplaceFrame(KEY, t0_, x_origin_);
         // Prior pose factor
@@ -204,7 +212,7 @@ public:
         // Prior velocity factor
         CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1_, "Vel0", t0_);
         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, KF1_->getV(), nullptr, false);
+        FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1_->getV(), nullptr, false);
 
         initializeData1();
 
@@ -213,7 +221,7 @@ public:
         // 1. Call setOrigin processorIMU  to generate a fake captureIMU -> generates b_imu
         // 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
         // 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
-        proc_inkin_->keyFrameCallback(KF1_, 0.0025);
+        proc_inkin_->keyFrameCallback(KF1_, 0.5);
         proc_imu_->setOrigin(KF1_);
         auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_);
         CIKin0->process();
@@ -272,7 +280,7 @@ public:
         Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
         CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_);
         FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
-        FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
+        FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
 
         // Add loose absolute factor on initial bp bias since dynamical trajectories 
         // able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
@@ -280,7 +288,7 @@ public:
         CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
         FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp);
         // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
-        FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic(),nullptr,false);
+        FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic(),nullptr,false);
  
     }
 
@@ -298,10 +306,11 @@ public:
         cap_ftp_data_.resize(32,1);
         cap_ftp_data_ << f_meas, tau_meas, pbc_meas_, acc_gyr_meas_.tail<3>(), kin_meas;
 
-        Qftp_ = Matrix<double, 15, 15>::Identity();
+        Qftp_ = Matrix<double, 18, 18>::Identity();
         Qftp_.block<6, 6>(0, 0) = cov_f_;
         Qftp_.block<6, 6>(6, 6) = cov_tau_;
         Qftp_.block<3, 3>(12, 12) = cov_pbc_;
+        Qftp_.block<3, 3>(15, 15) = cov_wb_;
     }
 
     void setZeroMotionData()
@@ -330,6 +339,7 @@ public:
         cov_f_ = 1e-4 * Matrix6d::Identity();
         cov_tau_ = 1e-4 * Matrix6d::Identity();
         cov_pbc_ = 1e-4 * Matrix3d::Identity();
+        cov_wb_ = 1e-4 * Matrix3d::Identity();
     }
 
     virtual void initializeData1()
@@ -355,17 +365,17 @@ public:
 
 };
 
-class Cst2KFZeroMotionBias : public Cst2KFZeroMotionZeroBias
+class Cst2KFZeroMotionBias : public Cst2KFZeroMotion
 {
 public:
     void SetUp() override
     {
         bias_pbc_ = BIAS_PBC_SMAL;
-        Cst2KFZeroMotionZeroBias::SetUp();
+        Cst2KFZeroMotion::SetUp();
     }
 };
 
-class Cst2KFXaxisLinearMotion : public Cst2KFZeroMotionZeroBias
+class Cst2KFXaxisLinearMotion : public Cst2KFZeroMotion
 {
 public:
     void initializeData1() override
@@ -385,9 +395,21 @@ public:
     }
 };
 
-class Cst2KFXaxisLinearMotionPbc : public Cst2KFZeroMotionZeroBias
+class Cst2KFXaxisLinearMotionPbc : public Cst2KFZeroMotion
 {
 public:
+
+    void SetUp() override
+    {
+        Cst2KFZeroMotion::SetUp();
+    }
+
+    void setOriginState() override
+    {
+        Cst2KFZeroMotion::setOriginState();
+        x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+    }
+
     void initializeData1() override
     {
         setZeroMotionData();
@@ -408,13 +430,19 @@ public:
     }
 };
 
-class Cst2KFXaxisLinearMotionPbcBias : public Cst2KFZeroMotionZeroBias
+class Cst2KFXaxisLinearMotionPbcBias : public Cst2KFZeroMotion
 {
 public:
     void SetUp() override
     {
         bias_pbc_ = BIAS_PBC_SMAL;
-        Cst2KFZeroMotionZeroBias::SetUp();
+        Cst2KFZeroMotion::SetUp();
+    }
+
+    void setOriginState() override
+    {
+        Cst2KFZeroMotion::setOriginState();
+        x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
     }
 
     void initializeData1() override
@@ -436,13 +464,19 @@ public:
         prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
     }
 };
-class Cst2KFXaxisLinearMotionPbcBiasPQbl : public Cst2KFZeroMotionZeroBias
+class Cst2KFXaxisLinearMotionPbcBiasPQbl : public Cst2KFZeroMotion
 {
 public:
     void SetUp() override
     {
         bias_pbc_ = BIAS_PBC_SMAL;
-        Cst2KFZeroMotionZeroBias::SetUp();
+        Cst2KFZeroMotion::SetUp();
+    }
+
+    void setOriginState() override
+    {
+        Cst2KFZeroMotion::setOriginState();
+        x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
     }
 
     void initializeData1() override
@@ -474,14 +508,55 @@ public:
     }
 };
 
+class Cst2KFXaxisRotationPureTorque : public Cst2KFZeroMotion
+{
+public:
+    void SetUp() override
+    {
+        Cst2KFZeroMotion::SetUp();
+    }
+
+    void initializeData1() override
+    {
+        setZeroMotionData();
+
+        // taus according to static Euler eq
+        tau1_ << M_PI/3, 0, 0; // with Identity rotational inertial matrix, rotation of PI/2 rad.s-2
+        w_meas_ << M_PI/3, 0, 0;
+
+        buildDataVectors();
+    }
+
+    void setOdomData() override
+    {
+        prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+    }
+};
+
 //////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////
 /////////////////////////////   TESTS   //////////////////////////////////
 //////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////
 
-TEST_F(Cst2KFZeroMotionZeroBias, ZeroMotionZeroBias)
+TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
 {
+    // TEST FIRST PURE INTEGRATION
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+    // COM position on Z axis is not observable with this problem
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+    // COM position on Z axis is not observable with this problem
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
+    // THEN SOLVE
     problem_->perturb();
 
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -530,18 +605,33 @@ TEST_F(Cst2KFZeroMotionBias, ZeroMotionBias)
     ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
 }
 
-TEST_F(Cst2KFXaxisLinearMotion, XLinearMotion)
+TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
 {
+
+    Vector3d posi_diff = ZERO3;
+    posi_diff[0] = 0.5*ACC * pow(3*DT,2);
+    Vector3d vel_diff = ZERO3;
+    vel_diff[0] = ACC * 3*DT;
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+    // COM position on Z axis is not observable with this problem
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+    // COM position on Z axis is not observable with this problem
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), posi_diff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
     problem_->perturb();
 
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
     // problem_->print(4, true, true, true);
     // std::cout << report << std::endl;
 
-    Vector3d posi_diff = ZERO3;
-    posi_diff[0] = 0.5*ACC * pow(3*DT,2);
-    Vector3d vel_diff = ZERO3;
-    vel_diff[0] = ACC * 3*DT;
     ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
     ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
     // COM position on Z axis is not observable with this problem
@@ -563,16 +653,29 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotion)
 
 TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
 {
+    Vector3d posi_diff = ZERO3;
+    posi_diff[0] = 0.5*ACC * pow(3*DT,2);
+    Vector3d vel_diff = ZERO3;
+    vel_diff[0] = ACC * 3*DT;
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+    // COM position on Z axis is not observable with this problem
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
+    // COM position on Z axis is not observable with this problem
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
     problem_->perturb();
 
     std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // problem_->print(4, true, true, true);
     // std::cout << report << std::endl;
 
-    Vector3d posi_diff = ZERO3;
-    posi_diff[0] = 0.5*ACC * pow(3*DT,2);
-    Vector3d vel_diff = ZERO3;
-    vel_diff[0] = ACC * 3*DT;
     ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
     ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
     // COM position on Z axis is not observable with this problem
@@ -655,6 +758,22 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
 }
 
 
+TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve)
+{
+
+    Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
+    Vector4d q_diff; q_diff << 1,0,0,0;
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
+    ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
+
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_diff, 1e-6);
+    ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
+}
+
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);

test/gtest_processor_inertial_kinematics.cpp

@@ -113,7 +113,7 @@ class FactorInertialKinematics_2KF : public testing::Test
         // 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);
+        FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF0_->getV(), nullptr, false);
         // Set origin of processor Imu
         std::static_pointer_cast<ProcessorImu>(proc)->setOrigin(KF0_);
 
@@ -122,7 +122,7 @@ class FactorInertialKinematics_2KF : public testing::Test
         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);
+        FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
 
     }
 
@@ -154,11 +154,10 @@ 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);
-
+    auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin_, meas_ikin0, Iq, Lq);
+    sen_ikin_->process(CIKin0);
 
     CIKin0->getSensorIntrinsic()->fix();