added a new test for the processor ftipd and modified the voteforkeyframe function

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;
 }

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)
 {