diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp index b3862ed88e9e5289f293eac251a625ba3a622c0c..a08c22e7b3ce410a7f686e898ecac31d4160b5a4 100644 --- a/src/test/gtest_IMU.cpp +++ b/src/test/gtest_IMU.cpp @@ -1472,7 +1472,7 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // // ---------- time t0 = 0; - num_integrations = 50; + num_integrations = 25; // ---------- initial pose p0 << 0,0,0; @@ -1484,8 +1484,8 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) bias_preint = -bias_real; // ---------- motion params - 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(); + a = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random(); + w = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random(); // ---------- fix boundaries p0_fixed = true; @@ -1520,14 +1520,31 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) i ++; } + i = 0; + t = 0; + MatrixXs Trj_x_optim_prc(10,Trj_D_preint_prc.cols()); + for (int i = 0; i < Trj_x_optim_prc.cols(); i++) + { + Trj_x_optim_prc.col(i) = problem->getState(t); + t += dt; + } + // printAll(report); WOLF_INFO("------------------------------------"); - WOLF_INFO("Exact x0 \n", x0.transpose()); - WOLF_INFO("Optim x0 \n", x0_optim.transpose()); + 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("Optim x1 \n", x1_optim .transpose()); + WOLF_INFO("Exact x1 \n", x1_exact .transpose()); + WOLF_INFO("------------------------------------"); + + WOLF_INFO("------------------------------------"); + WOLF_INFO("Exact x0 \n", x0 .transpose()); + WOLF_INFO("Optim x0 \n", x0_optim .transpose()); + WOLF_INFO("Optim_prc x \n", Trj_x_optim_prc.transpose()); + WOLF_INFO("Optim x1 \n", x1_optim .transpose()); + WOLF_INFO("Exact x1 \n", x1_exact .transpose()); WOLF_INFO("------------------------------------"); }