feat(tools): drive R-IEKF through the real EuRoC pipeline

sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp

@@ -25,6 +25,7 @@
 #include <branes/sdk/msckf/invariant_update.hpp>
 
 #include <array>
+#include <cmath>
 #include <cstddef>
 #include <span>
 #include <stdexcept>
@@ -238,7 +239,28 @@ class MsckfInvariantBackend {
                 return false;
         }
         const std::vector<T> 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<T> dx = cov_.update(H, std::span<const T>{proj.r}, std::span<const T>{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 (!isfinite(v)) {
+                cov_ = cov_snapshot;
+                return false;
+            }
+        }
 
         retract(dx);
         return true;

tests/tools/invariant_backend_adapter.cpp

@@ -0,0 +1,110 @@
+// 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<T, Backend> 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 <branes/math/cameras/pinhole_radtan.hpp>
+#include <branes/math/lie/so3.hpp>
+#include <branes/sdk/vio_backend.hpp>
+#include <branes/tools/invariant_backend_adapter.hpp>
+
+#include <catch2/catch_test_macros.hpp>
+#include <catch2/matchers/catch_matchers_floating_point.hpp>
+
+#include <cmath>
+#include <span>
+#include <vector>
+
+namespace bt = branes::tools;
+namespace bs = branes::sdk;
+using Catch::Matchers::WithinAbs;
+
+namespace {
+using T = double;
+using Adapter = bt::InvariantBackendAdapter<T>;
+using SO3 = branes::math::lie::SO3<T>;
+using Vec3 = branes::math::lie::detail::Vec<T, 3>;
+
+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<T> stationary_sample(double t) {
+    bs::ImuMeasurement<T> 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());
+
+    // 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());
+    for (int i = 149; 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<bs::FrontendObservation<T>> obs;
+    for (int k = 0; k < 4; ++k) {
+        bs::FrontendObservation<T> o;
+        o.feature_id = static_cast<std::uint64_t>(k);
+        o.camera_id = 0;
+        o.u = 300.0 + 40.0 * k;
+        o.v = 240.0;
+        obs.push_back(o);
+    }
+    const auto before_t = a.current_state().timestamp_s;
+    a.process_camera(1.0, std::span<const bs::FrontendObservation<T>>{obs});
+
+    // 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 > before_t);
+}

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<T, Backend> 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> (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 <branes/math/cameras/pinhole_radtan.hpp>
+#include <branes/math/lie/se23.hpp>
+#include <branes/math/lie/se3.hpp>
+#include <branes/math/lie/so3.hpp>
+#include <branes/sdk/imu_init.hpp>
+#include <branes/sdk/msckf/invariant_vio_backend.hpp>
+#include <branes/sdk/msckf/sqrt_covariance.hpp>
+#include <branes/sdk/vio_backend.hpp>
+
+#include <cstddef>
+#include <span>
+#include <vector>
+
+namespace branes::tools {
+
+/// Drop-in `Backend` for `VioEstimator<T, InvariantBackendAdapter<T>>` that runs
+/// the right-invariant filter on the real pipeline.
+template <math::Scalar T>
+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<T>;
+    using Impl = sdk::msckf::InvariantVioBackend<T, Cov>;
+    using Camera = math::cameras::PinholeRadtanCamera<T>;
+    using Vec3 = math::lie::detail::Vec<T, 3>;
+    using Vec2 = math::lie::detail::Vec<T, 2>;
+    using SO3 = math::lie::SO3<T>;
+    using SE3 = math::lie::SE3<T>;
+    using SE23 = math::lie::SE23<T>;
+    using NormalizedObs = sdk::msckf::NormalizedObs<T>;
+
+    /// 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<T, Backend> 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<T>(cfg.camera_noise_normalized);
+        icfg.backend.noise = sdk::msckf::ImuNoise<T>{static_cast<T>(cfg.gyro_noise_density),
+                                                     static_cast<T>(cfg.accel_noise_density),
+                                                     static_cast<T>(cfg.gyro_bias_random_walk),
+                                                     static_cast<T>(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<T>& 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<const sdk::FrontendObservation<T>> obs) {
+        if (!initialized_)
+            return;  // no clones before the nav seed exists
+        std::vector<NormalizedObs> nobs;
+        nobs.reserve(obs.size());
+        for (const auto& o : obs) {
+            if (o.camera_id != 0)
+                continue;
+            const auto bearing = cam_.unproject(math::cameras::Vec2<T>{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<const NormalizedObs>{nobs});
+    }
+
+    [[nodiscard]] sdk::NavState<T> current_state() const {
+        sdk::NavState<T> 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<T> 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<T>& a) {
+        return Vec3{{a[0], a[1], a[2]}};
+    }
+
+    void try_init() {
+        const sdk::ImuInitializer<T> init;
+        const std::span<const Vec3> 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<Vec3> init_gyro_, init_accel_;
+    double last_imu_t_ = 0.0;
+};
+
+}  // namespace branes::tools
+
+#endif  // BRANES_TOOLS_INVARIANT_BACKEND_ADAPTER_HPP