diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp index d9f1e879453aa855db61864516662205c307475e..0c3d88e2360377b72242d1e9f153a5c07374f5d9 100644 --- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp +++ b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp @@ -146,6 +146,9 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test) data(6) = 1.952*2; MatrixXd data_cov = MatrixXd::Identity(12, 12); + // Set CoM to zero so that an acceleration on X does not produce a torque, thereby producing non-null 'L' at KF1 + S->getStateBlock('C')->setState(Vector3d::Zero()); + CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr); CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr); CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr); @@ -158,8 +161,11 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test) CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin()); + P->print(4,1,1,1); + + C_KF1->getBuffer().print(1,1,1,0,0); - //We check that, effectively, the dron has moved 1m in the x direction, the x component of the + //We check that, effectively, the drone has moved 1m in the x direction, the x component of the //velocity is now 2m/s and nothing else has changed from the original state ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), Vector3d(1,0,0), 1e-8); ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()), Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()), 1e-8); @@ -254,6 +260,6 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buf int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.not_moving_test"; + // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.not_moving_test"; return RUN_ALL_TESTS(); } \ No newline at end of file