From d12f9682bd77c38f7279856fb7314c496d4c0bc6 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Tue, 9 Aug 2022 17:48:19 +0200
Subject: [PATCH] gtest modification
---
...problem_force_torque_inertial_dynamics.cpp | 161 +++++++++++++++++-
...e_torque_inertial_dynamics_solve_test.yaml | 2 +-
2 files changed, 159 insertions(+), 4 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index cc97b1c..066973c 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/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 <= 11.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
C->process();
@@ -284,7 +284,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
// 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 <= 7.0 ; t += 1.0)
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
@@ -305,9 +305,164 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
ASSERT_MATRIX_APPROX(cdm_estimated.head(2), cdm_true.head(2), 1e-6); // cdm_z is not observable while hovering
}
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hovering)
+{
+ S->getStateBlock('C')->setState(Vector3d(0.02, 0.02, 0.033));
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+
+ // 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
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
+ // 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()->fix();
+ C_KF0->getFrame()->getStateBlock('L')->unfix();
+ C_KF1->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->unfix();
+
+ 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 cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
+ C->process();
+ p->getOrigin()->getFrame()->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+ }
+
+
+ ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
+}
+
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_mom_hovering)
+{
+ 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')->fix();
+ 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()->fix();
+ C_KF0->getFrame()->getStateBlock('L')->unfix();
+ C_KF1->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->unfix();
+
+ 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 <= 60.0 ; t += 1.0)
+ {
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->setTimeStamp(t);
+ C->process();
+ p->getOrigin()->getFrame()->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
+ 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("-----------------------------");
+ }
+
+ 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.cdm_only_hovering";
+ ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.mass_and_cdm_and_ang_mom_hovering";
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 8eb8204..ca0697e 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -18,7 +18,7 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
plugin: "core"
- n_frames: 3
+ n_frames: 10
n_fix_first_frames: 1
viral_remove_empty_parent: true
map:
--
GitLab