Refactoring + new gtest: 3 KF, 2 FT factor

test/gtest_factor_force_torque.cpp

@@ -61,6 +61,9 @@ using namespace std;
 // SOME CONSTANTS
 const double Acc1 = 1.0;
 const double Acc2 = 2.5;
+const Vector3d zero3 = Vector3d::Zero();
+const Vector6d zero6 = Vector6d::Zero();
+
 
 
 Matrix9d computeKinJac(const Vector3d& w_unb,
@@ -120,7 +123,9 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
         }
         else 
         {   
-            Vector3d pert = Vector3d::Random();
+            VectorXd pert;
+            pert.resize(KF->getStateBlock(sb_name)->getSize());
+            pert.setRandom();
             KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
         }
     } 
@@ -133,6 +138,18 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
     }
 }
 
+FrameBasePtr createKFWithCDLBI(const ProblemPtr& problem, const TimeStamp& t, const Vector10d& x_origin,
+                              const Vector3d& c, const Vector3d& cd, const Vector3d& Lc, const Vector3d& bias_p, const Vector6d& bias_imu)
+{   
+    FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), "POV", 3, wolf::KEY, t, x_origin);
+    StateBlockPtr sbc = make_shared<StateBlock>(c);  KF->addStateBlock("C", sbc); problem->notifyStateBlock(sbc,ADD);
+    StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock("D", sbd); problem->notifyStateBlock(sbd,ADD);
+    StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock("L", sbL); problem->notifyStateBlock(sbL,ADD);
+    StateBlockPtr sbb = make_shared<StateBlock>(bias_p); KF->addStateBlock("B", sbb); problem->notifyStateBlock(sbb,ADD);
+    StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock("I", sbbimu); problem->notifyStateBlock(sbbimu,ADD);
+    return KF;
+}
+
 
 class FactorInertialKinematics_2KF : public testing::Test
 {
@@ -147,13 +164,15 @@ class FactorInertialKinematics_2KF : public testing::Test
         FrameBasePtr KFA_;
         FrameBasePtr KFB_;
         Matrix3d Qp_, Qv_, Qw_;
-        Vector3d bias_p_, bias_imu_;
+        Vector3d bias_p_;
+        Vector6d bias_imu_;
         FeatureInertialKinematicsPtr feat_ikinA_;
         FeatureInertialKinematicsPtr feat_ikinB_;
         FeatureForceTorquePtr feat_ftAB_;
         // SensorIMUPtr sen_imu_;
         // ProcessorIMUPtr processor_imu_;
-
+    
+    protected:
     virtual void SetUp()
     {
 
@@ -195,10 +214,8 @@ class FactorInertialKinematics_2KF : public testing::Test
 
 
         // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
-        Vector3d zero3; zero3 << 0,0,0;
-        Vector6d zero6; zero6 << zero3, zero3;
-        Vector3d bias_p_ = zero3;
-        Vector6d bias_imu_ = zero6;
+        bias_p_ = zero3;
+        bias_imu_ = zero6;
         StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("C", sbcA); problem_->notifyStateBlock(sbcA,ADD);
         StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("D", sbdA); problem_->notifyStateBlock(sbdA,ADD);
         StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock("L", sbLA); problem_->notifyStateBlock(sbLA,ADD);
@@ -238,16 +255,11 @@ class FactorInertialKinematics_2KF : public testing::Test
         // =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
         tB_.set(1);
 
-        KFB_ = FrameBase::emplace<FrameBase>(problem_->getTrajectory(), "POV", 3, wolf::KEY, tB_, x_origin_);
-        StateBlockPtr sbcB = make_shared<StateBlock>(zero3); KFB_->addStateBlock("C", sbcB); problem_->notifyStateBlock(sbcB,ADD);
-        StateBlockPtr sbdB = make_shared<StateBlock>(zero3); KFB_->addStateBlock("D", sbdB); problem_->notifyStateBlock(sbdB,ADD);
-        StateBlockPtr sbLB = make_shared<StateBlock>(zero3); KFB_->addStateBlock("L", sbLB); problem_->notifyStateBlock(sbLB,ADD);
-        StateBlockPtr sbbB = make_shared<StateBlock>(bias_p_); KFB_->addStateBlock("B", sbbB); problem_->notifyStateBlock(sbbB,ADD);
-        StateBlockPtr sbbimuB = make_shared<StateBlock>(bias_imu_); KFB_->addStateBlock("I", sbbimuB); problem_->notifyStateBlock(sbbimuB,ADD);
-
+        KFB_ = createKFWithCDLBI(problem_, tB_, x_origin_,
+                                zero3, zero3, zero3, bias_p_, bias_imu_);
         // Fix the one we cannot estimate
         // KFB_->getP()->fix();
-        KFB_->getO()->fix();
+        KFB_->getO()->fix();  // Not in the FT factor, so should be fixed (or absolute factor)
         // KFB_->getV()->fix();
         KFB_->getStateBlock("I")->fix();
         KFB_->getStateBlock("B")->fix();
@@ -268,7 +280,7 @@ class FactorInertialKinematics_2KF : public testing::Test
         CaptureBasePtr cap_ikinB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureInertialKinematics", tB_, nullptr);
         auto feat_ikinB = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinB, meas_ikinB, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
         feat_ikinB_ = static_pointer_cast<FeatureInertialKinematics>(feat_ikinB); // !! auto feat_ikinB_ creates no compilation error but a segfault
-        auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(feat_ikinB_, feat_ikinB_, sbbimuB, nullptr, false);
+        auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(feat_ikinB_, feat_ikinB_, KFB_->getStateBlock("I"), nullptr, false);
 
 
         // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
@@ -302,8 +314,9 @@ class FactorInertialKinematics_2KF : public testing::Test
 };
 
 
-class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
+class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
 {
+    protected:
     virtual void SetUp() override
     {
         FactorInertialKinematics_2KF::SetUp();
@@ -314,8 +327,9 @@ class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
     }
 };
 
-class FactorInertialKinematics_2KF_FY : public FactorInertialKinematics_2KF
+class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
 {
+    protected:
     virtual void SetUp() override
     {
         FactorInertialKinematics_2KF::SetUp();
@@ -326,6 +340,87 @@ class FactorInertialKinematics_2KF_FY : public FactorInertialKinematics_2KF
     }
 };
 
+class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
+{
+    protected:
+    virtual void SetUp() override
+    {
+        FactorInertialKinematics_2KF::SetUp();
+        Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished()); 
+        Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished()); 
+        Vector6d fmeas; fmeas << f1, f2; 
+        feat_ftAB_->setForcesMeas(fmeas);
+    }
+};
+
+class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
+{
+    public: 
+        FrameBasePtr KFC_;
+
+    protected:
+    virtual void SetUp() override
+    {
+        FactorInertialKinematics_2KF_ForceX::SetUp();
+        TimeStamp tC;
+        tC.set(2);
+
+        std::cout << "\n\n\n" << feat_ftAB_->getForcesMeas().transpose() << endl;
+        // WHY NOT SAME VALUE AS
+
+        // KFB_->getStateBlock("O")->unfix();
+        
+        KFC_ = createKFWithCDLBI(problem_, tC, x_origin_,
+                                 zero3, zero3, zero3, bias_p_, bias_imu_);
+        // Fix the one we cannot estimate
+        // KFB_->getP()->fix();
+        KFC_->getO()->fix();  // Not in the FT factor, so should be fixed (or absolute factor)
+        // KFB_->getV()->fix();
+        KFC_->getStateBlock("I")->fix();
+        KFC_->getStateBlock("B")->fix();
+
+        // ================ FACTOR INERTIAL KINEMATICS ON KFB
+        Vector3d pBC_measC = zero3; 
+        Vector3d vBC_measC = zero3;
+        Vector3d w_measC = zero3; 
+        Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
+        Matrix3d Iq; Iq.setIdentity(); 
+        Vector3d Lq = zero3; 
+
+        Matrix9d QkinC = computeKinCov(Qp_, Qv_, Qw_, pBC_measC-bias_p_, w_measC-bias_imu_.tail(3), Iq);
+        CaptureBasePtr cap_ikinB = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureInertialKinematics", tC, nullptr);
+        auto feat_BC = FeatureBase::emplace<FeatureInertialKinematics>(cap_ikinB, meas_ikinC, QkinC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+        auto feat_ikinBC = static_pointer_cast<FeatureInertialKinematics>(feat_BC); // !! auto feat_ikinB_ creates no compilation error but a segfault
+        auto fac_inkinBC = FactorBase::emplace<FactorInertialKinematics>(feat_ikinBC, feat_ikinBC, KFC_->getStateBlock("I"), nullptr, false);
+
+
+        // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
+        Vector3d f1;     f1 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
+        Vector3d tau1; tau1 << 0,0,0;
+        Vector3d pbl1; pbl1 << 0,0,0;
+        Vector4d bql1; bql1 << 0,0,0,1;
+        Vector3d f2;     f2 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
+        Vector3d tau2; tau2 << 0,0,0;
+        Vector3d pbl2; pbl2 << 0,0,0;
+        Vector4d bql2; bql2 << 0,0,0,1;
+        Vector3d pbc;   pbc << 0,0,0;
+        // aggregated meas
+        Vector6d f_meas; f_meas << f1, f2;
+        Vector6d tau_meas; tau_meas << tau1, tau2;
+        Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
+
+        Matrix6d cov_f =   1e-4 * Matrix6d::Identity(); 
+        Matrix6d cov_tau = 1e-4 * Matrix6d::Identity(); 
+        Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity(); 
+
+        CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC, nullptr);
+        auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
+                        tC - tB_, mass_,  
+                        f_meas, tau_meas, pbc, kin_meas,
+                        cov_f, cov_tau, cov_pbc);
+        FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, nullptr, false);
+    }
+};
 
 ////////////////////////////////////////////////////////
 // =============== TEST FONCTIONS ======================
