diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
index 8a27323084822face6a6f3b1a68494a89c475d31..47433003618936c0871f558669aa9ffa3793867c 100644
--- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
@@ -62,7 +62,7 @@ class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Tes
}
};
-TEST(ProcessorForceTorqueInertialPreintDynamics_yaml, force_registraion)
+TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, force_registraion)
{
FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
}
@@ -72,68 +72,67 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = - gravity();
- // data.segment<3>(6) = - 1.952*gravity();
+ data.segment<3>(6) = - 1.952*gravity();
MatrixXd data_cov = MatrixXd::Identity(12, 12);
- CaptureMotionPtr C0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
- CaptureMotionPtr C2 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
- CaptureMotionPtr C3 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C4 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
- CaptureMotionPtr C5 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
-
- C0->process();
- CaptureMotionPtr C_Origin = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
-
- C1->process();
- C2->process();
- C3->process();
- C4->process();
- C5->process();
-
- P->print(4, 1, 1, 1);
- WOLF_INFO("State: ", P->getState("POVL").vector("POVL").transpose());
-
- ASSERT_MATRIX_APPROX(C5->getStateBlock('P')->getState(), C_Origin->getStateBlock('P')->getState(), 1e-8);
- ASSERT_QUATERNION_APPROX(Quaterniond(C5->getStateBlock('O')->getState().data()), Quaterniond(C_Origin->getStateBlock('O')->getState().data()), 1e-8);
- ASSERT_MATRIX_APPROX(C5->getStateBlock('V')->getState(), C_Origin->getStateBlock('V')->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(C5->getStateBlock('L')->getState(), C_Origin->getStateBlock('L')->getState(), 1e-8);
-
- ASSERT_EQ(C_Origin->getTimeStamp(), 0.0);
- ASSERT_EQ(C_Origin, C0);
- ASSERT_EQ(C5->getTimeStamp(), 1.0);
-
- Vector3d p0 = C_Origin->getStateBlock('P')->getState();
- Quaterniond o0 = Quaterniond(C_Origin->getStateBlock('O')->getState().data());
- Vector3d v0 = C_Origin->getStateBlock('V')->getState();
- Vector3d l0 = C_Origin->getStateBlock('L')->getState();
-
- Vector3d p1 = C5->getStateBlock('P')->getState();
- Quaterniond o1 = Quaterniond(C_Origin->getStateBlock('O')->getState().data());
- Vector3d v1 = C5->getStateBlock('V')->getState();
- Vector3d l1 = C5->getStateBlock('L')->getState();
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 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());
+
+
+ //We check that, effectively, the dron has not moved
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), C_KF0->getFrame()->getStateBlock('P')->getState(), 1e-8);
+ ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()), Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), C_KF0->getFrame()->getStateBlock('V')->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
- //ASSERT_MATRIX_APPROX(betweenStates())
+ //Checking that the captures we have taken are the correct ones by looking both time stands
+ ASSERT_EQ(C_KF0->getTimeStamp(), C0_0->getTimeStamp());
+ ASSERT_EQ(C_KF1->getTimeStamp(), C5_1->getTimeStamp());
- // inline void betweenStates(const MatrixBase<D1>& p1,
- // const MatrixBase<D2>& v1,
- // const MatrixBase<D5>& L1,
- // const QuaternionBase<D6>& q1,
- // const MatrixBase<D7>& p2,
- // const MatrixBase<D8>& v2,
- // const MatrixBase<D11>& L2,
- // const QuaternionBase<D12>& q2,
- // const T dt,
- // MatrixBase<D13>& dpi,
- // MatrixBase<D14>& dvi,
- // MatrixBase<D15>& dpd,
- // MatrixBase<D16>& dvd,
- // MatrixBase<D17>& dL,
- // QuaternionBase<D18>& dq)
+ Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
+ Quaterniond q0 = Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
+ Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
+ Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
+ Quaterniond q1 = Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
+ Vector3d dpi;
+ Vector3d dvi;
+ Vector3d dpd;
+ Vector3d dvd;
+ Vector3d dL;
+ Quaterniond dq;
+ double dt = 1.0;
+ VectorXd betw_states(19);
+
+ betweenStates(p0,v0,L0,q0,p1,v1,L1,q1,dt,dpi,dvi,dpd,dvd,dL,dq);
+
+ betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
+
+ VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+
+ //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
+ ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
}
@@ -143,6 +142,22 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
// VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
// data.segment<3>(0) = -gravity();
// data(0) = 2;
+// data.segment<3>(6) = - 1.952*gravity();
+// data(6) = 1.952*2;
+// MatrixXd data_cov = MatrixXd::Identity(12, 12);
+// 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);
+// CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 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();
+
+// P->print(4, 1, 1, 1);
+// WOLF_INFO("State: ", P->getState("POVL").vector("POVL").transpose());
+
// MatrixXd data_cov = MatrixXd::Identity(12, 12);