feat(tools): real-data observability / null-space-leak diagnostic

tests/tools/observability_inspect.cpp

@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: MIT
+//
+// Gate for the real-data observability inspector core (branes/tools/
+// observability_inspect.hpp, #212 / #337). The full obs_inspect tool taps a live
+// EuRoC run, which CI does not vendor, so here we drive the same header math on a
+// constructed clone window and pin the gauge property the diagnosis relies on:
+//
+//   * at a CONSISTENT linearization (H and N at the same poses) the SHIPPED camera
+//     Jacobian annihilates the 4-DoF gauge -> both translation and yaw leak are
+//     ~machine-eps (a regression in the production projection_jacobians would
+//     break this, not just a copy of it);
+//   * under a PERTURBED clone window with N held at the true gauge, the TRANSLATION
+//     leak stays ~0 (structurally protected by the +/-Hf cancellation) while the
+//     YAW leak grows -- the over-confidence mechanism, localized to yaw.
+//
+// ASCII-only TEST_CASE names (the Windows MSVC ctest job rejects non-ASCII).
+
+#include <branes/math/lie/detail.hpp>
+#include <branes/math/lie/so3.hpp>
+#include <branes/sdk/msckf/camera_updater.hpp>
+#include <branes/tools/observability_inspect.hpp>
+
+#include <catch2/catch_test_macros.hpp>
+#include <catch2/matchers/catch_matchers_floating_point.hpp>
+
+#include <cmath>
+#include <random>
+#include <vector>
+
+namespace bt = branes::tools;
+namespace ms = branes::sdk::msckf;
+namespace ld = branes::math::lie::detail;
+using Catch::Matchers::WithinAbs;
+
+namespace {
+using T = double;
+using Vec3 = ld::Vec<T, 3>;
+using SO3 = branes::math::lie::SO3<T>;
+
+// A small translating clone window with mild rotation diversity + one feature at
+// healthy parallax — the same shape as observability_probe::make_scene.
+std::vector<bt::ObsPose<T>> window(std::size_t m) {
+    std::vector<bt::ObsPose<T>> cl;
+    for (std::size_t c = 0; c < m; ++c) {
+        const T u = static_cast<T>(c);
+        cl.push_back({SO3::exp(Vec3{{0.04 * std::sin(u), 0.03 * u - 0.1, 0.05 * std::cos(u)}}),
+                      Vec3{{0.3 * u, 0.1 * std::sin(u), 0.05 * u}}});
+    }
+    return cl;
+}
+}  // namespace
+
+TEST_CASE("observability inspector: shipped Jacobian annihilates the gauge at a consistent point",
+          "[tools][obs_inspect]") {
+    const ms::CameraUpdater<T> updater(std::vector<ms::CameraExtrinsics<T>>{ms::CameraExtrinsics<T>{}});
+    const Vec3 g{{0.0, 0.0, 1.0}};
+    const Vec3 p_f{{0.2, -0.1, 6.0}};
+    const auto cl = window(5);
+
+    const auto N = bt::obs_build_N<T>(cl, p_f, g);
+    const auto [tr, yaw] = bt::obs_leak<T>(bt::obs_build_H<T>(updater, cl, p_f), N);
+
+    // H linearized at the same poses N is built from => the gauge is annihilated.
+    REQUIRE_THAT(tr, WithinAbs(0.0, 1e-9));
+    REQUIRE_THAT(yaw, WithinAbs(0.0, 1e-9));
+}
+
+TEST_CASE("observability inspector: perturbing the window leaks yaw but not translation", "[tools][obs_inspect]") {
+    const ms::CameraUpdater<T> updater(std::vector<ms::CameraExtrinsics<T>>{ms::CameraExtrinsics<T>{}});
+    const Vec3 g{{0.0, 0.0, 1.0}};
+    const Vec3 p_f{{0.2, -0.1, 6.0}};
+    const auto truth = window(5);
+    const auto N = bt::obs_build_N<T>(truth, p_f, g);  // gauge at the TRUE poses
+
+    // Perturb the clone estimates (the cross-window linearization drift), keep N at
+    // the true gauge — the standard-EKF inconsistency the diagnosis measures.
+    std::mt19937_64 rng(0x0B5E11ull);
+    auto perturbed = [&](T s) {
+        auto e = truth;
+        for (auto& c : e) {
+            c.R =
+                c.R * SO3::exp(Vec3{{s * bt::obs_urand<T>(rng), s * bt::obs_urand<T>(rng), s * bt::obs_urand<T>(rng)}});
+            c.p = Vec3{{c.p[0] + s * bt::obs_urand<T>(rng),
+                        c.p[1] + s * bt::obs_urand<T>(rng),
+                        c.p[2] + s * bt::obs_urand<T>(rng)}};
+        }
+        return e;
+    };
+
+    const auto [tr_small, yaw_small] = bt::obs_leak<T>(bt::obs_build_H<T>(updater, perturbed(0.01), p_f), N);
+    const auto [tr_big, yaw_big] = bt::obs_leak<T>(bt::obs_build_H<T>(updater, perturbed(0.05), p_f), N);
+
+    // Translation is structurally protected: the clone-dp and feature-dp columns
+    // are -Hf and +Hf and cancel for ANY single H, perturbed or not.
+    REQUIRE_THAT(tr_small, WithinAbs(0.0, 1e-9));
+    REQUIRE_THAT(tr_big, WithinAbs(0.0, 1e-9));
+    // Yaw leaks, and grows with the perturbation — the over-confidence enters here.
+    REQUIRE(yaw_small > 1e-6);
+    REQUIRE(yaw_big > yaw_small);
+}

