From 5131200d6039df69e1f4aee09a369a2d1d53bd8a Mon Sep 17 00:00:00 2001 From: Theodore Omtzigt Date: Sun, 21 Jun 2026 16:15:38 -0400 Subject: [PATCH] feat(tools): real-data observability / null-space-leak diagnostic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds obs_inspect, the real-data companion to the synthetic observability_probe (#337). It taps every MSCKF update over a real EuRoC sequence and drives the SHIPPED CameraUpdater::projection_jacobians over the live clone window + triangulated feature, measuring the per-direction gauge leak ‖H·N‖ for the 4-DoF unobservable subspace (global translation x3 + yaw x1): - consistent leak (H and N at the same estimate): the gauge-annihilation sanity check — must be ~machine-eps, confirming the production Jacobian is correct; - real leak (H perturbed by the filter's OWN per-clone sigma from P, N at the unperturbed gauge): the synthetic sigma-sweep evaluated at the uncertainty the filter actually reports on this window. On V2_03_difficult the consistent leak is ~1e-15 (Jacobian 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 (sigma_theta to 5.6 deg) — localizing the #212 over-confidence to the yaw gauge on real data, and pointing at the right-invariant (R-IEKF) fix. The testable core (build_H/build_N/leak) lives in a header using fully-qualified types (no `using namespace`, so it composes with the backend headers); the EuRoC driver is read-only and not a CI gate. The gauge property is gated by tests/tools/observability_inspect.cpp on a constructed clone window. Relates to #212, #337 Co-Authored-By: Claude Opus 4.8 (1M context) --- tests/tools/observability_inspect.cpp | 100 ++++++ tools/README.md | 32 +- .../branes/tools/observability_inspect.hpp | 134 +++++++ tools/src/obs_inspect.cpp | 332 ++++++++++++++++++ 4 files changed, 596 insertions(+), 2 deletions(-) create mode 100644 tests/tools/observability_inspect.cpp create mode 100644 tools/include/branes/tools/observability_inspect.hpp create mode 100644 tools/src/obs_inspect.cpp diff --git a/tests/tools/observability_inspect.cpp b/tests/tools/observability_inspect.cpp new file mode 100644 index 0000000..88a79e5 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include + +#include +#include +#include + +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; +using SO3 = branes::math::lie::SO3; + +// A small translating clone window with mild rotation diversity + one feature at +// healthy parallax — the same shape as observability_probe::make_scene. +std::vector> window(std::size_t m) { + std::vector> cl; + for (std::size_t c = 0; c < m; ++c) { + const T u = static_cast(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 updater(std::vector>{ms::CameraExtrinsics{}}); + 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(cl, p_f, g); + const auto [tr, yaw] = bt::obs_leak(bt::obs_build_H(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 updater(std::vector>{ms::CameraExtrinsics{}}); + 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(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(rng), s * bt::obs_urand(rng), s * bt::obs_urand(rng)}}); + c.p = Vec3{{c.p[0] + s * bt::obs_urand(rng), + c.p[1] + s * bt::obs_urand(rng), + c.p[2] + s * bt::obs_urand(rng)}}; + } + return e; + }; + + const auto [tr_small, yaw_small] = bt::obs_leak(bt::obs_build_H(updater, perturbed(0.01), p_f), N); + const auto [tr_big, yaw_big] = bt::obs_leak(bt::obs_build_H(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); +} diff --git a/tools/README.md b/tools/README.md index 9ee630a..3f8f230 100644 --- a/tools/README.md +++ b/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 | diff --git a/tools/include/branes/tools/observability_inspect.hpp b/tools/include/branes/tools/observability_inspect.hpp new file mode 100644 index 0000000..0ae4aa2 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +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 +struct ObsPose { + math::lie::SO3 R{}; + math::lie::detail::Vec 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 +[[nodiscard]] sdk::msckf::DynMat obs_build_H(const sdk::msckf::CameraUpdater& updater, + const std::vector>& cl, + const math::lie::detail::Vec& p_f) { + const std::size_t m = cl.size(); + sdk::msckf::State 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(c)}); + sdk::msckf::DynMat H(2 * m, 6 * m + 3); + for (std::size_t c = 0; c < m; ++c) { + sdk::msckf::CameraObservation 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 +[[nodiscard]] sdk::msckf::DynMat obs_build_N(const std::vector>& cl, + const math::lie::detail::Vec& p_f, + const math::lie::detail::Vec& g) { + const std::size_t m = cl.size(); + sdk::msckf::DynMat 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 gx = math::lie::detail::hat(g); + for (std::size_t c = 0; c < m; ++c) { + const math::lie::detail::Vec dth = cl[c].R.inverse() * g; // R_c^T g + const math::lie::detail::Vec 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 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 +[[nodiscard]] std::pair obs_leak(const sdk::msckf::DynMat& H, const sdk::msckf::DynMat& N) { + const sdk::msckf::DynMat 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 +[[nodiscard]] T obs_urand(Rng& rng) { + return T(static_cast(rng() >> 11) * (2.0 / 9007199254740992.0) - 1.0); +} + +} // namespace branes::tools + +#endif // BRANES_TOOLS_OBSERVABILITY_INSPECT_HPP diff --git a/tools/src/obs_inspect.cpp b/tools/src/obs_inspect.cpp new file mode 100644 index 0000000..2a4e954 --- /dev/null +++ b/tools/src/obs_inspect.cpp @@ -0,0 +1,332 @@ +// SPDX-License-Identifier: MIT +// +// obs_inspect — real-data observability / null-space-leak diagnostic (#212, #337). +// +// The synthetic observability_probe (sdk/eval/observability_probe.hpp, #337) +// established the MECHANISM behind the EuRoC over-confidence: a monocular VIO has +// a 4-DoF unobservable gauge (global translation x3, yaw x1); a consistent filter +// must never gain information along it, i.e. 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 H*N != 0 and the filter fabricates information -- +// over-confidence. The synthetic probe showed translation stays ~0 while YAW +// leaks and grows with the perturbation sigma. +// +// This tool MEASURES that leak on REAL data. It runs the shipped estimator over a +// real EuRoC sequence and, for every MSCKF update, drives the SHIPPED Jacobian +// (CameraUpdater::projection_jacobians, real extrinsic) on the LIVE clone window + +// the live triangulated feature, and forms the stacked H exactly as the updater +// does. Two leaks per update: +// +// * consistent leak -- H and N both at the current clone estimates => the +// gauge-annihilation sanity check on real geometry (must be ~ machine-eps; a +// nonzero value would mean the production Jacobian itself is wrong, not the +// linearization-point story); +// * real leak -- H at the clone window PERTURBED by the FILTER'S OWN claimed +// per-clone sigma (read from P), N at the unperturbed gauge, averaged over +// several seeded draws. This is the synthetic sigma-sweep evaluated at the +// uncertainty the filter actually reports on this window: given that +// uncertainty, how much yaw/translation information does the shipped H +// spuriously inject? +// +// Output: per-update JSONL (window size, mean clone sigma, consistent/real yaw & +// translation leak, NIS) + a summary localizing the leak to yaw vs translation on +// the real sequence. Read-only -- no filter change. EuRoC (~1.5 GB) is not +// vendored, so this is a developer diagnostic. +// +// Usage: +// obs_inspect --dataset [--out ] [--max-frames N] [--draws K] +// Then inspect /observability.jsonl. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace bs = branes::sdk; +namespace bt = branes::tools; +namespace cv = branes::cv; +namespace ms = branes::sdk::msckf; +namespace ld = branes::math::lie::detail; +using json = nlohmann::json; +using T = double; +using Backend = bs::MsckfBackend; +using Estimator = bs::VioEstimator; +using DVec3 = ld::Vec; +using DMat3 = ld::Mat; +using DSO3 = branes::math::lie::SO3; + +namespace { + +struct Args { + std::string dataset; + std::string out = "build/stage_probes/OBS"; + std::uint64_t max_frames = std::numeric_limits::max(); + int draws = 8; // seeded perturbation draws to average the real leak + bool help = false; +}; + +std::uint64_t to_u64(const std::string& raw) { + std::size_t pos = 0; + unsigned long long v = std::stoull(raw, &pos); + if (raw.empty() || pos != raw.size()) + throw std::runtime_error("obs_inspect: invalid integer '" + raw + "'"); + return v; +} + +Args parse(int argc, char** argv) { + Args a; + auto next = [&](int& i) -> std::string { + if (i + 1 >= argc) + throw std::runtime_error("obs_inspect: missing value after " + std::string(argv[i])); + return argv[++i]; + }; + for (int i = 1; i < argc; ++i) { + std::string_view v = argv[i]; + if (v == "--help" || v == "-h") + a.help = true; + else if (v == "--dataset") + a.dataset = next(i); + else if (v == "--out") + a.out = next(i); + else if (v == "--max-frames") + a.max_frames = to_u64(next(i)); + else if (v == "--draws") + a.draws = static_cast(to_u64(next(i))); + else + throw std::runtime_error("obs_inspect: unknown argument '" + std::string(v) + "'"); + } + return a; +} + +// EuRoC MAV cam0 calibration (intrinsics + extrinsic), matching s6_inspect/vio_pipeline. +Backend::CameraCalibration euroc_cam0() { + Backend::CameraCalibration cal; + cal.intrinsics = + Backend::Camera(458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05); + DMat3 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 = bs::sfm::so3_from_matrix(R); + cal.extrinsics.p_imu_cam = DVec3{{-0.0216401454975, -0.064676986768, 0.00981073058949}}; + return cal; +} + +using bt::ObsPose; + +void usage() { + std::cout << "obs_inspect -- real-data observability / null-space-leak diagnostic (#212, #337)\n\n" + " obs_inspect --dataset [--out ] [--max-frames N] [--draws K]\n"; +} + +} // namespace + +int main(int argc, char** argv) { + Args args; + try { + args = parse(argc, argv); + } catch (const std::exception& ex) { + std::cerr << ex.what() << "\n"; + return 2; + } + if (args.help) { + usage(); + return 0; + } + if (args.dataset.empty()) { + std::cerr << "obs_inspect: --dataset is required.\n\n"; + usage(); + return 2; + } + + std::vector> imu; + std::vector images; + try { + imu = bs::euroc::parse_imu(args.dataset); + images = bs::euroc::parse_images(args.dataset); + } catch (const std::exception& ex) { + std::cerr << "obs_inspect: " << ex.what() << "\n"; + return 1; + } + if (images.empty() || imu.empty()) { + std::cerr << "obs_inspect: empty EuRoC streams\n"; + return 1; + } + + const auto cal = euroc_cam0(); + const ms::CameraUpdater updater(std::vector>{cal.extrinsics}); + + std::error_code ec; + std::filesystem::create_directories(args.out, ec); + std::ofstream os(args.out + "/observability.jsonl"); + if (!os) { + std::cerr << "obs_inspect: cannot write " << args.out << "/observability.jsonl\n"; + return 1; + } + + Estimator est(Backend(std::vector{cal})); + bs::VioConfig cfg; + est.configure(cfg); + est.activate(); + + const DVec3 g{{T{0}, T{0}, T{1}}}; // yaw axis = world up (config gravity is -z) + double frame_t = 0.0; + std::uint64_t n_updates = 0, n_measured = 0; + double sum_cons_yaw = 0.0, sum_cons_tr = 0.0; + double sum_real_yaw = 0.0, sum_real_tr = 0.0, max_real_yaw = 0.0; + + est.backend().set_update_observer([&](const ms::State& s, + const ms::FeatureTrack& track, + const ms::NisSample& nis, + bool accepted, + const ms::DynMat&) { + ++n_updates; + const std::size_t m = track.observations.size(); + if (m < 2) + return; + + DVec3 p_f{}; + if (!updater.triangulate(s, track.observations, p_f)) + return; + + std::vector> cl; + cl.reserve(m); + const ms::DynMat P = s.covariance(); + std::vector sth(m, T{0}), sp(m, T{0}); // per-clone theta / p sigma from P + for (std::size_t i = 0; i < m; ++i) { + const std::size_t ci = track.observations[i].clone_index; + if (ci >= s.clones.size()) + return; + cl.push_back(ObsPose{s.clones[ci].R, s.clones[ci].p}); + const std::size_t off = s.clone_offset(ci); + T vth = T{0}, vp = T{0}; + for (std::size_t k = 0; k < 3; ++k) { + vth += P(off + k, off + k); + vp += P(off + 3 + k, off + 3 + k); + } + sth[i] = std::sqrt(vth / T{3}); + sp[i] = std::sqrt(vp / T{3}); + } + + // (1) Consistent leak: H and N at the same current estimate => must be ~0. + const ms::DynMat N = bt::obs_build_N(cl, p_f, g); + const auto [cons_tr, cons_yaw] = bt::obs_leak(bt::obs_build_H(updater, cl, p_f), N); + + // (2) Real leak: perturb the clone window by the filter's OWN per-clone + // sigma, N at the unperturbed gauge, averaged over seeded draws. + double dyaw = 0.0, dtr = 0.0; + std::mt19937_64 rng(0x0B5E11ull ^ (n_updates * 0x9E3779B97F4A7C15ull)); + for (int d = 0; d < args.draws; ++d) { + std::vector> pe = cl; + for (std::size_t i = 0; i < m; ++i) { + pe[i].R = pe[i].R * DSO3::exp(DVec3{{sth[i] * bt::obs_urand(rng), + sth[i] * bt::obs_urand(rng), + sth[i] * bt::obs_urand(rng)}}); + pe[i].p = DVec3{{pe[i].p[0] + sp[i] * bt::obs_urand(rng), + pe[i].p[1] + sp[i] * bt::obs_urand(rng), + pe[i].p[2] + sp[i] * bt::obs_urand(rng)}}; + } + const auto [tr, yaw] = bt::obs_leak(bt::obs_build_H(updater, pe, p_f), N); + dyaw += static_cast(yaw); + dtr += static_cast(tr); + } + dyaw /= args.draws; + dtr /= args.draws; + + T mean_sth = T{0}, mean_sp = T{0}; + for (std::size_t i = 0; i < m; ++i) { + mean_sth += sth[i]; + mean_sp += sp[i]; + } + mean_sth /= static_cast(m); + mean_sp /= static_cast(m); + + ++n_measured; + sum_cons_yaw += static_cast(cons_yaw); + sum_cons_tr += static_cast(cons_tr); + sum_real_yaw += dyaw; + sum_real_tr += dtr; + if (dyaw > max_real_yaw) + max_real_yaw = dyaw; + + json j{{"index", n_updates - 1}, + {"t", frame_t}, + {"n_obs", m}, + {"accepted", accepted}, + {"nis_over_dof", nis.valid && nis.dof > 0 ? static_cast(nis.value) / nis.dof : 0.0}, + {"clone_sigma_theta_deg", static_cast(mean_sth) * 180.0 / 3.14159265358979323846}, + {"clone_sigma_p_mm", static_cast(mean_sp) * 1000.0}, + {"leak_yaw_consistent", static_cast(cons_yaw)}, + {"leak_trans_consistent", static_cast(cons_tr)}, + {"leak_yaw_real", dyaw}, + {"leak_trans_real", dtr}}; + os << j.dump() << '\n'; + }); + + std::size_t imu_idx = 0; + std::uint64_t processed = 0; + for (const auto& frame : images) { + if (processed >= args.max_frames) + break; + std::size_t end = imu_idx; + while (end < imu.size() && imu[end].timestamp_s <= frame.t_s) + ++end; + if (end > imu_idx) { + est.feed_imu(std::span>{imu.data() + imu_idx, end - imu_idx}); + imu_idx = end; + } + cv::OwnedImage img; + try { + img = cv::read_png(frame.path); + } catch (const std::exception&) { + continue; + } + frame_t = frame.t_s; + est.feed_image(frame.t_s, std::as_const(img).view()); + ++processed; + } + os.flush(); + + const double nm = n_measured ? static_cast(n_measured) : 1.0; + const double ry = sum_real_yaw / nm, rt = sum_real_tr / nm; + std::cout << "obs_inspect: processed " << processed << " frames, " << n_updates << " updates (" << n_measured + << " measured)\n" + << " consistent leak (sanity, want ~0): yaw " << sum_cons_yaw / nm << " trans " << sum_cons_tr / nm + << "\n" + << " real leak @ filter's own clone sigma: yaw " << ry << " trans " << rt << "\n" + << " max real yaw leak: " << max_real_yaw << "\n" + << " -> " + << (ry > 4.0 * rt + 1e-12 ? "YAW dominates: over-confidence enters the yaw gauge" + : "no clear yaw dominance") + << "\n records: " << args.out << "/observability.jsonl\n"; + return 0; +}