diff --git a/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp b/sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp index bbcc253..bde2376 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,28 @@ 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}); + // δ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; diff --git a/tests/tools/invariant_backend_adapter.cpp b/tests/tools/invariant_backend_adapter.cpp new file mode 100644 index 0000000..1f3b836 --- /dev/null +++ b/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 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()); + + // 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> 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); + } + const auto before_t = a.current_state().timestamp_s; + a.process_camera(1.0, std::span>{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); +} 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..20314a6 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 @@ -62,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"; @@ -144,6 +171,16 @@ 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); + 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) { std::ostringstream o; o << '['; @@ -227,6 +264,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) { @@ -452,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; @@ -472,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); @@ -494,6 +521,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 +545,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,8 +570,92 @@ 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); + 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; +} + +// ── 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; + 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; + } + + const auto cam0 = euroc_cam0(); + Estimator est(Adapter(cam0.intrinsics, cam0.R_imu_cam, cam0.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); + 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; } @@ -587,12 +700,21 @@ 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 = " + // 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"; + if (args.video && !args.invariant) std::cout << " overlay: node docs-site/scripts/gen-overlay.mjs " << args.out << "\n"; return 0; }