feat(tools): obs_inspect R-IEKF vs standard yaw-leak comparison

tests/tools/observability_inspect.cpp

@@ -98,3 +98,34 @@ TEST_CASE("observability inspector: perturbing the window leaks yaw but not tran
     REQUIRE(yaw_small > 1e-6);
     REQUIRE(yaw_big > yaw_small);
 }
+
+TEST_CASE("observability inspector: the R-IEKF parameterization does not leak yaw under perturbation",
+          "[tools][obs_inspect]") {
+    const ms::CameraExtrinsics<T> ext{};  // identity extrinsic
+    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 Ninv = bt::obs_build_N_invariant<T>(truth, p_f, g);
+
+    // Consistent point: the shipped invariant Jacobian annihilates the invariant gauge.
+    const auto [tr0, yaw0] = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(truth, p_f, ext), Ninv);
+    REQUIRE_THAT(tr0, WithinAbs(0.0, 1e-9));
+    REQUIRE_THAT(yaw0, WithinAbs(0.0, 1e-9));
+
+    // Same clone-window perturbation that makes the STANDARD yaw leak (previous
+    // test). The invariant gauge directions are constants (rho = e_k, phi = g), so
+    // the leak must STAY ~0 — yaw observable by construction. This is the fix.
+    std::mt19937_64 rng(0x0B5E11ull);
+    auto e = truth;
+    for (auto& c : e) {
+        c.R =
+            c.R *
+            SO3::exp(Vec3{{0.05 * bt::obs_urand<T>(rng), 0.05 * bt::obs_urand<T>(rng), 0.05 * bt::obs_urand<T>(rng)}});
+        c.p = Vec3{{c.p[0] + 0.05 * bt::obs_urand<T>(rng),
+                    c.p[1] + 0.05 * bt::obs_urand<T>(rng),
+                    c.p[2] + 0.05 * bt::obs_urand<T>(rng)}};
+    }
+    const auto [tr_p, yaw_p] = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(e, p_f, ext), Ninv);
+    REQUIRE_THAT(tr_p, WithinAbs(0.0, 1e-9));
+    REQUIRE_THAT(yaw_p, WithinAbs(0.0, 1e-9));  // flat — unlike the standard filter
+}

tools/README.md

