From 932a8600b9d77d43b27d16be8160a1d9580d1ebf Mon Sep 17 00:00:00 2001 From: Theodore Omtzigt Date: Mon, 9 Nov 2026 19:01:22 -0500 Subject: [PATCH] fix(sdk): keep an imu sample that arrives after an image at its time When an image reaches the filter clock before the IMU sample that shares its timestamp, the image holds the previous sample up to the clock and the sample then arrives with dt = 0. Both backends dropped it as a duplicate, so the next interval started from the rate interpolated between the older and the later sample instead of from the sample itself, breaking the two-endpoint convention of #616. Both now remember a sample new at the clock without propagating again, and still reject true duplicates. EuRoC's replay delivers IMU samples at or before a frame first, so no recorded number changes. The tests compare image-first with IMU-first delivery; without the fix they differ by 1.25e-3 rad. Raised in review of #618. Co-Authored-By: Claude Opus 5.5 --- .../sdk/msckf/invariant_vio_backend.hpp | 6 ++ sdk/include/branes/sdk/msckf_backend.hpp | 7 ++ tests/sdk/filter_clock.cpp | 84 +++++++++++++++++++ 3 files changed, 97 insertions(+) diff --git a/sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp b/sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp index 300141f4..19cba3c4 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 49b223f8..3c493b90 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 c6da6edc..4439eadc 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); +}