Use Sensor/ProcessorPose to account for pose measurements

test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp

@@ -38,6 +38,8 @@
 #include <core/ceres_wrapper/solver_ceres.h>
 #include <core/math/rotations.h>
 #include <core/tree_manager/tree_manager_sliding_window.h>
+#include <core/capture/capture_pose.h>
+
 
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
@@ -56,17 +58,27 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
 {
   public:
     ProblemPtr                              P;
+    SolverCeresPtr                          solver;
+    
+    // FTI sensor
     SensorForceTorqueInertialPtr            S;
     ProcessorForceTorqueInertialDynamicsPtr p;
-    SolverCeresPtr                          solver;
     Vector6d                                bias_true;
     Vector3d                                cdm_true;
     Vector3d                                inertia_true;
     double                                  mass_true;
-    std::vector<double>                     time_stamp;
+    
     double                                  dt;
+    
+    // Pose sensor
+    SensorBasePtr                           SP;
+    Vector7d                                pose;
+
+    // Reading csv data
+    std::vector<double>                     time_stamp;
     std::vector<Vector3d>                   position, vlin, vang, force, torque, a_meas;
     std::vector<Quaterniond>                quaternion;
+
     // Fitxer CSV
     std::fstream fout;
 
@@ -218,9 +230,12 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
         // solver->getSolverOptions().gradient_tolerance               = 1e-15;
         // solver->getSolverOptions().parameter_tolerance              = 1e-15;
 
-        S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+        // FTI sensor and processor
+        S = std::static_pointer_cast<SensorForceTorqueInertial>(P->findSensor("sensor FTI"));
         p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
 
+        // Pose sensor
+        SP = P->findSensor("sensor Pose");
     }
 };
 
@@ -380,11 +395,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
 
     int i_init = 0;
 
-    // fix measured position and orientation
-    p->getOrigin()->getFrame()->getStateBlock('P')->fix();
-    p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
-    p->getOrigin()->getFrame()->getStateBlock('O')->fix();
-    p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
+    // Pose
+    pose << position[i_init] , quaternion[i_init].coeffs();
+    CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
+    CP->process();
 
     unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
 
@@ -402,25 +416,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
         data.segment<3>(9) = torque[i];
         MatrixXd data_cov  = S->getNoiseCov();
 
+        pose << position[i] , quaternion[i].coeffs();
+
         // ESTIMATOR
 
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->process();
 
-        // check if new KF
+        CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
+        CP->process();
+
+        // check if new KF and solve
         if (last_kf_id != p->getOrigin()->getFrame()->id())
         {
             last_kf_id = p->getOrigin()->getFrame()->id();
 
-            // fix measured position and orientation
-            p->getOrigin()->getFrame()->getStateBlock('P')->fix();
-            p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
-            p->getOrigin()->getFrame()->getStateBlock('O')->fix();
-            p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
-
             // solve!
             report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
         
             // results of this iteration
             WOLF_INFO("Time                    : ", t, " s.");
@@ -434,12 +446,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
         }
     }
 
-    report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
-
-
     // FINAL RESULTS
-    WOLF_INFO(report);
     WOLF_INFO("True bias             * : ", bias_true.transpose(), " m/s2 - rad/s.");
     WOLF_INFO("Guess bias              : ", bias_guess.transpose(), " m/s2 - rad/s.");
     WOLF_INFO("Estimated bias          : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
@@ -455,7 +462,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
     WOLF_INFO("-----------------------------");
 
     ASSERT_MATRIX_APPROX(bias_true   , S->getStateBlock('I')->getState(), 0.11);
-    ASSERT_MATRIX_APPROX(cdm_true    , S->getStateBlock('C')->getState(), 1e-3);
+    ASSERT_MATRIX_APPROX(cdm_true    , S->getStateBlock('C')->getState(), 1e-2);
     ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
     ASSERT_NEAR         (mass_true   , S->getStateBlock('m')->getState()(0), 2e-2);
 }
@@ -493,27 +500,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
 
     // S->addPriorParameter('m',                                // mass
     //                      Vector1d(mass_guess),               // mass
-    //                      0.1 * 0.1 * Matrix1d::Identity());  // cov
+    //                      0.01 * 0.01 * Matrix1d::Identity());  // cov
 
-    // Force processor to make a KF at t=0
+    int i_init = 0;
+    
+    // Force FTI processor to make a KF at t=0
     CaptureMotionPtr C0 =
         std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
     C0->process();
 
-    int i_init = 0;
-
-    // fix measured position and orientation
-    p->getOrigin()->getFrame()->getStateBlock('P')->fix();
-    p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
-    p->getOrigin()->getFrame()->getStateBlock('O')->fix();
-    p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
-
-    unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+    // Pose
+    pose << position[i_init] , quaternion[i_init].coeffs();
+    CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
+    CP->process();
 
     for (int i = i_init + 1; i < time_stamp.size(); i++)  // start with second data
     {
         // SIMULATOR
-
         TimeStamp t = time_stamp[i];
 
         // Data
@@ -523,23 +526,18 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
         data.segment<3>(6) = force[i];
         data.segment<3>(9) = torque[i];
         MatrixXd data_cov  = S->getNoiseCov();
+        
+        pose << position[i] , quaternion[i].coeffs();
 
         // ESTIMATOR
 
+        // FTI
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->process();
 
-        // check if new KF
-        if (last_kf_id != p->getOrigin()->getFrame()->id())
-        {
-            last_kf_id = p->getOrigin()->getFrame()->id();
-
-            // fix measured position and orientation
-            p->getOrigin()->getFrame()->getStateBlock('P')->fix();
-            p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
-            p->getOrigin()->getFrame()->getStateBlock('O')->fix();
-            p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
-        }
+        // Pose
+        CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
+        CP->process();
     }
 
     std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
@@ -565,7 +563,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
     ASSERT_MATRIX_APPROX(bias_true   , S->getStateBlock('I')->getState(), 0.21);
     ASSERT_MATRIX_APPROX(cdm_true    , S->getStateBlock('C')->getState(), 2e-3);
     ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
-    ASSERT_NEAR         (mass_true   , S->getStateBlock('m')->getState()(0), 3e-2);
+    ASSERT_NEAR         (mass_true   , S->getStateBlock('m')->getState()(0), 4e-2);
 }
 
 int main(int argc, char** argv)

test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml

@@ -49,6 +49,19 @@ config:
     inertia:                      [0.015,0.015,0.030]      # moments of inertia i_xx, i_yy, i_zz [kg m2]
     mass:                         2.00                              # mass [kg]
 
+   - 
+    name: sensor Pose
+    type: SensorPose
+    plugin: core
+    extrinsic:
+      pose: [0,0,0, 0,0,0,1]
+    std_px: 0.001
+    std_py: 0.001
+    std_pz: 0.001
+    std_ox: 0.001
+    std_oy: 0.001
+    std_oz: 0.001
+
   processors:
    -
     name: "proc FTID"
@@ -67,3 +80,14 @@ config:
       max_buff_length:  999   # motion deltas
       dist_traveled:    999   # meters
       angle_turned:     999   # radians (1 rad approx 57 deg, approx 60 deg)
+  
+   - 
+    name: prc Pose
+    type: ProcessorPose
+    plugin: core
+    sensor_name: sensor Pose
+    time_tolerance: 0.01
+    keyframe_vote:
+      voting_active:    true
+    apply_loss_function: false
+