Check trajectory produced by getState()

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,6 +1520,15 @@ 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("------------------------------------");
@@ -1530,6 +1539,14 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
     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("------------------------------------");
+
 }
 
 int main(int argc, char **argv)