Cleanup code

test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp

@@ -67,14 +67,15 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
     Vector3d                                cdm_true;
     Vector3d                                inertia_true;
     double                                  mass_true;
-    
-    double                                  dt;
+    Vector<double, 12>                      data_fti;
     
     // Pose sensor
     SensorBasePtr                           SP;
-    Vector7d                                pose;
+    Vector7d                                data_pose;
+
+    double                                  dt;
 
-    // Reading csv data
+    // Reading csv data_fti
     std::vector<double>                     time_stamp;
     std::vector<Vector3d>                   position, vlin, vang, force, torque, a_meas;
     std::vector<Quaterniond>                quaternion;
@@ -97,11 +98,11 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
 
         while (std::getline(data_simulation, line_data))
         {
-            std::stringstream data(line_data);
+            std::stringstream data_str(line_data);
 
             // time stamp
             string string_time_stamp;
-            std::getline(data, string_time_stamp, delimiter);
+            std::getline(data_str, string_time_stamp, delimiter);
             time_stamp.push_back(std::stod(string_time_stamp));
 
             // position
@@ -109,9 +110,9 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             string string_pos_y;
             string string_pos_z;
 
-            std::getline(data, string_pos_x, delimiter);
-            std::getline(data, string_pos_y, delimiter);
-            std::getline(data, string_pos_z, delimiter);
+            std::getline(data_str, string_pos_x, delimiter);
+            std::getline(data_str, string_pos_y, delimiter);
+            std::getline(data_str, string_pos_z, delimiter);
 
             position_i(0) = std::stod(string_pos_x);
             position_i(1) = std::stod(string_pos_y);
@@ -125,10 +126,10 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             string string_quat_z;
             string string_quat_w;
 
-            std::getline(data, string_quat_x, delimiter);
-            std::getline(data, string_quat_y, delimiter);
-            std::getline(data, string_quat_z, delimiter);
-            std::getline(data, string_quat_w, delimiter);
+            std::getline(data_str, string_quat_x, delimiter);
+            std::getline(data_str, string_quat_y, delimiter);
+            std::getline(data_str, string_quat_z, delimiter);
+            std::getline(data_str, string_quat_w, delimiter);
 
             Quaterniond quaternion_i(std::stod(string_quat_w),
                                      std::stod(string_quat_x),
@@ -142,9 +143,9 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             string string_vlin_y;
             string string_vlin_z;
 
-            std::getline(data, string_vlin_x, delimiter);
-            std::getline(data, string_vlin_y, delimiter);
-            std::getline(data, string_vlin_z, delimiter);
+            std::getline(data_str, string_vlin_x, delimiter);
+            std::getline(data_str, string_vlin_y, delimiter);
+            std::getline(data_str, string_vlin_z, delimiter);
 
             vlin_i(0) = std::stod(string_vlin_x);
             vlin_i(1) = std::stod(string_vlin_y);
@@ -157,9 +158,9 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             string string_vang_y;
             string string_vang_z;
 
-            std::getline(data, string_vang_x, delimiter);
-            std::getline(data, string_vang_y, delimiter);
-            std::getline(data, string_vang_z, delimiter);
+            std::getline(data_str, string_vang_x, delimiter);
+            std::getline(data_str, string_vang_y, delimiter);
+            std::getline(data_str, string_vang_z, delimiter);
 
             vang_i(0) = std::stod(string_vang_x);
             vang_i(1) = std::stod(string_vang_y);
@@ -172,9 +173,9 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             string string_force_y;
             string string_force_z;
 
-            std::getline(data, string_force_x, delimiter);
-            std::getline(data, string_force_y, delimiter);
-            std::getline(data, string_force_z, delimiter);
+            std::getline(data_str, string_force_x, delimiter);
+            std::getline(data_str, string_force_y, delimiter);
+            std::getline(data_str, string_force_z, delimiter);
 
             force_i(0) = std::stod(string_force_x);
             force_i(1) = std::stod(string_force_y);
@@ -187,9 +188,9 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
             string string_torque_y;
             string string_torque_z;
 
-            std::getline(data, string_torque_x, delimiter);
-            std::getline(data, string_torque_y, delimiter);
-            std::getline(data, string_torque_z, delimiter);
+            std::getline(data_str, string_torque_x, delimiter);
+            std::getline(data_str, string_torque_y, delimiter);
+            std::getline(data_str, string_torque_z, delimiter);
 
             torque_i(0) = std::stod(string_torque_x);
             torque_i(1) = std::stod(string_torque_y);
@@ -210,7 +211,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
     {
         bias_true    = Vector6d::Zero();
         cdm_true     = {0, 0, 0};
-        inertia_true = {0.0134943, 0.0141622, 0.0237319};  // rounded {0.017598, 0.017957, 0.029599}
+        inertia_true = {0.0134943, 0.0141622, 0.0237319};
         mass_true    = 1.9828;
 
         std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
@@ -317,23 +318,18 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
     p->getOrigin()->getFrame()->getStateBlock('L')->setState(ang_mom_init);
 
 
-    for (int i = i_init + 1; i < time_stamp.size(); i++)  // start with second data
+    for (int i = i_init + 1; i < time_stamp.size(); i++)  // start with second data_fti
     {
         // SIMULATOR
 
         TimeStamp t = time_stamp[i];
 
         // Data
-        VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
-        data.segment<3>(0) = a_meas[i];
-        data.segment<3>(3) = vang[i];
-        data.segment<3>(6) = force[i];
-        data.segment<3>(9) = torque[i];
-        MatrixXd data_cov  = S->getNoiseCov();
+        data_fti << a_meas[i], vang[i], force[i], torque[i];
 
         // ESTIMATOR
 
-        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
         C->process();
 
         // results printed in the csv in the order established before
@@ -396,8 +392,8 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
     int i_init = 0;
 
     // Pose
-    pose << position[i_init] , quaternion[i_init].coeffs();
-    CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
+    data_pose << position[i_init] , quaternion[i_init].coeffs();
+    CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
     CP->process();
 
     unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
@@ -409,21 +405,15 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
         TimeStamp t = time_stamp[i];
 
         // Data
-        VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
-        data.segment<3>(0) = a_meas[i];
-        data.segment<3>(3) = vang[i];
-        data.segment<3>(6) = force[i];
-        data.segment<3>(9) = torque[i];
-        MatrixXd data_cov  = S->getNoiseCov();
-
-        pose << position[i] , quaternion[i].coeffs();
+        data_fti  << a_meas[i], vang[i], force[i], torque[i];
+        data_pose << position[i] , quaternion[i].coeffs();
 
         // ESTIMATOR
 
-        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
         C->process();
 
-        CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
+        CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, data_pose, SP->getNoiseCov());
         CP->process();
 
         // check if new KF and solve
@@ -506,12 +496,12 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
     
     // 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);
+        std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), S->getNoiseCov(), nullptr);
     C0->process();
 
     // Pose
-    pose << position[i_init] , quaternion[i_init].coeffs();
-    CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, pose, SP->getNoiseCov());
+    data_pose << position[i_init] , quaternion[i_init].coeffs();
+    CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
     CP->process();
 
     for (int i = i_init + 1; i < time_stamp.size(); i++)  // start with second data
@@ -520,23 +510,17 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
         TimeStamp t = time_stamp[i];
 
         // Data
-        VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
-        data.segment<3>(0) = a_meas[i];
-        data.segment<3>(3) = vang[i];
-        data.segment<3>(6) = force[i];
-        data.segment<3>(9) = torque[i];
-        MatrixXd data_cov  = S->getNoiseCov();
-        
-        pose << position[i] , quaternion[i].coeffs();
+        data_fti  << a_meas[i], vang[i], force[i], torque[i];
+        data_pose << position[i] , quaternion[i].coeffs();
 
         // ESTIMATOR
 
-        // FTI
-        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+        // fti
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data_fti, S->getNoiseCov(), nullptr);
         C->process();
 
         // Pose
-        CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, pose, SP->getNoiseCov());
+        CaptureBasePtr CP = std::make_shared<CapturePose>(t, SP, data_pose, SP->getNoiseCov());
         CP->process();
     }