From 923b6914bdcca215de8123bac578275c2b199216 Mon Sep 17 00:00:00 2001 From: Theodore Omtzigt Date: Tue, 30 Jun 2026 09:39:25 -0400 Subject: [PATCH 1/3] feat(tools): drive R-IEKF through the real EuRoC pipeline MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds the end-to-end harness that runs the right-invariant (R-IEKF) VIO backend (#347/#348) through the validated VioEstimator front-end / track manager on real EuRoC, so it can be compared head-to-head with the standard MSCKF instead of the confounded hand-rolled Monte-Carlo loop. - invariant_backend_adapter.hpp: a VioBackend-conforming wrapper that bridges InvariantVioBackend's minimal API (set_nav / process_imu / process_camera-normalized / nav) to the VioEstimator contract. Boot- straps from a static, gravity-aligned IMU window; unprojects pixels to bearings; maps SE2(3) back to NavState. Uses the Joseph FullCovariance form (the backend default) — the square-root form diverges on real data. - vio_pipeline: a --invariant path (run_euroc_invariant) mirroring the standard EuRoC run, reporting ATE + mean position sigma so the two backends are measured on the same sequence with the same front end. - msckf_invariant_backend: guard the update against a non-finite dx — snapshot the covariance and reject the track (it never happened) rather than abort in SO3::normalize on a degenerate real-data measurement. Result on V1_01 / V2_03: the R-IEKF now runs stably end to end, but underperforms the standard MSCKF on both ATE (1.3 m vs 0.27 m) and position-sigma consistency. The position-norm ATE/sigma ratio cannot isolate the yaw-gauge effect the R-IEKF targets (standard MSCKF is already ~calibrated at 2.1 on position); a gauge-anchored attitude-NEES metric is the needed next step (the #365 acceptance criterion) and is a follow-up. Relates to #365 Co-Authored-By: Claude Opus 4.8 (1M context) --- .../sdk/msckf/msckf_invariant_backend.hpp | 14 ++ tests/tools/invariant_backend_adapter.cpp | 101 ++++++++++ .../tools/invariant_backend_adapter.hpp | 184 ++++++++++++++++++ tools/src/vio_pipeline.cpp | 116 ++++++++++- 4 files changed, 411 insertions(+), 4 deletions(-) create mode 100644 tests/tools/invariant_backend_adapter.cpp create mode 100644 tools/include/branes/tools/invariant_backend_adapter.hpp diff --git a/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp b/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp index bbcc253..08ad89b 100644 --- a/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp +++ b/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -238,7 +239,20 @@ class MsckfInvariantBackend { return false; } const std::vector R_diag(proj.rows, r2); + // A filter must never apply a non-finite correction, nor let a pathological + // update poison the covariance. The χ² gate (cholesky) rejects a non-PD S, + // but on real data a barely-conditioned sqrt-form update can still emit a + // non-finite δx; snapshot, and on any non-finite component restore the + // covariance and reject the track (it never happened). Keeps the estimator + // alive on a degenerate measurement instead of aborting in SO3::normalize. + const Cov cov_snapshot = cov_; const std::vector dx = cov_.update(H, std::span{proj.r}, std::span{R_diag}); + for (const T v : dx) { + if (!std::isfinite(static_cast(v))) { + cov_ = cov_snapshot; + return false; + } + } retract(dx); return true; diff --git a/tests/tools/invariant_backend_adapter.cpp b/tests/tools/invariant_backend_adapter.cpp new file mode 100644 index 0000000..18d43c2 --- /dev/null +++ b/tests/tools/invariant_backend_adapter.cpp @@ -0,0 +1,101 @@ +// SPDX-License-Identifier: MIT +// +// Gate for the R-IEKF EuRoC adapter (branes/tools/invariant_backend_adapter.hpp, +// #347/#348). The full end-to-end comparison runs in vio_pipeline over EuRoC +// (not vendored in CI); here we pin the adapter's contract bridges that the +// harness relies on, on synthetic input: +// +// * it bootstraps from a static IMU window (gravity-aligned) — initialized() +// flips true only after enough samples, and the seeded attitude levels the +// measured gravity (roll/pitch recovered, zero velocity); +// * it satisfies the VioEstimator duck-typed contract (process_imu / +// process_camera / current_state) and maps the SE2(3) nav back to a NavState. +// +// ASCII-only TEST_CASE names (the Windows MSVC ctest job rejects non-ASCII). + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace bt = branes::tools; +namespace bs = branes::sdk; +using Catch::Matchers::WithinAbs; + +namespace { +using T = double; +using Adapter = bt::InvariantBackendAdapter; +using SO3 = branes::math::lie::SO3; +using Vec3 = branes::math::lie::detail::Vec; + +Adapter make_adapter() { + // EuRoC-like intrinsics; identity extrinsic keeps the test about the bridge. + Adapter::Camera cam(458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05); + return Adapter(cam, SO3{}, Vec3{}); +} + +bs::ImuMeasurement stationary_sample(double t) { + bs::ImuMeasurement m; + m.timestamp_s = t; + m.angular_velocity = {0.0, 0.0, 0.0}; + m.linear_acceleration = {0.0, 0.0, 9.81}; // at rest: specific force = +g (world up) + return m; +} +} // namespace + +TEST_CASE("invariant adapter: bootstraps from a static IMU window", "[tools][invariant_adapter]") { + Adapter a = make_adapter(); + bs::VioConfig cfg; + a.initialize(cfg); + REQUIRE_FALSE(a.initialized()); + + // Feed a static window; init should fire once enough samples accumulate. + for (int i = 0; i < 200; ++i) + a.process_imu(stationary_sample(0.005 * i)); + REQUIRE(a.initialized()); + + const auto ns = a.current_state(); + // Zero-velocity seed. + REQUIRE_THAT(ns.velocity_world[0], WithinAbs(0.0, 1e-9)); + REQUIRE_THAT(ns.velocity_world[1], WithinAbs(0.0, 1e-9)); + REQUIRE_THAT(ns.velocity_world[2], WithinAbs(0.0, 1e-9)); + // Gravity-aligned attitude: rotating the measured specific force (+z body) into + // world should land on world up (+z), i.e. the seed levels roll/pitch. + const Vec3 up_body{{0.0, 0.0, 1.0}}; + const Vec3 up_world = ns.T_world_imu.rotation() * up_body; + REQUIRE_THAT(up_world[2], WithinAbs(1.0, 1e-6)); +} + +TEST_CASE("invariant adapter: satisfies the VioEstimator backend contract", "[tools][invariant_adapter]") { + Adapter a = make_adapter(); + a.initialize(bs::VioConfig{}); + for (int i = 0; i < 200; ++i) + a.process_imu(stationary_sample(0.005 * i)); + REQUIRE(a.initialized()); + + // process_camera takes pixel FrontendObservations; with no real parallax the + // update may decline, but the call path (unprojection → backend) must be safe. + std::vector> obs; + for (int k = 0; k < 4; ++k) { + bs::FrontendObservation o; + o.feature_id = static_cast(k); + o.camera_id = 0; + o.u = 300.0 + 40.0 * k; + o.v = 240.0; + obs.push_back(o); + } + a.process_camera(1.0, std::span>{obs}); + + // covariance() is a square PSD-sized matrix; current_state() stamp advances. + const auto P = a.covariance(); + REQUIRE(P.rows == P.cols); + REQUIRE(P.rows >= 15); + REQUIRE(a.current_state().timestamp_s > 0.0); +} diff --git a/tools/include/branes/tools/invariant_backend_adapter.hpp b/tools/include/branes/tools/invariant_backend_adapter.hpp new file mode 100644 index 0000000..b695cd5 --- /dev/null +++ b/tools/include/branes/tools/invariant_backend_adapter.hpp @@ -0,0 +1,184 @@ +// SPDX-License-Identifier: MIT +// +// branes/tools/invariant_backend_adapter.hpp — adapts the right-invariant +// (R-IEKF) VIO backend to the VioEstimator contract so it can run +// through the real EuRoC pipeline and be compared against the standard MSCKF +// backend (#347/#348, the yaw-leak fix; validated at the Jacobian level by +// obs_inspect, this is the end-to-end harness). +// +// InvariantVioBackend has a deliberately minimal, parameterization-honest API +// (set_nav / process_imu(gyro,accel,t) / process_camera(t, NormalizedObs) / +// nav() / covariance()) and no auto-init. VioEstimator instead wants +// initialize(VioConfig) + process_imu(ImuMeasurement) + process_camera(t, +// FrontendObservation-in-PIXELS) + current_state(). This thin adapter bridges +// the two: +// • init — accumulates IMU until a static / gravity-aligned bootstrap succeeds +// (the same ImuInitializer the standard backend uses), then set_nav() with a +// zero-velocity, origin seed (the dynamic-VI scale path was measured to wash +// out, so a gravity-aligned static seed is the fair, simple choice); +// • observations — unprojects each pixel through the camera model to the +// normalized bearing the invariant backend expects; +// • state — maps the SE₂(3) nav back to a NavState (T_world_imu + velocity). +// +// Lives in tools/ (not sdk/) — it is a validation harness, not a shipped path, +// and makes NO change to the load-bearing filter. Header-only, C++20. + +#ifndef BRANES_TOOLS_INVARIANT_BACKEND_ADAPTER_HPP +#define BRANES_TOOLS_INVARIANT_BACKEND_ADAPTER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace branes::tools { + +/// Drop-in `Backend` for `VioEstimator>` that runs +/// the right-invariant filter on the real pipeline. +template +class InvariantBackendAdapter { +public: + // Joseph full-covariance form — the invariant backend's default and the same + // representation the standard MSCKF uses on real data. The square-root form + // (run_synthetic_invariant's choice) is numerically finicky and diverges on + // real EuRoC; the Joseph form is the robust one for this harness. + using Cov = sdk::msckf::FullCovariance; + using Impl = sdk::msckf::InvariantVioBackend; + using Camera = math::cameras::PinholeRadtanCamera; + using Vec3 = math::lie::detail::Vec; + using Vec2 = math::lie::detail::Vec; + using SO3 = math::lie::SO3; + using SE3 = math::lie::SE3; + using SE23 = math::lie::SE23; + using NormalizedObs = sdk::msckf::NormalizedObs; + + /// Built with the EuRoC cam0 calibration: intrinsics (to unproject pixels) + + /// the camera↔IMU extrinsic (handed to the invariant backend). + InvariantBackendAdapter(Camera cam, SO3 R_imu_cam, Vec3 p_imu_cam) + : cam_(cam), R_imu_cam_(R_imu_cam), p_imu_cam_(p_imu_cam) {} + + // ── VioEstimator contract ──────────────────────────────────── + + void initialize(const sdk::VioConfig& cfg) { + typename Impl::Config icfg; + icfg.max_clones = 11; + icfg.backend.initial_sigma = T{1} / T{20}; + icfg.backend.normalized_sigma = static_cast(cfg.camera_noise_normalized); + icfg.backend.noise = sdk::msckf::ImuNoise{static_cast(cfg.gyro_noise_density), + static_cast(cfg.accel_noise_density), + static_cast(cfg.gyro_bias_random_walk), + static_cast(cfg.accel_bias_random_walk)}; + icfg.backend.enable_gating = true; + icfg.backend.chi2_per_dof = T{16}; // 4-sigma gate (matches run_synthetic_invariant) + icfg.backend.R_imu_cam = R_imu_cam_; + icfg.backend.p_imu_cam = p_imu_cam_; + impl_ = Impl(icfg); + initialized_ = false; + init_gyro_.clear(); + init_accel_.clear(); + last_imu_t_ = 0.0; + } + + void process_imu(const sdk::ImuMeasurement& m) { + // ImuMeasurement carries std::array; the backend / initializer take the + // Lie Vec3 — convert element-wise (as run_synthetic_invariant does). + const Vec3 gyro = to_vec3(m.angular_velocity); + const Vec3 accel = to_vec3(m.linear_acceleration); + if (!initialized_) { + // Keep a sliding window of the most recent samples and try to bootstrap. + init_gyro_.push_back(gyro); + init_accel_.push_back(accel); + last_imu_t_ = m.timestamp_s; + if (init_gyro_.size() > kInitSamples) { + init_gyro_.erase(init_gyro_.begin()); + init_accel_.erase(init_accel_.begin()); + } + if (init_gyro_.size() >= kInitSamples) + try_init(); + return; + } + impl_.process_imu(gyro, accel, m.timestamp_s); + } + + void process_camera(double t, std::span> obs) { + if (!initialized_) + return; // no clones before the nav seed exists + std::vector nobs; + nobs.reserve(obs.size()); + for (const auto& o : obs) { + if (o.camera_id != 0) + continue; + const auto bearing = cam_.unproject(math::cameras::Vec2{o.u, o.v}); + if (!(bearing[2] > T{0})) + continue; // ray not in front of the camera + nobs.push_back(NormalizedObs{o.feature_id, 0, Vec2{{bearing[0] / bearing[2], bearing[1] / bearing[2]}}}); + } + impl_.process_camera(t, std::span{nobs}); + } + + [[nodiscard]] sdk::NavState current_state() const { + sdk::NavState ns; + const auto& nav = impl_.nav(); + ns.timestamp_s = nav.timestamp; + ns.T_world_imu = SE3(nav.X.rotation(), nav.X.position()); + const auto& v = nav.X.velocity(); + ns.velocity_world = {v[0], v[1], v[2]}; // Lie Vec3 → std::array + return ns; + } + + // ── Telemetry passthroughs (used by the euroc-invariant harness) ────────── + + [[nodiscard]] bool initialized() const noexcept { + return initialized_; + } + [[nodiscard]] sdk::msckf::DynMat covariance() const { + return impl_.covariance(); + } + [[nodiscard]] std::size_t num_clones() const noexcept { + return impl_.num_clones(); + } + [[nodiscard]] const Impl& impl() const noexcept { + return impl_; + } + +private: + static Vec3 to_vec3(const sdk::Vec3& a) { + return Vec3{{a[0], a[1], a[2]}}; + } + + void try_init() { + const sdk::ImuInitializer init; + const std::span g{init_gyro_}, a{init_accel_}; + auto r = init.try_static(g, a); + if (!r.success) + r = init.try_gravity_align(g, a); // looser band: roll/pitch from mean specific force + if (!r.success) + return; // keep accumulating; retry on the next sample + // Zero-velocity, origin seed — a near-static EuRoC start; the dynamic-VI + // scale path was measured to wash out end-to-end, so this is the fair seed. + impl_.set_nav(SE23(r.R_world_imu, Vec3{}, Vec3{}), r.gyro_bias, r.accel_bias, last_imu_t_); + initialized_ = true; + } + + static constexpr std::size_t kInitSamples = 150; // ~0.75 s @ 200 Hz + + Camera cam_; + SO3 R_imu_cam_; + Vec3 p_imu_cam_; + Impl impl_{typename Impl::Config{}}; + bool initialized_ = false; + std::vector init_gyro_, init_accel_; + double last_imu_t_ = 0.0; +}; + +} // namespace branes::tools + +#endif // BRANES_TOOLS_INVARIANT_BACKEND_ADAPTER_HPP diff --git a/tools/src/vio_pipeline.cpp b/tools/src/vio_pipeline.cpp index 775ec23..1f9ab86 100644 --- a/tools/src/vio_pipeline.cpp +++ b/tools/src/vio_pipeline.cpp @@ -34,6 +34,7 @@ #include #include // so3_from_matrix #include +#include #include #include @@ -144,6 +145,14 @@ std::string mat3_json(const Mat& P, std::size_t off) { o << ']'; return o.str(); } +// Mean position σ from a covariance's 3×3 position block at `off`: √(trace/3). +// The over-confidence tell — compared against the actual ATE, a tiny σ means the +// filter trusts a covariance far smaller than its true error. +template +double pos_sigma(const Mat& P, std::size_t off) { + const double tr = P(off, off) + P(off + 1, off + 1) + P(off + 2, off + 2); + return std::sqrt((tr > 0.0 ? tr : 0.0) / 3.0); +} std::string nums_json(const std::vector& xs) { std::ostringstream o; o << '['; @@ -227,6 +236,7 @@ struct RunResult { double nis_normalized = 0; std::size_t frames = 0; double mean_features = 0; + double mean_pos_sigma_m = 0; ///< mean filter-reported position σ (√trace/3 of the P pos block) }; ev::SyntheticConfig robot_preset(const std::string& r) { @@ -494,6 +504,7 @@ bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofst std::vector> traj; std::size_t imu_idx = 0, feat_total = 0, n = 0; + double sigma_sum = 0; for (std::size_t f = 0; f < images.size(); ++f) { const double t = images[f].t_s; std::size_t end = imu_idx; @@ -517,6 +528,7 @@ bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofst sp.pose = est.current_pose(); traj.push_back(sp); ++n; + sigma_sum += pos_sigma(est.backend().state().covariance(), msckf::State::kPos); const auto tf = est.tracked_features(); feat_total += tf.size(); const double nis = safe_nis(est.backend().nis_consistency()); @@ -541,6 +553,95 @@ bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofst out.frames = n; out.mean_features = n ? static_cast(feat_total) / static_cast(n) : 0; out.nis_normalized = safe_nis(est.backend().nis_consistency()); + out.mean_pos_sigma_m = n ? sigma_sum / static_cast(n) : 0; + const auto assoc = ev::associate(traj, gt, 0.02); + out.ate_rms_m = assoc.estimated.empty() ? 0 : ev::ate_rmse(assoc.estimated, assoc.reference); + return true; +} + +// ── EuRoC, right-invariant (R-IEKF) backend (#347/#348) ────────────────────── +// The end-to-end counterpart to the standard run_euroc, on the SAME real frontend +// + GT, so ATE and the reported position σ are directly comparable. The invariant +// backend reaches the EuRoC pipeline through InvariantBackendAdapter (tools/), +// which bridges its parameterization-honest API to the VioEstimator contract. +bool run_euroc_invariant(const Args& args, const VioConfig& cfg, RunResult& out, std::ofstream* stream) { + using Adapter = branes::tools::InvariantBackendAdapter; + using Estimator = branes::sdk::VioEstimator; + using Mat3 = branes::math::lie::detail::Mat; + constexpr std::size_t kInvPos = 6; // invariant nav error-state: δθ(0) δv(3) δp(6) + + std::vector> imu; + std::vector images; + std::vector> gt; + try { + imu = euroc::parse_imu(args.dataset); + images = euroc::parse_images(args.dataset); + gt = euroc::parse_groundtruth(args.dataset); + } catch (const std::exception& e) { + std::cout << " EuRoC dataset not readable at '" << args.dataset << "': " << e.what() << "\n"; + return false; + } + if (images.empty() || imu.empty()) { + std::cout << " EuRoC streams empty at '" << args.dataset << "'. Skipping.\n"; + return false; + } + + typename Adapter::Camera intr( + 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05); + Mat3 R{}; + R(0, 0) = 0.0148655429818; + R(0, 1) = -0.999880929698; + R(0, 2) = 0.00414029679422; + R(1, 0) = 0.999557249008; + R(1, 1) = 0.0149672133247; + R(1, 2) = 0.025715529948; + R(2, 0) = -0.0257744366974; + R(2, 1) = 0.00375618835797; + R(2, 2) = 0.999660727178; + const auto R_imu_cam = branes::sdk::sfm::so3_from_matrix(R); + const Vec3 p_imu_cam{{-0.0216401454975, -0.064676986768, 0.00981073058949}}; + + Estimator est(Adapter(intr, R_imu_cam, p_imu_cam)); + est.configure(cfg); + est.activate(); + + std::vector> traj; + std::size_t imu_idx = 0, feat_total = 0, n = 0; + double sigma_sum = 0; + for (std::size_t f = 0; f < images.size(); ++f) { + const double t = images[f].t_s; + std::size_t end = imu_idx; + while (end < imu.size() && imu[end].timestamp_s <= t) + ++end; + if (end > imu_idx) { + est.feed_imu(std::span>{imu.data() + imu_idx, end - imu_idx}); + imu_idx = end; + } + branes::cv::OwnedImage img; + try { + img = branes::cv::read_png(images[f].path); + } catch (const std::exception&) { + continue; + } + est.feed_image(t, std::as_const(img).view()); + if (!est.backend().initialized()) + continue; + ev::StampedPose sp; + sp.t_s = t; + sp.pose = est.current_pose(); + traj.push_back(sp); + ++n; + sigma_sum += pos_sigma(est.backend().covariance(), kInvPos); + feat_total += est.num_tracked_features(); + if (stream && stream->is_open()) { + const Vec3 ep = est.current_pose().translation(); + *stream << "{\"type\":\"est\",\"t\":" << t << ",\"p\":" << v3str(ep) + << ",\"pcov\":" << mat3_json(est.backend().covariance(), kInvPos) << "}\n"; + } + } + out.frames = n; + out.mean_features = n ? static_cast(feat_total) / static_cast(n) : 0; + out.mean_pos_sigma_m = n ? sigma_sum / static_cast(n) : 0; const auto assoc = ev::associate(traj, gt, 0.02); out.ate_rms_m = assoc.estimated.empty() ? 0 : ev::ate_rmse(assoc.estimated, assoc.reference); return true; @@ -587,12 +688,19 @@ int main(int argc, char** argv) { auto stream = open("run.jsonl"); auto frames = args.video ? open("frames.jsonl") : std::ofstream{}; RunResult r; - if (!run_euroc(args, VioConfig{}, r, &stream, args.video ? &frames : nullptr)) + const bool ok = args.invariant ? run_euroc_invariant(args, VioConfig{}, r, &stream) + : run_euroc(args, VioConfig{}, r, &stream, args.video ? &frames : nullptr); + if (!ok) return 0; - std::cout << " source euroc : " << args.dataset << "\n frames tracked : " << r.frames + const char* backend = args.invariant ? "euroc [R-IEKF]" : "euroc [MSCKF]"; + std::cout << " source : " << backend << " " << args.dataset << "\n frames tracked : " << r.frames << "\n mean features : " << r.mean_features << "\n ATE (Horn) : " << r.ate_rms_m - << " m\n NIS (normalized): " << r.nis_normalized << "\n"; - if (args.video) + << " m\n mean pos sigma : " << r.mean_pos_sigma_m << " m (over-confidence: ATE/sigma = " + << (r.mean_pos_sigma_m > 0 ? r.ate_rms_m / r.mean_pos_sigma_m : 0.0) << ")"; + if (!args.invariant) + std::cout << "\n NIS (normalized): " << r.nis_normalized; + std::cout << "\n"; + if (args.video && !args.invariant) std::cout << " overlay: node docs-site/scripts/gen-overlay.mjs " << args.out << "\n"; return 0; } From 6914e506d9036646333d665c07750a27a0101c63 Mon Sep 17 00:00:00 2001 From: Theodore Omtzigt Date: Tue, 30 Jun 2026 17:45:27 -0400 Subject: [PATCH 2/3] refactor(tools): address CodeRabbit review on the R-IEKF EuRoC harness MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - msckf_invariant_backend: use an unqualified isfinite (ADL) for the δx guard instead of narrowing each component to double, so a custom math::Scalar (Universal posit) supplies its own — honors the no-hardcoded-double rule. δx is documented as the sentinel: a poisoned covariance resurfaces as a non-finite δx on the next update. - vio_pipeline: extract the EuRoC cam0 calibration into a shared euroc_cam0() helper so the MSCKF and R-IEKF runs are driven from byte-identical intrinsics/extrinsics (a fair comparison can't drift). - vio_pipeline: fail the EuRoC run when pose association is empty rather than reporting a perfect ATE 0 — an empty association means missing GT, timestamp drift, or an untracked trajectory. - vio_pipeline: stop masking a non-finite position σ as 0 (in pos_sigma and the over-confidence print) so a poisoned covariance is surfaced, which is exactly what this harness exists to expose. - tests: assert the adapter stays uninitialized before the bootstrap window fills, and that process_camera advances the state timestamp (compare before vs after) instead of only checking it is positive. V1_01 R-IEKF ATE is unchanged at 1.31 m after the calibration refactor. Relates to #365 Co-Authored-By: Claude Opus 4.8 (1M context) --- .../sdk/msckf/msckf_invariant_backend.hpp | 10 ++- tests/tools/invariant_backend_adapter.cpp | 18 +++- tools/src/vio_pipeline.cpp | 86 +++++++++++-------- 3 files changed, 73 insertions(+), 41 deletions(-) diff --git a/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp b/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp index 08ad89b..bde2376 100644 --- a/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp +++ b/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp @@ -247,8 +247,16 @@ class MsckfInvariantBackend { // alive on a degenerate measurement instead of aborting in SO3::normalize. const Cov cov_snapshot = cov_; const std::vector dx = cov_.update(H, std::span{proj.r}, std::span{R_diag}); + // δx = K·r is the sentinel: a non-finite update means the gain/innovation + // blew up, and the same factorization would have poisoned the covariance — + // restoring the snapshot rolls both back. (A covariance that turns + // non-finite while δx stays finite resurfaces as a non-finite δx on the + // next update and is caught here then.) Use an unqualified isfinite so a + // custom math::Scalar (e.g. a Universal posit) supplies its own via ADL — + // never narrow to double, per the no-hardcoded-double rule. + using std::isfinite; for (const T v : dx) { - if (!std::isfinite(static_cast(v))) { + if (!isfinite(v)) { cov_ = cov_snapshot; return false; } diff --git a/tests/tools/invariant_backend_adapter.cpp b/tests/tools/invariant_backend_adapter.cpp index 18d43c2..779cad2 100644 --- a/tests/tools/invariant_backend_adapter.cpp +++ b/tests/tools/invariant_backend_adapter.cpp @@ -56,8 +56,15 @@ TEST_CASE("invariant adapter: bootstraps from a static IMU window", "[tools][inv a.initialize(cfg); REQUIRE_FALSE(a.initialized()); - // Feed a static window; init should fire once enough samples accumulate. - for (int i = 0; i < 200; ++i) + // A handful of samples is far short of the bootstrap window: init must NOT + // fire early (a regression that bootstraps on too little data would be caught + // here, not just by the final positive check below). + for (int i = 0; i < 10; ++i) + a.process_imu(stationary_sample(0.005 * i)); + REQUIRE_FALSE(a.initialized()); + + // Once a full static window accumulates, init fires. + for (int i = 10; i < 200; ++i) a.process_imu(stationary_sample(0.005 * i)); REQUIRE(a.initialized()); @@ -91,11 +98,14 @@ TEST_CASE("invariant adapter: satisfies the VioEstimator backend contract", "[to o.v = 240.0; obs.push_back(o); } + const auto before_t = a.current_state().timestamp_s; a.process_camera(1.0, std::span>{obs}); - // covariance() is a square PSD-sized matrix; current_state() stamp advances. + // covariance() is a square PSD-sized matrix; and process_camera must have + // advanced the state stamp (propagation to the frame time), not merely left + // the positive bootstrap stamp in place. const auto P = a.covariance(); REQUIRE(P.rows == P.cols); REQUIRE(P.rows >= 15); - REQUIRE(a.current_state().timestamp_s > 0.0); + REQUIRE(a.current_state().timestamp_s > before_t); } diff --git a/tools/src/vio_pipeline.cpp b/tools/src/vio_pipeline.cpp index 1f9ab86..20314a6 100644 --- a/tools/src/vio_pipeline.cpp +++ b/tools/src/vio_pipeline.cpp @@ -63,6 +63,32 @@ using branes::sdk::ImuMeasurement; using branes::sdk::MsckfBackend; using branes::sdk::VioConfig; +// EuRoC cam0 calibration (pinhole-radtan intrinsics + cam↔IMU extrinsic). Shared +// by the standard (MSCKF) and right-invariant (R-IEKF) EuRoC runs so the two +// backends are always driven from byte-identical calibration — any drift between +// the paths would silently invalidate the MSCKF-vs-R-IEKF comparison. +struct EurocCam0 { + branes::math::cameras::PinholeRadtanCamera intrinsics; + branes::math::lie::SO3 R_imu_cam; + Vec3 p_imu_cam; +}; +inline EurocCam0 euroc_cam0() { + branes::math::lie::detail::Mat R{}; + R(0, 0) = 0.0148655429818; + R(0, 1) = -0.999880929698; + R(0, 2) = 0.00414029679422; + R(1, 0) = 0.999557249008; + R(1, 1) = 0.0149672133247; + R(1, 2) = 0.025715529948; + R(2, 0) = -0.0257744366974; + R(2, 1) = 0.00375618835797; + R(2, 2) = 0.999660727178; + return EurocCam0{branes::math::cameras::PinholeRadtanCamera( + 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05), + branes::sdk::sfm::so3_from_matrix(R), + Vec3{{-0.0216401454975, -0.064676986768, 0.00981073058949}}}; +} + struct Args { std::string out; std::string source = "synthetic"; @@ -151,6 +177,8 @@ std::string mat3_json(const Mat& P, std::size_t off) { template double pos_sigma(const Mat& P, std::size_t off) { const double tr = P(off, off) + P(off + 1, off + 1) + P(off + 2, off + 2); + if (!std::isfinite(tr)) + return tr; // surface a poisoned covariance — this harness exists to expose it, not hide it as 0 return std::sqrt((tr > 0.0 ? tr : 0.0) / 3.0); } std::string nums_json(const std::vector& xs) { @@ -462,7 +490,6 @@ RunResult run_synthetic(const ev::SyntheticData& w, bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofstream* stream, std::ofstream* frames) { using Backend = MsckfBackend; using Estimator = branes::sdk::VioEstimator; - using Mat3 = branes::math::lie::detail::Mat; std::vector> imu; std::vector images; @@ -482,21 +509,11 @@ bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofst return false; } + const auto cam0 = euroc_cam0(); typename Backend::CameraCalibration cal; - cal.intrinsics = typename Backend::Camera( - 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05); - Mat3 R{}; - R(0, 0) = 0.0148655429818; - R(0, 1) = -0.999880929698; - R(0, 2) = 0.00414029679422; - R(1, 0) = 0.999557249008; - R(1, 1) = 0.0149672133247; - R(1, 2) = 0.025715529948; - R(2, 0) = -0.0257744366974; - R(2, 1) = 0.00375618835797; - R(2, 2) = 0.999660727178; - cal.extrinsics.R_imu_cam = branes::sdk::sfm::so3_from_matrix(R); - cal.extrinsics.p_imu_cam = Vec3{{-0.0216401454975, -0.064676986768, 0.00981073058949}}; + cal.intrinsics = cam0.intrinsics; + cal.extrinsics.R_imu_cam = cam0.R_imu_cam; + cal.extrinsics.p_imu_cam = cam0.p_imu_cam; Estimator est(Backend(std::vector{cal})); est.configure(cfg); @@ -555,7 +572,12 @@ bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofst out.nis_normalized = safe_nis(est.backend().nis_consistency()); out.mean_pos_sigma_m = n ? sigma_sum / static_cast(n) : 0; const auto assoc = ev::associate(traj, gt, 0.02); - out.ate_rms_m = assoc.estimated.empty() ? 0 : ev::ate_rmse(assoc.estimated, assoc.reference); + if (assoc.estimated.empty()) { + std::cerr << " EuRoC pose association produced no matches (missing GT, timestamp drift, or no tracked " + "trajectory); cannot compute ATE.\n"; + return false; // an empty association is a failure, not a perfect (ATE 0) run + } + out.ate_rms_m = ev::ate_rmse(assoc.estimated, assoc.reference); return true; } @@ -567,7 +589,6 @@ bool run_euroc(const Args& args, const VioConfig& cfg, RunResult& out, std::ofst bool run_euroc_invariant(const Args& args, const VioConfig& cfg, RunResult& out, std::ofstream* stream) { using Adapter = branes::tools::InvariantBackendAdapter; using Estimator = branes::sdk::VioEstimator; - using Mat3 = branes::math::lie::detail::Mat; constexpr std::size_t kInvPos = 6; // invariant nav error-state: δθ(0) δv(3) δp(6) std::vector> imu; @@ -586,22 +607,8 @@ bool run_euroc_invariant(const Args& args, const VioConfig& cfg, RunResult& out, return false; } - typename Adapter::Camera intr( - 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05); - Mat3 R{}; - R(0, 0) = 0.0148655429818; - R(0, 1) = -0.999880929698; - R(0, 2) = 0.00414029679422; - R(1, 0) = 0.999557249008; - R(1, 1) = 0.0149672133247; - R(1, 2) = 0.025715529948; - R(2, 0) = -0.0257744366974; - R(2, 1) = 0.00375618835797; - R(2, 2) = 0.999660727178; - const auto R_imu_cam = branes::sdk::sfm::so3_from_matrix(R); - const Vec3 p_imu_cam{{-0.0216401454975, -0.064676986768, 0.00981073058949}}; - - Estimator est(Adapter(intr, R_imu_cam, p_imu_cam)); + const auto cam0 = euroc_cam0(); + Estimator est(Adapter(cam0.intrinsics, cam0.R_imu_cam, cam0.p_imu_cam)); est.configure(cfg); est.activate(); @@ -643,7 +650,12 @@ bool run_euroc_invariant(const Args& args, const VioConfig& cfg, RunResult& out, out.mean_features = n ? static_cast(feat_total) / static_cast(n) : 0; out.mean_pos_sigma_m = n ? sigma_sum / static_cast(n) : 0; const auto assoc = ev::associate(traj, gt, 0.02); - out.ate_rms_m = assoc.estimated.empty() ? 0 : ev::ate_rmse(assoc.estimated, assoc.reference); + if (assoc.estimated.empty()) { + std::cerr << " EuRoC pose association produced no matches (missing GT, timestamp drift, or no tracked " + "trajectory); cannot compute ATE.\n"; + return false; // an empty association is a failure, not a perfect (ATE 0) run + } + out.ate_rms_m = ev::ate_rmse(assoc.estimated, assoc.reference); return true; } @@ -695,8 +707,10 @@ int main(int argc, char** argv) { const char* backend = args.invariant ? "euroc [R-IEKF]" : "euroc [MSCKF]"; std::cout << " source : " << backend << " " << args.dataset << "\n frames tracked : " << r.frames << "\n mean features : " << r.mean_features << "\n ATE (Horn) : " << r.ate_rms_m - << " m\n mean pos sigma : " << r.mean_pos_sigma_m << " m (over-confidence: ATE/sigma = " - << (r.mean_pos_sigma_m > 0 ? r.ate_rms_m / r.mean_pos_sigma_m : 0.0) << ")"; + << " m\n mean pos sigma : " << r.mean_pos_sigma_m + << " m (over-confidence: ATE/sigma = " + // Divide whenever σ is usable; let a non-finite σ propagate (don't mask a poisoned covariance as 0). + << (r.mean_pos_sigma_m != 0.0 ? r.ate_rms_m / r.mean_pos_sigma_m : 0.0) << ")"; if (!args.invariant) std::cout << "\n NIS (normalized): " << r.nis_normalized; std::cout << "\n"; From c14f025906142924d70f3fd8dfc2a8fd27ff8a2c Mon Sep 17 00:00:00 2001 From: Theodore Omtzigt Date: Tue, 30 Jun 2026 19:31:00 -0400 Subject: [PATCH 3/3] test(tools): pin the adapter bootstrap boundary at kInitSamples-1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Assert the adapter is still uninitialized one sample short of the window and initializes exactly as it fills, instead of checking far below the threshold — the off-by-one pin also catches a silently-lowered threshold (CodeRabbit follow-up). Relates to #365 Co-Authored-By: Claude Opus 4.8 (1M context) --- tests/tools/invariant_backend_adapter.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/tests/tools/invariant_backend_adapter.cpp b/tests/tools/invariant_backend_adapter.cpp index 779cad2..1f3b836 100644 --- a/tests/tools/invariant_backend_adapter.cpp +++ b/tests/tools/invariant_backend_adapter.cpp @@ -56,15 +56,14 @@ TEST_CASE("invariant adapter: bootstraps from a static IMU window", "[tools][inv a.initialize(cfg); REQUIRE_FALSE(a.initialized()); - // A handful of samples is far short of the bootstrap window: init must NOT - // fire early (a regression that bootstraps on too little data would be caught - // here, not just by the final positive check below). - for (int i = 0; i < 10; ++i) + // Pin the bootstrap boundary exactly: one sample short of the window + // (kInitSamples = 150 in the adapter) it must still be down, and it must come + // up as soon as the window fills. Asserting at the off-by-one catches both an + // early bootstrap and a silently-lowered threshold. + for (int i = 0; i < 149; ++i) a.process_imu(stationary_sample(0.005 * i)); REQUIRE_FALSE(a.initialized()); - - // Once a full static window accumulates, init fires. - for (int i = 10; i < 200; ++i) + for (int i = 149; i < 200; ++i) a.process_imu(stationary_sample(0.005 * i)); REQUIRE(a.initialized());