Solve 2 arcs with 12 KFs and self-calib

test/gtest_factor_diff_drive.cpp

@@ -379,9 +379,214 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
 
 }
 
+TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
+{
+
+    ProblemPtr problem = Problem::create("PO", 2);
+    CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+    // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+    IntrinsicsDiffDrivePtr intr = std::make_shared<IntrinsicsDiffDrive>();
+    intr->radius_left       = 1.0; // DO NOT MODIFY THESE VALUES !!!
+    intr->radius_right      = 1.0; // DO NOT MODIFY THESE VALUES !!!
+    intr->wheel_separation  = 1.0; // DO NOT MODIFY THESE VALUES !!!
+    intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+    Vector3s extr(0, 0, 0);
+    auto sen    = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+    auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+    auto calib_preint  = sensor->getIntrinsic()->getState();
+    Vector3s calib_gt(1,1,1);
+
+    // params and processor
+    ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
+    params->angle_turned    = 99;
+    params->dist_traveled   = 99;
+    params->max_buff_length = 99;
+    params->voting_active   = true;
+    params->max_time_span   = 1.5;
+    auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+    auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+
+    TimeStamp t(0.0);
+    Scalar dt = 1.0;
+    Vector3s x0(0,0,0);
+    Vector3s x1(1.5, -1.5, -M_PI/2.0);
+    Vector3s x2(3.0, -3.0, 0.0);
+    Matrix3s P0; P0.setIdentity();
+
+    auto F0 = problem->setPrior(x0, P0, t, 0.1);
+
+    // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
+    int N = 6;
+    Vector3s data;
+    Matrix2s data_cov; data_cov.setIdentity();
+
+    data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
+    data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+    for (int n = 0; n < N; n++)
+    {
+        t += dt;
+        C->setTimeStamp(t);
+        C->process();
+    }
+
+    // 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;
+        C->setTimeStamp(t);
+        C->process();
+    }
+
+    for (t = 0; t < (2*N+1)*dt; t += dt)
+    {
+        WOLF_TRACE("x(", t.getSeconds(), ") = ", problem->getState(t).transpose());
+    }
+
+
+    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
+    F0->fix();
+    F2->fix();
+    sensor->unfixIntrinsics();
+
+    Vector3s calib_pert = calib_gt + Vector3s::Random()*0.2;
+    sensor->getIntrinsic()->setState(calib_pert);
+
+    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    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("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);
+
+    std::cout << "\n\n" << std::endl;
+
+}
+
+TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
+{
+
+    ProblemPtr problem = Problem::create("PO", 2);
+    CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+    // make a sensor first // DO NOT MODIFY THESE VALUES !!!
+    IntrinsicsDiffDrivePtr intr = std::make_shared<IntrinsicsDiffDrive>();
+    intr->radius_left       = 0.95;
+    intr->radius_right      = 0.98;
+    intr->wheel_separation  = 1.05;
+    intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+    Vector3s extr(0, 0, 0);
+    auto sen    = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
+    auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+    auto calib_preint  = sensor->getIntrinsic()->getState();
+    Vector3s calib_gt(1.0, 1.0, 1.0); // ground truth calib
+
+    // params and processor
+    ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
+    params->angle_turned    = 99;
+    params->dist_traveled   = 99;
+    params->max_buff_length = 99;
+    params->voting_active   = true;
+    params->max_time_span   = 1.5;
+    auto prc = problem->installProcessor("DIFF DRIVE", "diff drive", sensor, params);
+    auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+
+
+    TimeStamp t(0.0);
+    Scalar dt = 1.0;
+    Vector3s x0(0,0,0);
+    Vector3s x1(1.5, -1.5, -M_PI/2.0);
+    Vector3s x2(3.0, -3.0, 0.0);
+    Matrix3s P0; P0.setIdentity();
+
+    auto F0 = problem->setPrior(x0, P0, t, 0.1);
+
+    // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
+    int N = 6;
+    Vector3s data;
+    Matrix2s data_cov; data_cov.setIdentity();
+
+    data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
+    data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+    for (int n = 0; n < N; n++)
+    {
+        t += dt;
+        WOLF_TRACE("ts: ", t);
+        C->setTimeStamp(t);
+        C->process();
+    }
+
+    // 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();
+    }
+
+    auto F2 = problem->getLastKeyFrame();
+
+    F2->setState(x2); // Impose known final state regardless of integrated value.
+
+    // Fix all but S ;
+    F0->fix();
+    F2->fix();
+    sensor->unfixIntrinsics();
+
+
+    WOLF_TRACE("\n  ========== SOLVE =========");
+    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+    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("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);
+
+}
+
 int main(int argc, char **argv)
 {
 testing::InitGoogleTest(&argc, argv);
+::testing::GTEST_FLAG(filter) = "FactorDiffDrive.*";
+//::testing::GTEST_FLAG(filter) = "FactorDiffDrive.preintegrate_and_solve_sensor_intrinsics";
 return RUN_ALL_TESTS();
 }