diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp
index 80969eaac78613f572686b624fab3837361c429f..9a932ac48572c4485d9fb885ba74b90f60468ab1 100644
--- a/src/test/gtest_IMU.cpp
+++ b/src/test/gtest_IMU.cpp
@@ -68,6 +68,9 @@ class Process_Constraint_IMU : public testing::Test
VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
VectorXs x0_optim; // optimized using constraint_imu
+ // Trajectory buffer of correction Jacobians
+ std::vector<MatrixXs> Buf_Jac_preint_prc;
+
// Trajectory matrices
MatrixXs Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
MatrixXs Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
@@ -81,7 +84,6 @@ class Process_Constraint_IMU : public testing::Test
bool p1_fixed, q1_fixed, v1_fixed;
-
virtual void SetUp( )
{
string wolf_root = _WOLF_ROOT_DIR;
@@ -246,7 +248,7 @@ class Process_Constraint_IMU : public testing::Test
J_D_b .setZero();
q = q0;
TimeStamp t(0);
-// trajectory.get().push_back(processor_imu->motionZero(t));
+ trajectory.get().emplace_back(t, Vector6s::Zero(), M6, VectorXs::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXs::Zero(9,6));
for (int n = 0; n < motion.cols(); n++)
{
t += dt;
@@ -365,7 +367,7 @@ class Process_Constraint_IMU : public testing::Test
// Integrate Trajectories all methods
virtual void integrateAllTrajectories()
{
- Size cols = motion.cols();
+ Size cols = motion.cols() + 1;
Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
@@ -375,10 +377,12 @@ class Process_Constraint_IMU : public testing::Test
// Build exact trajectories
int col = 0;
+ Scalar Dt = 0;
for (auto m : Buf_exact.get())
{
Trj_D_exact.col(col) = m.delta_integr_;
- Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, DT );
+ Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt );
+ Dt += dt;
col ++;
}
@@ -392,17 +396,18 @@ class Process_Constraint_IMU : public testing::Test
// Build trajectories preintegrated and corrected with imu_tools
col = 0;
+ Dt = 0;
for (auto m : Buf_preint_imu.get())
{
// preint
Trj_D_preint_imu.col(col) = m.delta_integr_;
- Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col), DT );
+ Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );
// corrected
- MatrixXs J_D_bias = m.jacobian_calib_;
- VectorXs step = J_D_bias * (bias_real - bias_preint);
+ VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
- Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col), DT );
+ Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
+ Dt += dt;
col ++;
}
@@ -422,21 +427,22 @@ class Process_Constraint_IMU : public testing::Test
// Build trajectories preintegrated and corrected with processorIMU
- col = -1;
+ Dt = 0;
+ col = 0;
+ Buf_Jac_preint_prc.clear();
for (auto m : Buf_D_preint_prc.get())
{
- if (col != -1)
- {
- // preint
- Trj_D_preint_prc.col(col) = m.delta_integr_;
- Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col), DT );
-
- // corrected
- MatrixXs J_D_bias = m.jacobian_calib_;
- VectorXs step = J_D_bias * (bias_real - bias_preint);
- Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
- Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col), DT );
- }
+ // preint
+ Trj_D_preint_prc.col(col) = m.delta_integr_;
+ Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );
+
+ // corrected
+ VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
+ Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
+ Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );
+
+ Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
+ Dt += dt;
col ++;
}
@@ -655,6 +661,24 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
sensor_odo->process(capture_odo);
}
+ virtual void integrateAllTrajectories() override
+ {
+ // ===================================== IMU
+ Process_Constraint_IMU::integrateAllTrajectories();
+
+ // ===================================== ODO
+ Vector6s data;
+ Vector3s p1 = x1_exact.head(3);
+ Quaternions q1 (x1_exact.data() + 3);
+ Vector3s dp = q0.conjugate() * (p1 - p0);
+ Vector3s dth = log_q( q0.conjugate() * q1 );
+ data << dp, dth;
+
+ CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
+
+ sensor_odo->process(capture_odo);
+ }
+
virtual void buildProblem() override
{
// ===================================== IMU
@@ -1421,9 +1445,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
a = Matrix<Scalar, 3, 50>::Random();
w = Matrix<Scalar, 3, 50>::Random();
- a = Matrix<Scalar,3,50>::Identity();
- w = Matrix<Scalar,3,50>::Identity();
-
// ---------- fix boundaries
p0_fixed = true;
q0_fixed = false;
@@ -1439,7 +1460,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
// ===================================== RUN ALL
string report = runAll(1);
- printAll(report);
+// printAll(report);
assertAll();
@@ -1466,11 +1487,8 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
bias_preint = -bias_real;
// ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- a = Matrix<Scalar,3,50>::Identity();
- w = Matrix<Scalar,3,50>::Identity();
+ a = Matrix<Scalar, 3, 50>::Ones() + 0.1 * Matrix<Scalar, 3, 50>::Random();
+ w = Matrix<Scalar, 3, 50>::Ones() + 0.1 * Matrix<Scalar, 3, 50>::Random();
// ---------- fix boundaries
p0_fixed = true;
@@ -1486,23 +1504,43 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
// ===================================== RUN ALL
configureAll();
-// integrateAllTrajectories();
- integrateAll();
+ integrateAllTrajectories();
buildProblem();
string report = solveProblem(1);
- printAll(report);
-
assertAll();
+ // Build optimal trajectory
+ MatrixXs Trj_x_optim(10,Trj_D_preint_prc.cols());
+ Scalar Dt = 0;
+ Size i = 0;
+ for (auto J_D_b : Buf_Jac_preint_prc)
+ {
+ VectorXs step = J_D_b * (bias_0 - bias_preint);
+ VectorXs D_opt = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
+ Trj_x_optim.col(i) = imu::composeOverState(x0_optim, D_opt, Dt);
+ Dt += dt;
+ i ++;
+ }
+
+ // printAll(report);
+
+ WOLF_INFO("------------------------------------");
+ WOLF_INFO("Exact x0 \n", x0.transpose());
+ WOLF_INFO("Optim x0 \n", x0_optim.transpose());
+ WOLF_INFO("Optim x \n", Trj_x_optim.transpose());
+ WOLF_INFO("Optim x1 \n", x1_optim.transpose());
+ WOLF_INFO("Exact x1 \n", x1_exact.transpose());
+ WOLF_INFO("------------------------------------");
+
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU.*";
+ // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU.*";
// ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.*";
- ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.*MotionRandom_PqV_b__pqV_b";
+ ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
return RUN_ALL_TESTS();
}