Document tests of preint and solve

test/gtest_factor_diff_drive.cpp

@@ -382,6 +382,41 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
 TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
 {
 
+    /*
+     *  The trajectory is an S-shape of two 90-deg arcs of radius 1.5m.
+     *  Passage poses are:
+     *      x0: Initial:  0 ,   0  ,  0
+     *      x1: Half   : 1.5, -1.5 , -pi/2
+     *      x2: Final  :  3 ,  -3  ,  0
+     *
+     *       x0      1.5       3
+     *       *> ------+--------+------> X
+     *       |    *
+     *       |
+     *       |       *
+     *       |
+     *  -1.5 +        * x1
+     *       |        v
+     *       |         *
+     *       |
+     *       |            *    x2
+     *    -3 +                 *>
+     *       |
+     *       v
+     *       -Y
+     *
+     *  Notes:
+     *   - We make two arcs because the intrinsics are not observable with just one arc.
+     *   - The deltas are pre-integrated with ground truth values of the intrinsics. They are perturbed before solving.
+     *     - From x0 to x1 we integrate 6 identical deltas. There are 2 intermediate keyframes.
+     *     - From x1 to x2 we integrate 6 identical deltas, turning to the other side as the first 6. There are 2 intermediate keyframes.
+     *     - The total of deltas is 12.
+     *   - The total of keyframes is 7.
+     *     - Keyframes x0 and x2 are fixed to provide the boundary conditions.
+     *     - The other 5 keyframes including x1 are estimated.
+     *   - The sensor intrinsics are also estimated.
+     */
+
     ProblemPtr problem = Problem::create("PO", 2);
     CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
 
@@ -454,30 +489,33 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
     auto F2 = problem->getLastKeyFrame(); // this matches the end of the arc precisely so it may be used for checks.
 
 
-    // Fix all but S ; perturb Sensor ; solve ; print and assert values of Sensor
+    // Fix boundaries and unfix S ;
     F0->fix();
     F2->fix();
     sensor->unfixIntrinsics();
 
+    // Perturb S
     Vector3s calib_pert = calib_gt + Vector3s::Random()*0.2;
     sensor->getIntrinsic()->setState(calib_pert);
 
+    WOLF_TRACE("\n  ========== SOLVE =========");
     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+    WOLF_TRACE("\n", report);
 
-    WOLF_TRACE("\n  ========== SOLVED =========");
     for (t = 0; t < (2*N+1)*dt; t += dt)
     {
         WOLF_TRACE("x(", t.getSeconds(), ") = ", problem->getState(t).transpose());
     }
-    WOLF_TRACE("F1           : ", problem->getState(N*dt).transpose());
-    WOLF_TRACE("F2           : ", F2->getState().transpose());
+    WOLF_TRACE("x1           : ", problem->getState(N*dt).transpose());
+    WOLF_TRACE("x2           : ", F2->getState().transpose());
     WOLF_TRACE("calib_preint : ", calib_preint.transpose());
     WOLF_TRACE("calib_pert   : ", calib_pert.transpose());
     WOLF_TRACE("calib_est    : ", sensor->getIntrinsic()->getState().transpose());
     WOLF_TRACE("calib_gt     : ", calib_gt.transpose());
 
     ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-6);
-    ASSERT_MATRIX_APPROX(problem->getCurrentState(), x2, 1e-6);
+    ASSERT_MATRIX_APPROX(problem->getState(  N*dt), x1, 1e-6);
+    ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
 
     std::cout << "\n\n" << std::endl;
 
@@ -485,6 +523,42 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
 
 TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 {
+    /*
+     *  The trajectory is an S-shape of two 90-deg arcs of radius 1.5m.
+     *  Passage poses are:
+     *      x0: Initial:  0 ,   0  ,  0
+     *      x1: Half   : 1.5, -1.5 , -pi/2
+     *      x2: Final  :  3 ,  -3  ,  0
+     *
+     *       x0      1.5       3
+     *       *> ------+--------+------> X
+     *       |    *
+     *       |
+     *       |       *
+     *       |
+     *  -1.5 +        * x1
+     *       |        v
+     *       |         *
+     *       |
+     *       |            *    x2
+     *    -3 +                 *>
+     *       |
+     *       v
+     *       -Y
+     *
+     *  Notes:
+     *   - We make two arcs because the intrinsics are not observable with just one arc.
+     *   - The deltas are pre-integrated with wrong values of the intrinsics. The true values must be found by solving.
+     *     - From x0 to x1 we integrate 6 identical deltas. There are 2 intermediate keyframes.
+     *     - From x1 to x2 we integrate 6 identical deltas, turning to the other side as the first 6. There are 2 intermediate keyframes.
+     *     - The total of deltas is 12.
+     *   - The total of keyframes is 7.
+     *     - Keyframes x0 and x2 are fixed to provide the boundary conditions.
+     *     - The other 5 keyframes including x1 are estimated.
+     *   - The sensor intrinsics are also estimated.
+     */
+
+
 
     ProblemPtr problem = Problem::create("PO", 2);
     CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
@@ -534,7 +608,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
     for (int n = 0; n < N; n++)
     {
         t += dt;
-        WOLF_TRACE("ts: ", t);
         C->setTimeStamp(t);
         C->process();
     }
@@ -542,11 +615,12 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
     // left turn 90 deg in N steps --> ends up in (3, -3, 0)
     data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
     data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+
     C->setData(data);
+
     for (int n = 0; n < N; n++)
     {
         t += dt;
-        WOLF_TRACE("ts: ", t);
         C->setTimeStamp(t);
         C->process();
     }
@@ -555,37 +629,37 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
     F2->setState(x2); // Impose known final state regardless of integrated value.
 
-    // Fix all but S ;
+    // Fix boundaries and unfix S ;
     F0->fix();
     F2->fix();
     sensor->unfixIntrinsics();
 
 
     WOLF_TRACE("\n  ========== SOLVE =========");
-    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
-
+    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
     WOLF_TRACE("\n", report);
 
     for (t = 0; t < (2*N+1)*dt; t += dt)
     {
         WOLF_TRACE("x(", t.getSeconds(), ") = ", problem->getState(t).transpose());
     }
-    WOLF_TRACE("F1           : ", problem->getState(N*dt).transpose());
-    WOLF_TRACE("F2           : ", F2->getState().transpose());
+    WOLF_TRACE("x1           : ", problem->getState(N*dt).transpose());
+    WOLF_TRACE("x2           : ", F2->getState().transpose());
     WOLF_TRACE("calib_preint : ", calib_preint.transpose());
     WOLF_TRACE("calib_est    : ", sensor->getIntrinsic()->getState().transpose());
     WOLF_TRACE("calib_GT     : ", calib_gt.transpose());
 
 
     ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
-    ASSERT_MATRIX_APPROX(problem->getCurrentState(), x2, 1e-6);
+    ASSERT_MATRIX_APPROX(problem->getState(  N*dt), x1, 1e-2);
+    ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
 
 }
 
 int main(int argc, char **argv)
 {
 testing::InitGoogleTest(&argc, argv);
-::testing::GTEST_FLAG(filter) = "FactorDiffDrive.*";
+//::testing::GTEST_FLAG(filter) = "FactorDiffDrive.*";
 //::testing::GTEST_FLAG(filter) = "FactorDiffDrive.preintegrate_and_solve_sensor_intrinsics";
 return RUN_ALL_TESTS();
 }