diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index b04d2ad5c468e4492c92570675857c8492d3f6ab..eef69b049765ca3b4c37e76abfdfae3d4e79ed5e 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/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();
}