@@ -355,12 +355,17 @@ per-clone σ from P, N at the unperturbed gauge).
 ```
 
 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.
+consistent/real/invariant 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 **standard 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.
+
+It also measures the **right-invariant (R-IEKF) leak on the same perturbed clone
+windows** (driving the shipped `build_invariant_measurement`): because the
+invariant gauge directions are estimate-independent constants (ρ = e_k, φ = ĝ), the
+invariant yaw leak stays ~0 where the standard one rises — the real-data evidence
+that R-IEKF is observable-by-construction and is the fix the diagnosis points to.
 
 ## Layout
 

tools/include/branes/tools/observability_inspect.hpp

@@ -29,6 +29,7 @@
 #include <branes/math/lie/so3.hpp>
 #include <branes/sdk/msckf/camera_updater.hpp>
 #include <branes/sdk/msckf/dense.hpp>
+#include <branes/sdk/msckf/invariant_update.hpp>
 #include <branes/sdk/msckf/state.hpp>
 
 #include <cmath>
@@ -106,6 +107,62 @@ template <math::Scalar T>
     return N;
 }
 
+/// The RIGHT-INVARIANT (R-IEKF) stacked Jacobian over the clone window, driving the
+/// SHIPPED build_invariant_measurement (invariant_update.hpp) — the world-frame
+/// twist parameterization ξ_c = (φ, ρ). Same column layout as obs_build_H so it
+/// composes with obs_build_N_invariant / obs_leak: [ per clone φ(3) ρ(3) | feature
+/// δp(3) ]. This is the candidate fix's Jacobian, measured on the same real geometry.
+template <math::Scalar T>
+[[nodiscard]] sdk::msckf::DynMat<T> obs_build_H_invariant(const std::vector<ObsPose<T>>& cl,
+                                                          const math::lie::detail::Vec<T, 3>& p_f,
+                                                          const sdk::msckf::CameraExtrinsics<T>& ext) {
+    const std::size_t m = cl.size();
+    std::vector<sdk::msckf::InvariantClone<T>> ic;
+    ic.reserve(m);
+    for (const auto& c : cl)
+        ic.push_back({c.R, c.p});
+    std::vector<sdk::msckf::InvariantObs<T>> obs;
+    obs.reserve(m);
+    for (std::size_t c = 0; c < m; ++c)
+        obs.push_back({c, 0, {}});  // xy only enters the residual, not H
+    const std::vector<sdk::msckf::InvariantCalib<T>> cal{{ext.R_imu_cam, ext.p_imu_cam}};
+    const auto M = sdk::msckf::build_invariant_measurement<T>(ic, obs, p_f, cal, /*estimate_calib=*/false);
+    sdk::msckf::DynMat<T> H(2 * m, 6 * m + 3);
+    if (!M.ok)
+        return H;               // degenerate window ⇒ zeros (leak 0)
+    const std::size_t n = M.n;  // 6*m (no calib columns)
+    for (std::size_t row = 0; row < 2 * m; ++row) {
+        for (std::size_t cc = 0; cc < 6 * m; ++cc)
+            H(row, cc) = M.H_x[row * n + cc];  // [φ_c ρ_c] per clone
+        for (std::size_t b = 0; b < 3; ++b)
+            H(row, 6 * m + b) = M.H_f[row * 3 + b];  // feature
+    }
+    return H;
+}
+
+/// The invariant 4-DoF gauge. Crucially the CLONE directions are CONSTANTS (no
+/// estimate): translation ρ = e_k, yaw φ = ĝ for EVERY clone — so a perturbed
+/// window cannot break the annihilation. Only the (marginalized) feature carries
+/// the estimate. Cols 0–2 translation, col 3 yaw.
+template <math::Scalar T>
+[[nodiscard]] sdk::msckf::DynMat<T> obs_build_N_invariant(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 ρ = e_k (constant)
+            N(6 * c + k, 3) = g[k];      // yaw: clone φ = ĝ (constant, estimate-independent)
+        }
+    for (std::size_t k = 0; k < 3; ++k)
+        N(6 * m + k, k) = T{1};  // translation: feature δp
+    const math::lie::detail::Vec<T, 3> dpf = math::lie::detail::hat(g) * p_f;
+    for (std::size_t k = 0; k < 3; ++k)
+        N(6 * m + k, 3) = dpf[k];  // yaw: feature [g]× p_f
+    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>

tools/src/obs_inspect.cpp

@@ -28,6 +28,11 @@
 //     uncertainty the filter actually reports on this window: given that
 //     uncertainty, how much yaw/translation information does the shipped H
 //     spuriously inject?
+//   * R-IEKF leak -- the SAME perturbed windows run through the shipped
+//     right-invariant Jacobian (build_invariant_measurement) + the invariant gauge,
+//     whose clone directions are estimate-independent constants. On V2_03 this is
+//     ~1e-16 where the standard leak is ~0.4 -- the candidate fix, validated on
+//     real data.
 //
 // 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
@@ -203,6 +208,7 @@ int main(int argc, char** argv) {
     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;
+    double sum_inv_yaw = 0.0, max_inv_yaw = 0.0;  // R-IEKF leak at the SAME perturbations
 
     est.backend().set_update_observer([&](const ms::State<T>& s,
                                           const ms::FeatureTrack<T>& track,
@@ -242,8 +248,12 @@ int main(int argc, char** argv) {
         const auto [cons_tr, cons_yaw] = bt::obs_leak<T>(bt::obs_build_H<T>(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;
+        // sigma, N at the unperturbed gauge, averaged over seeded draws. For each
+        // perturbed window also measure the RIGHT-INVARIANT (R-IEKF) leak — the
+        // candidate fix — on the SAME geometry: its gauge directions are constants,
+        // so it should stay ~0 where the standard one leaks.
+        const ms::DynMat<T> Ninv = bt::obs_build_N_invariant<T>(cl, p_f, g);
+        double dyaw = 0.0, dtr = 0.0, dyaw_inv = 0.0;
         std::mt19937_64 rng(0x0B5E11ull ^ (n_updates * 0x9E3779B97F4A7C15ull));
         for (int d = 0; d < args.draws; ++d) {
             std::vector<ObsPose<T>> pe = cl;
@@ -258,9 +268,12 @@ int main(int argc, char** argv) {
             const auto [tr, yaw] = bt::obs_leak<T>(bt::obs_build_H<T>(updater, pe, p_f), N);
             dyaw += static_cast<double>(yaw);
             dtr += static_cast<double>(tr);
+            const auto inv = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(pe, p_f, cal.extrinsics), Ninv);
+            dyaw_inv += static_cast<double>(inv.second);
         }
         dyaw /= args.draws;
         dtr /= args.draws;
+        dyaw_inv /= args.draws;
 
         T mean_sth = T{0}, mean_sp = T{0};
         for (std::size_t i = 0; i < m; ++i) {
@@ -275,8 +288,11 @@ int main(int argc, char** argv) {
         sum_cons_tr += static_cast<double>(cons_tr);
         sum_real_yaw += dyaw;
         sum_real_tr += dtr;
+        sum_inv_yaw += dyaw_inv;
         if (dyaw > max_real_yaw)
             max_real_yaw = dyaw;
+        if (dyaw_inv > max_inv_yaw)
+            max_inv_yaw = dyaw_inv;
 
         json j{{"index", n_updates - 1},
                {"t", frame_t},
@@ -288,7 +304,8 @@ int main(int argc, char** argv) {
                {"leak_yaw_consistent", static_cast<double>(cons_yaw)},
                {"leak_trans_consistent", static_cast<double>(cons_tr)},
                {"leak_yaw_real", dyaw},
-               {"leak_trans_real", dtr}};
+               {"leak_trans_real", dtr},
+               {"leak_yaw_invariant", dyaw_inv}};
         os << j.dump() << '\n';
     });
 
@@ -317,16 +334,18 @@ int main(int argc, char** argv) {
     os.flush();
 
     const double nm = n_measured ? static_cast<double>(n_measured) : 1.0;
-    const double ry = sum_real_yaw / nm, rt = sum_real_tr / nm;
+    const double ry = sum_real_yaw / nm, rt = sum_real_tr / nm, iy = sum_inv_yaw / 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
+              << "  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"
+              << "  STANDARD leak @ filter's own clone sigma:  yaw " << ry << "  trans " << rt << "  (max yaw "
+              << max_real_yaw << ")\n"
+              << "  R-IEKF leak @ the SAME perturbations:      yaw " << iy << "  (max yaw " << max_inv_yaw << ")\n"
               << "  -> "
-              << (ry > 4.0 * rt + 1e-12 ? "YAW dominates: over-confidence enters the yaw gauge"
-                                        : "no clear yaw dominance")
+              << (ry > 4.0 * rt + 1e-12 ? "YAW dominates the standard filter; " : "no clear standard yaw dominance; ")
+              << (iy < ry / 10.0 + 1e-12 ? "R-IEKF flattens it (observable by construction)"
+                                         : "R-IEKF does NOT flatten it")
               << "\n  records: " << args.out << "/observability.jsonl\n";
     return 0;
 }