diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 0f66c36204298dd6631965ab6ea5f337ef7175e0..b407b3123447b3f7fd38bafd32c5365e81ef0d8f 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -217,17 +217,20 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
return true;
}
// dist_traveled
- // if (getBuffer().back() > params_force_torque_inertial_preint_dynamics_->dist_traveled)
- // {
- // WOLF_DEBUG( "PM: vote: dist traveled" );
- // return true;
- // }
- // angle_turned
- // if (getBuffer().back() > params_force_torque_inertial_preint_dynamics_->angle_turned)
- // {
- // WOLF_DEBUG( "PM: vote: angle turned" );
- // return true;
- // }
+ double dist = getBuffer().back().delta_integr_.head<3>().norm();
+ if (dist > params_force_torque_inertial_preint_dynamics_->dist_traveled)
+ //li fem cas a la delta_integr que dona la imu? o fem, per exemple, una mitjana amb la delta_integr que ve de la dinà mica?
+ {
+ WOLF_DEBUG( "PM: vote: distance traveled" );
+ return true;
+ }
+ // angle turned
+ double angle = 2.0 * acos(getMotion().delta_integr_(15));
+ if (angle > params_force_torque_inertial_preint_dynamics_->angle_turned)
+ {
+ WOLF_DEBUG( "PM: vote: angle turned" );
+ return true;
+ }
return false;
}
diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
index 47433003618936c0871f558669aa9ffa3793867c..6fc1430582d776c268bc356bf159b87fce2ccd87 100644
--- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
@@ -136,32 +136,67 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
}
-// Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
-// TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_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);
-
-
-// }
+//Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
+TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_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();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+
+ //We check that, effectively, the dron 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);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), Vector3d(2,0,0), 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
+
+
+
+ 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);
+
+}
int main(int argc, char **argv)
{