Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_) {
Expand Down
7 changes: 7 additions & 0 deletions sdk/include/branes/sdk/msckf_backend.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -202,6 +202,13 @@ class MsckfBackendT final : public VioBackend<T> {
}
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
Expand Down
84 changes: 84 additions & 0 deletions tests/sdk/filter_clock.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <catch2/catch_test_macros.hpp>

#include <cmath>
#include <span>
#include <vector>

Expand Down Expand Up @@ -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 <typename Feed>
double msckf_yaw_after_tie(Feed feed) {
using D = double;
bs::MsckfBackend<D> backend;
bs::VioConfig cfg;
cfg.max_clones = 8;
backend.initialize(cfg);
bs::ImuMeasurement<D> 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<bs::FrontendObservation<double>> none;
const auto camera = [&](bs::MsckfBackend<double>& b, double t) {
b.process_camera(t, std::span<const bs::FrontendObservation<double>>{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<double>;
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<ms::NormalizedObs<double>> 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<const ms::NormalizedObs<double>>{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);
}
Loading