Fix all. Print full optimized trajectory

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();
 }