tools/README.md

@@ -91,14 +91,15 @@ replays a stage and visualizes it.
 | `docs-site/scripts/gen-marginalization-figures.mjs` | before/after covariance heatmap with the dropped clone block outlined |
 | `tools/src/s10_inspect.cpp` | **S10 online-calibration inspector** (filter-internal tier) — perturbed extrinsic + `estimate_extrinsics` → in-state extrinsic converging vs the dataset reference |
 | `docs-site/scripts/gen-calibration-figures.mjs` | extrinsic rot/trans error-vs-time convergence inside the ±3σ band |
+| `tools/src/obs_inspect.cpp` | **observability / null-space-leak diagnostic** (#212/#337) — taps every real update, drives the shipped `projection_jacobians` over the live clone window → per-update yaw vs translation gauge leak ‖H·N‖ |
 
 EuRoC (~1.5 GB) is not vendored, so these are developer tools, **not CI gates**;
 the trace schema and the inspector logic are gated by `tests/tools/vio_trace*.cpp`,
 `tests/tools/s4_frontend_inspect.cpp`, `tests/tools/s0_sensor_model_inspect.cpp`,
 `tests/tools/s5_triangulation_inspect.cpp`, `tests/tools/s6_update_inspect.cpp`,
 `tests/tools/s1_init_inspect.cpp`, `tests/tools/s2_propagation_inspect.cpp`,
-`tests/tools/s3_augmentation_inspect.cpp`, `tests/tools/s9_marginalization_inspect.cpp`
-and `tests/tools/s10_calibration_inspect.cpp`.
+`tests/tools/s3_augmentation_inspect.cpp`, `tests/tools/s9_marginalization_inspect.cpp`,
+`tests/tools/s10_calibration_inspect.cpp` and `tests/tools/observability_inspect.cpp`.
 
 ### Trace tap (`asl_trace`)
 
@@ -334,6 +335,33 @@ draws the error decaying toward the reference inside the filter's own (shrinking
 ±3σ band. (On a physically-consistent EuRoC run the error converges, as the
 `calib_recovery` unit test proves on consistent data.)
 
+### Observability / null-space-leak diagnostic (`obs_inspect`)
+
+The real-data companion to the synthetic `observability_probe` (#337). A monocular
+VIO has a 4-DoF unobservable gauge — global translation (3) + rotation about
+gravity, *yaw* (1) — and a consistent filter must never gain information along it:
+the stacked camera Jacobian must annihilate the gauge, ‖H·N‖ ≈ 0. That holds only
+at a single consistent linearization; a standard EKF re-linearizes each clone at
+its own drifted estimate, so across a window the gauge leaks and the filter
+fabricates information → over-confidence. This tool **measures the leak on real
+data**: it taps every MSCKF update, drives the shipped `projection_jacobians` over
+the live clone window + triangulated feature, and reports two leaks per update —
+the *consistent* leak (H and N at the same estimate; the gauge-annihilation sanity
+check, must be ~machine-ε) and the *real* leak (H perturbed by the filter's own
+per-clone σ from P, N at the unperturbed gauge).
+
+```bash
+./build/tools/obs_inspect --dataset /path/to/V2_03_difficult/mav0 --out build/obs --max-frames 300
+```
+
+Output `observability.jsonl`: per-update window size, mean clone σ, and the
+consistent/real yaw & translation leaks. On V2_03 the consistent leak is ~1e-15
+(the shipped Jacobian is correct), the translation leak is **structurally 0**
+(the ±Hf cancellation protects it), and the **yaw leak is nonzero on 100% of
+updates and grows with the clone window's attitude drift** — localizing the #212
+over-confidence to the yaw gauge on real data. The fix is the right-invariant
+(R-IEKF) parameterization, whose gauge directions are estimate-independent.
+
 ## Layout
 
 | Piece | Role |

tools/include/branes/tools/observability_inspect.hpp

@@ -0,0 +1,134 @@
+// SPDX-License-Identifier: MIT
+//
+// branes/tools/observability_inspect.hpp — the testable core of the real-data
+// observability / null-space-leak diagnostic (obs_inspect.cpp, #212 / #337).
+//
+// The synthetic observability_probe (sdk/eval/observability_probe.hpp) proves the
+// gauge-leak MECHANISM on a synthetic scene. This header is its real-data
+// companion: the same three operations — build the stacked camera Jacobian H by
+// driving the SHIPPED CameraUpdater::projection_jacobians over a clone window,
+// build the analytic 4-DoF gauge N (global translation x3 + rotation-about-gravity
+// x1), and measure the per-direction leak ‖H·N‖ — but parameterized so the
+// obs_inspect tool can feed it the LIVE filter's clone window and the gate test
+// can feed it a constructed one. Unlike the eval probe it takes the production
+// updater (so it respects the real extrinsic) and uses fully-qualified types (no
+// `using namespace`) so it composes with the backend headers without the
+// Vec3-ambiguity that bites a `using namespace obs_detail` TU.
+//
+// A consistent filter must never gain information along the gauge: H·N ≈ 0. That
+// holds only at a single consistent linearization — so the gate test asserts the
+// consistent leak is ~machine-ε (the shipped Jacobian annihilates the gauge), and
+// that under a perturbed clone window the TRANSLATION leak stays ≈ 0 (structurally
+// protected by the ±Hf cancellation) while the YAW leak grows. Header-only,
+// C++20, type-generic.
+
+#ifndef BRANES_TOOLS_OBSERVABILITY_INSPECT_HPP
+#define BRANES_TOOLS_OBSERVABILITY_INSPECT_HPP
+
+#include <branes/math/lie/detail.hpp>
+#include <branes/math/lie/so3.hpp>
+#include <branes/sdk/msckf/camera_updater.hpp>
+#include <branes/sdk/msckf/dense.hpp>
+#include <branes/sdk/msckf/state.hpp>
+
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <utility>
+#include <vector>
+
+namespace branes::tools {
+
+/// One pose in the clone window the leak machinery operates on: the live filter's
+/// current clone estimate, or a perturbed copy of it.
+template <math::Scalar T>
+struct ObsPose {
+    math::lie::SO3<T> R{};
+    math::lie::detail::Vec<T, 3> p{};
+};
+
+/// Stacked camera measurement Jacobian over the clone window, driving the SHIPPED
+/// CameraUpdater::projection_jacobians (so the gauge property of the REAL filter
+/// code is what gets measured). Columns: [ per clone δθ(3) δp(3) | feature δp(3) ];
+/// rows: 2 per clone. δθ block = Htheta, δp block = −Hf, feature = Hf — exactly the
+/// layout CameraUpdater::update stacks into H.
+template <math::Scalar T>
+[[nodiscard]] sdk::msckf::DynMat<T> obs_build_H(const sdk::msckf::CameraUpdater<T>& updater,
+                                                const std::vector<ObsPose<T>>& cl,
+                                                const math::lie::detail::Vec<T, 3>& p_f) {
+    const std::size_t m = cl.size();
+    sdk::msckf::State<T> s(T{1});
+    s.clones.reserve(m);
+    for (std::size_t c = 0; c < m; ++c)
+        s.clones.push_back({cl[c].R, cl[c].p, static_cast<double>(c)});
+    sdk::msckf::DynMat<T> H(2 * m, 6 * m + 3);
+    for (std::size_t c = 0; c < m; ++c) {
+        sdk::msckf::CameraObservation<T> o;
+        o.clone_index = c;
+        o.camera_index = 0;
+        const auto J = updater.projection_jacobians(s, o, p_f);
+        for (std::size_t a = 0; a < 2; ++a)
+            for (std::size_t b = 0; b < 3; ++b) {
+                H(2 * c + a, 6 * c + b) = J.Htheta(a, b);   // δθ_c
+                H(2 * c + a, 6 * c + 3 + b) = -J.Hf(a, b);  // δp_c = −Hf
+                H(2 * c + a, 6 * m + b) = J.Hf(a, b);       // feature δp
+            }
+    }
+    return H;
+}
+
+/// The analytic 4-DoF gauge over [ clones | feature ], at poses `cl` / feature
+/// `p_f`. Cols 0–2: global translation (shift every clone + the feature by e_k).
+/// Col 3: rotation about gravity g — δθ_c = R_c^T g, δp_c = [g]× p_c, δp_f = [g]× p_f.
+template <math::Scalar T>
+[[nodiscard]] sdk::msckf::DynMat<T> obs_build_N(const std::vector<ObsPose<T>>& cl,
+                                                const math::lie::detail::Vec<T, 3>& p_f,
+                                                const math::lie::detail::Vec<T, 3>& g) {
+    const std::size_t m = cl.size();
+    sdk::msckf::DynMat<T> N(6 * m + 3, 4);
+    for (std::size_t c = 0; c < m; ++c)
+        for (std::size_t k = 0; k < 3; ++k)
+            N(6 * c + 3 + k, k) = T{1};  // translation: clone δp
+    for (std::size_t k = 0; k < 3; ++k)
+        N(6 * m + k, k) = T{1};  // translation: feature δp
+    const math::lie::detail::Mat<T, 3, 3> gx = math::lie::detail::hat(g);
+    for (std::size_t c = 0; c < m; ++c) {
+        const math::lie::detail::Vec<T, 3> dth = cl[c].R.inverse() * g;  // R_c^T g
+        const math::lie::detail::Vec<T, 3> dp = gx * cl[c].p;            // [g]× p_c
+        for (std::size_t k = 0; k < 3; ++k) {
+            N(6 * c + k, 3) = dth[k];
+            N(6 * c + 3 + k, 3) = dp[k];
+        }
+    }
+    const math::lie::detail::Vec<T, 3> dpf = gx * p_f;
+    for (std::size_t k = 0; k < 3; ++k)
+        N(6 * m + k, 3) = dpf[k];
+    return N;
+}
+
+/// Per-gauge-direction leak: column norms of H·N. `.first` = translation (RSS of
+/// cols 0–2), `.second` = yaw (norm of col 3).
+template <math::Scalar T>
+[[nodiscard]] std::pair<T, T> obs_leak(const sdk::msckf::DynMat<T>& H, const sdk::msckf::DynMat<T>& N) {
+    const sdk::msckf::DynMat<T> HN = sdk::msckf::mul(H, N);
+    using std::sqrt;
+    T tr = T{0}, yaw = T{0};
+    for (std::size_t i = 0; i < HN.rows; ++i) {
+        for (std::size_t k = 0; k < 3; ++k)
+            tr += HN(i, k) * HN(i, k);
+        yaw += HN(i, 3) * HN(i, 3);
+    }
+    return {sqrt(tr), sqrt(yaw)};
+}
+
+/// Portable reproducible draw in [−1, 1) from a 64-bit engine (matches
+/// observability_probe::urand), so the perturbation sweep is deterministic
+/// everywhere.
+template <math::Scalar T, class Rng>
+[[nodiscard]] T obs_urand(Rng& rng) {
+    return T(static_cast<double>(rng() >> 11) * (2.0 / 9007199254740992.0) - 1.0);
+}
+
+}  // namespace branes::tools
+
+#endif  // BRANES_TOOLS_OBSERVABILITY_INSPECT_HPP