@@ -363,7 +458,7 @@ TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
 }
 
 
-TEST_F(FactorInertialKinematics_2KF_FX,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
 {   
     string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
@@ -389,7 +484,7 @@ TEST_F(FactorInertialKinematics_2KF_FX,ZeroMvt)
 }
 
 
-TEST_F(FactorInertialKinematics_2KF_FY,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
 {   
     string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
@@ -414,6 +509,63 @@ TEST_F(FactorInertialKinematics_2KF_FY,ZeroMvt)
     ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
 }
 
+
+TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
+{   
+    string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+    perturbateAllIfUnFixed(KFA_);
+    perturbateAllIfUnFixed(KFB_);
+
+    problem_->print(4,true,true,true);
+    report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+    std::cout << report << std::endl;
+    problem_->print(4,true,true,true);
+
+    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, 0, Acc1/2).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, 0, Acc1).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+}
+
+TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
+{   
+    string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+    perturbateAllIfUnFixed(KFA_);
+    perturbateAllIfUnFixed(KFB_);
+    perturbateAllIfUnFixed(KFC_);
+
+    problem_->print(4,true,true,true);
+    report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+    std::cout << report << std::endl;
+    problem_->print(4,true,true,true);
+
+    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
+    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2, 0, 0).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFC_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2 + Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
+    ASSERT_MATRIX_APPROX(KFC_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);        // No acc btw B and C -> same vel
+    ASSERT_MATRIX_APPROX(KFC_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFC_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+}
+
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);