new test unfixing blocks

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -205,7 +205,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 22.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
         C->process();
@@ -359,7 +359,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 30.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
         C->process();
@@ -440,7 +440,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
     // which if not eliminated contaminate the overall solution.
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 26.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->setTimeStamp(t);
@@ -463,9 +463,102 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
     ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
 }
 
+//Here we only fix P and O from each key frame
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_fixing)
+{
+    S->getStateBlock('C')->setState(Vector3d(0.05, 0.05, 0.05));
+    S->getStateBlock('m')->setState(Vector1d(1.50));
+    Vector3d cdm_guess  = S->getStateBlock('C')->getState();
+    double   mass_guess = S->getStateBlock('m')->getState()(0);
+
+    // Data
+    VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
+    data.segment<3>(0) = -gravity();
+    data.segment<3>(6) = -mass_true * gravity();
+    MatrixXd data_cov  = MatrixXd::Identity(12, 12);
+
+    // We fix the parameters of the sensor except for the cdm and the mass
+    S->getStateBlock('P')->fix();
+    S->getStateBlock('O')->fix();
+    S->getStateBlock('I')->unfix();
+    S->getStateBlock('C')->unfix();
+    S->getStateBlock('i')->fix();
+    S->getStateBlock('m')->unfix();
+    // S->setStateBlockDynamic('I');
+
+    CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+    CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+    CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+    CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+    CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
+
+    C0_0->process();
+    CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+    C1_0->process();
+    C2_0->process();
+    C3_0->process();
+    C4_0->process();
+    C5_1->process();
+
+    CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+    C_KF0->getFrame()->unfix();
+    C_KF0->getFrame()->getStateBlock('P')->fix();
+    C_KF0->getFrame()->getStateBlock('O')->fix();
+    C_KF1->getFrame()->unfix();
+    C_KF1->getFrame()->getStateBlock('P')->fix();
+    C_KF1->getFrame()->getStateBlock('O')->fix();
+
+    P->print(4, 1, 1, 1);
+
+    WOLF_INFO("======== SOLVE PROBLEM =======")
+    std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+    WOLF_INFO(report);  // should show a very low iteration number (possibly 1)
+
+    WOLF_INFO("True mass     : ", mass_true, " Kg.");
+    WOLF_INFO("True cdm      : ", cdm_true.transpose(), " m.");
+    WOLF_INFO("Guess mass    : ", mass_guess, " Kg.");
+    WOLF_INFO("Guess cdm     : ", cdm_guess.transpose(), " m.");
+    WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+    WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+    WOLF_INFO("-----------------------------");
+
+
+    // Do a number of iterations with more keyframes, solving so that the calibration gets better and better
+    // Here we take advantage of the sliding window, thereby getting rid progressively 
+    // of the old factors, which contained calibration parameters far from the converged values,
+    // which if not eliminated contaminate the overall solution.
+    // This is due to these first factors relying on the linearization Jacobian to correct the
+    // poorly preintegrated delta.
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 30.0 ; t += 1.0)
+    {
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+        C->setTimeStamp(t);
+        C->process(); //En aquesta línia el programa s'aborta
+        p->getOrigin()->getFrame()->unfix();
+        p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+        p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+        report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+        WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+        WOLF_INFO("Estimated ang mom : ", p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(), " m^2 kg/s.");
+
+        WOLF_INFO("-----------------------------");
+    }
+
+    auto cdm_estimated  = S->getStateBlock('C')->getState();
+    auto mass_estimated = S->getStateBlock('m')->getState()(0);
+
+
+    ASSERT_NEAR(mass_estimated, mass_true, 1e-6);
+    ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
+}
+
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    //::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.mass_and_cdm_and_ang_mom_hovering";
+    //::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.hovering_test_without_fixing";
     return RUN_ALL_TESTS();
 }

test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml

@@ -18,7 +18,7 @@ config:
     tree_manager: 
       type: "TreeManagerSlidingWindow"
       plugin: "core"
-      n_frames: 10
+      n_frames: 3
       n_fix_first_frames: 1
       viral_remove_empty_parent: true
   map: