diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 88dd905ea4b9aa9ca198900a2ded81a2bf15d7e5..d9ba3204e9bad27d772dc74fbdda7a75ebc2f3fb 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/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();
}