diff --git a/sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp b/sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp index 300141f..19cba3c 100644 --- a/sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp +++ b/sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp @@ -119,6 +119,12 @@ class InvariantVioBackend { /// than the time `set_nav` seeded: integrating those would run the seeded /// state forward over an interval that precedes it. void process_imu(const Vec3& gyro, const Vec3& accel, double t) { + if (have_last_ && t == last_imu_time_ && (!have_sample_ || last_sample_t_ < t)) { + // An image reached the clock first and held the previous sample up + // to it; this sample, new at the clock, starts the next interval. + remember_sample(gyro, accel, t); + return; + } if (have_last_ ? t <= last_imu_time_ : t < last_imu_time_) return; if (have_last_) { diff --git a/sdk/include/branes/sdk/msckf_backend.hpp b/sdk/include/branes/sdk/msckf_backend.hpp index 49b223f..3c493b9 100644 --- a/sdk/include/branes/sdk/msckf_backend.hpp +++ b/sdk/include/branes/sdk/msckf_backend.hpp @@ -202,6 +202,13 @@ class MsckfBackendT final : public VioBackend { } if (have_last_time_) { const double dt = imu.timestamp_s - last_imu_time_; + if (dt == 0.0 && (!have_sample_ || last_sample_t_ < imu.timestamp_s)) { + // An image reached the clock first and held the previous sample + // up to it; this sample, new at the clock, starts the next + // interval. Nothing to propagate. + remember_sample(gyro, accel, imu.timestamp_s); + return; + } if (!(dt > 0.0)) return; // drop out-of-order / duplicate samples — don't touch filter time // The interval from the filter clock to this sample, from both of its diff --git a/tests/sdk/filter_clock.cpp b/tests/sdk/filter_clock.cpp index c6da6ed..4439ead 100644 --- a/tests/sdk/filter_clock.cpp +++ b/tests/sdk/filter_clock.cpp @@ -16,6 +16,7 @@ #include +#include #include #include @@ -90,3 +91,86 @@ TEST_CASE("invariant VIO backend in float: the nav clock is the sample time, not vio.process_imu(gyro, accel, t + kDt); REQUIRE(vio.nav().timestamp == t + kDt); } + +// An image that reaches the clock before its coincident IMU sample (#618 +// review). The image holds the previous sample up to the clock; the sample that +// then arrives at the clock is new and must start the next interval, not be +// dropped as a duplicate. The gyro is w0 up to and at the tie and w1 after it, +// so the hold over the gap equals the interpolated interval, and image-first +// must match IMU-first to roundoff. Dropping the tie sample would start the next +// interval from the midpoint of w0 and w1, about 1e-3 rad off. +namespace { + +template +double msckf_yaw_after_tie(Feed feed) { + using D = double; + bs::MsckfBackend backend; + bs::VioConfig cfg; + cfg.max_clones = 8; + backend.initialize(cfg); + bs::ImuMeasurement m; + m.angular_velocity = {0.0, 0.0, 0.0}; + m.linear_acceleration = {0.0, 0.0, 9.81}; + double t = kT0; + for (int k = 0; k < 120; ++k) { + t = kT0 + kDt * k; + m.timestamp_s = t; + backend.process_imu(m); + } + REQUIRE(backend.initialized()); + feed(backend, m, t + kDt); // the tie, with the gyro still w0 = 0 + m.angular_velocity = {0.0, 0.0, 1.0}; // w1 + m.timestamp_s = t + 2 * kDt; + backend.process_imu(m); + return backend.current_state().T_world_imu.rotation().log()[2]; +} + +} // namespace + +TEST_CASE("MSCKF backend: an IMU sample that arrives after an image at its time is kept", + "[sdk][msckf][backend][clock]") { + const std::vector> none; + const auto camera = [&](bs::MsckfBackend& b, double t) { + b.process_camera(t, std::span>{none}); + }; + const double imu_first = msckf_yaw_after_tie([&](auto& b, auto m, double t) { + m.timestamp_s = t; + b.process_imu(m); + camera(b, t); + }); + const double image_first = msckf_yaw_after_tie([&](auto& b, auto m, double t) { + camera(b, t); + m.timestamp_s = t; + b.process_imu(m); + }); + REQUIRE(imu_first > 1e-3); // the step rotated: the test can see a dropped sample + REQUIRE(std::abs(image_first - imu_first) < 1e-12); +} + +TEST_CASE("invariant VIO backend: an IMU sample that arrives after an image at its time is kept", + "[sdk][riekf][backend][clock]") { + using Vio = ms::InvariantVioBackend; + using Vec3 = Vio::Vec3; + const Vec3 accel{{0.0, 0.0, 9.81}}; + const Vec3 w0{{0.0, 0.0, 0.0}}; + const Vec3 w1{{0.0, 0.0, 1.0}}; + const std::vector> none; + const auto run = [&](bool image_first) { + Vio vio; + vio.set_nav(Vio::SE23{}, Vec3{}, Vec3{}, kT0); + vio.process_imu(w0, accel, kT0 + kDt); + const double t_tie = kT0 + 2 * kDt; + const auto camera = [&] { vio.process_camera(t_tie, std::span>{none}); }; + if (image_first) + camera(); + vio.process_imu(w0, accel, t_tie); + if (!image_first) + camera(); + vio.process_imu(w1, accel, t_tie + kDt); + return vio.nav().X.rotation().log()[2]; + }; + const double imu_first = run(false); + const double image_first = run(true); + REQUIRE(imu_first > 1e-3); + REQUIRE(std::abs(image_first - imu_first) < 1e-12); +}