[AZ-332] C1 OKVIS2 Strategy: facade + binding skeleton

Python facade (`Okvis2Strategy`) is production-quality and satisfies
AZ-331's `VioStrategy` protocol; full AC-1..10 coverage with
AC-9 + NFR-perf marked `tier2`. The C++ pybind11 binding compiles
and loads but throws `OkvisFatalException("estimator not yet wired")`
on first `add_frame` — the `okvis::ThreadedKFVio` wiring is a tier2
follow-up the Step-15 Product Completeness Gate is expected to track
as a remediation task.

Resolved contradictions:

* Constructor signature aligned with the AZ-331 factory: `(config, *,
  fdr_client, clock=None)`. Calibration / preintegrator / logger
  built internally from config. No churn on AZ-331.
* IMU substrate: OKVIS2 owns its internal estimator IMU integration;
  the AZ-276 `ImuPreintegrator` is a separate substrate consumed by
  E-C5's fusion graph. Single source of truth lives at the sample
  stream, not the integrator instance.
* FDR API: `FdrClient.enqueue(record)` with new `vio.health` kind
  added to AZ-272 `KNOWN_PAYLOAD_KEYS`.

CI matrix forces `-DBUILD_OKVIS2=OFF` until the tier2 wiring task
brings Ceres / SuiteSparse / OKVIS2 vendored submodules into the
Linux build.

Files: 17 added/modified across `c1_vio/`, `fdr_client/records.py`,
`cpp/okvis2/CMakeLists.txt`, CI workflow, AZ-332 task spec
(implementation-notes section), batch 23 report.

Tests: 17 new (15 tier1 + 2 tier2). Full Tier-1 suite: 1109 pass,
2 skipped (env), 2 deselected (tier2). No regressions.

Co-authored-by: Cursor <cursoragent@cursor.com>

src/gps_denied_onboard/components/c1_vio/__init__.py

@@ -25,7 +25,7 @@ from gps_denied_onboard._types.nav import (
     VioState,
     WarmStartPose,
 )
-from gps_denied_onboard.components.c1_vio.config import C1VioConfig
+from gps_denied_onboard.components.c1_vio.config import C1VioConfig, Okvis2Config
 from gps_denied_onboard.components.c1_vio.errors import (
     VioDegradedError,
     VioError,
@@ -40,6 +40,7 @@ register_component_block("c1_vio", C1VioConfig)
 __all__ = [
     "C1VioConfig",
     "FeatureQuality",
+    "Okvis2Config",
     "VioDegradedError",
     "VioError",
     "VioFatalError",

src/gps_denied_onboard/components/c1_vio/_native/okvis2_binding.cpp

@@ -0,0 +1,318 @@
+// AZ-332 — pybind11 binding for OKVIS2 (production-default C1 VIO).
+//
+// Exposes a narrow surface that mirrors what the Python facade
+// (`gps_denied_onboard.components.c1_vio.okvis2.Okvis2Strategy`)
+// needs — NOT the full OKVIS2 estimator API. The surface is:
+//
+//   Okvis2Backend
+//     ctor(yaml_config: str, camera_intrinsics_3x3: ndarray[float64, 3, 3])
+//     add_frame(frame_id: str, ts_ns: int, image: ndarray[uint8, H, W, C]) -> bool
+//     add_imu(ts_ns: int, accel: ndarray[float64, 3], gyro: ndarray[float64, 3]) -> None
+//     get_latest_output() -> dict | None
+//     reset(body_T_world: ndarray[float64, 4, 4], velocity: ndarray[float64, 3],
+//           accel_bias: ndarray[float64, 3], gyro_bias: ndarray[float64, 3]) -> None
+//     health() -> dict
+//
+// Frame buffers cross the FFI boundary as `py::array_t<uint8_t,
+// c_style|forcecast>` so the camera-ingest path (AZ-265
+// LiveCameraFrameSource) can hand off a contiguous numpy array without a
+// copy — Risk-2 mitigation per the AZ-332 task spec.
+//
+// Exception envelope: every OKVIS2 / Eigen / std::runtime_error inside a
+// binding method is caught and rethrown as one of three Python-side
+// exceptions registered via `py::register_exception`. The Python facade
+// then rewraps those into the VioError family.
+
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+#include <pybind11/stl.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <array>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <stdexcept>
+#include <string>
+
+// OKVIS2 estimator headers. The exact include path is determined by the
+// vendored upstream's CMake export. The skeleton compiles without these
+// headers because the actual ThreadedKFVio wiring lives in
+// _build_estimator() / _drive_estimator(), which today STUB and surface a
+// runtime error if invoked. Wiring them in is the follow-up task within
+// AZ-332's tier2 deliverable bundle.
+//
+// #include <okvis/ThreadedKFVio.hpp>
+// #include <okvis/Estimator.hpp>
+// #include <okvis/VioParametersReader.hpp>
+
+namespace py = pybind11;
+
+namespace {
+
+// ---------------------------------------------------------------------------
+// Exception types — registered as Python-side classes via
+// `py::register_exception` in PYBIND11_MODULE below. The Python facade
+// catches these and rewraps into the VioError family.
+
+class OkvisInitException : public std::runtime_error {
+ public:
+  using std::runtime_error::runtime_error;
+};
+
+class OkvisFatalException : public std::runtime_error {
+ public:
+  using std::runtime_error::runtime_error;
+};
+
+class OkvisOptimizationException : public std::runtime_error {
+ public:
+  using std::runtime_error::runtime_error;
+};
+
+// ---------------------------------------------------------------------------
+// Pose / output struct produced by the estimator step.
+struct EstimatorOutput {
+  std::string frame_id;
+  Eigen::Matrix4d pose_T_world_body;
+  Eigen::Matrix<double, 6, 6> pose_covariance_6x6;
+  Eigen::Vector3d accel_bias;
+  Eigen::Vector3d gyro_bias;
+  int tracked_features = 0;
+  int new_features = 0;
+  int lost_features = 0;
+  double mean_parallax = 0.0;
+  double mre_px = 0.0;
+  std::int64_t emitted_at_ns = 0;
+};
+
+// ---------------------------------------------------------------------------
+// Internal estimator state machine — INIT until N keyframes converge,
+// TRACKING during nominal operation, DEGRADED on feature-count drop,
+// LOST after consecutive failed updates.
+enum class HealthState : int { Init = 0, Tracking = 1, Degraded = 2, Lost = 3 };
+
+const char* state_to_str(HealthState s) {
+  switch (s) {
+    case HealthState::Init:
+      return "init";
+    case HealthState::Tracking:
+      return "tracking";
+    case HealthState::Degraded:
+      return "degraded";
+    case HealthState::Lost:
+      return "lost";
+  }
+  return "init";
+}
+
+// ---------------------------------------------------------------------------
+// Okvis2Backend — the C++ surface exposed to Python.
+class Okvis2Backend {
+ public:
+  Okvis2Backend(const std::string& yaml_config,
+                py::array_t<double, py::array::c_style | py::array::forcecast>
+                    camera_intrinsics_3x3)
+      : yaml_config_(yaml_config) {
+    if (camera_intrinsics_3x3.ndim() != 2 ||
+        camera_intrinsics_3x3.shape(0) != 3 ||
+        camera_intrinsics_3x3.shape(1) != 3) {
+      throw OkvisInitException(
+          "Okvis2Backend: camera_intrinsics_3x3 must be a 3x3 float64 array");
+    }
+    auto buf = camera_intrinsics_3x3.unchecked<2>();
+    for (py::ssize_t i = 0; i < 3; ++i) {
+      for (py::ssize_t j = 0; j < 3; ++j) {
+        K_(i, j) = buf(i, j);
+      }
+    }
+    _build_estimator();
+  }
+
+  // Push a nav-camera frame into the estimator.
+  // Returns true if the estimator produced a new output for this frame
+  // (caller then calls `get_latest_output()`); false if the frame was
+  // consumed but did not yield a new output (e.g. dropped as non-keyframe).
+  bool add_frame(
+      const std::string& frame_id, std::int64_t ts_ns,
+      py::array_t<std::uint8_t,
+                  py::array::c_style | py::array::forcecast> image) {
+    if (image.ndim() < 2 || image.ndim() > 3) {
+      throw OkvisOptimizationException(
+          "Okvis2Backend.add_frame: image must be 2-D (grayscale) or 3-D (HxWxC)");
+    }
+    pending_frame_id_ = frame_id;
+    pending_ts_ns_ = ts_ns;
+    return _drive_estimator(image);
+  }
+
+  void add_imu(std::int64_t ts_ns,
+               py::array_t<double,
+                           py::array::c_style | py::array::forcecast> accel,
+               py::array_t<double,
+                           py::array::c_style | py::array::forcecast> gyro) {
+    if (accel.size() != 3 || gyro.size() != 3) {
+      throw OkvisOptimizationException(
+          "Okvis2Backend.add_imu: accel and gyro must be length-3 float64 arrays");
+    }
+    if (ts_ns <= last_imu_ts_ns_) {
+      throw OkvisOptimizationException(
+          "Okvis2Backend.add_imu: ts_ns must be strict-monotonic");
+    }
+    last_imu_ts_ns_ = ts_ns;
+    // Real OKVIS2 IMU push lands here once the estimator is wired in.
+    // For the skeleton we just record the most recent sample — the
+    // estimator's IMU integration is performed inside ThreadedKFVio.
+    auto a = accel.unchecked<1>();
+    auto g = gyro.unchecked<1>();
+    last_accel_ = Eigen::Vector3d(a(0), a(1), a(2));
+    last_gyro_ = Eigen::Vector3d(g(0), g(1), g(2));
+  }
+
+  std::optional<py::dict> get_latest_output() const {
+    std::lock_guard<std::mutex> lk(output_mtx_);
+    if (!latest_output_.has_value()) {
+      return std::nullopt;
+    }
+    const auto& o = *latest_output_;
+    py::dict d;
+    d["frame_id"] = o.frame_id;
+    d["pose_T_world_body"] = py::array_t<double>(
+        {4, 4}, {sizeof(double) * 4, sizeof(double)},
+        o.pose_T_world_body.data());
+    d["pose_covariance_6x6"] = py::array_t<double>(
+        {6, 6}, {sizeof(double) * 6, sizeof(double)},
+        o.pose_covariance_6x6.data());
+    d["accel_bias"] = py::array_t<double>(
+        {3}, {sizeof(double)}, o.accel_bias.data());
+    d["gyro_bias"] = py::array_t<double>(
+        {3}, {sizeof(double)}, o.gyro_bias.data());
+    d["tracked_features"] = o.tracked_features;
+    d["new_features"] = o.new_features;
+    d["lost_features"] = o.lost_features;
+    d["mean_parallax"] = o.mean_parallax;
+    d["mre_px"] = o.mre_px;
+    d["emitted_at_ns"] = o.emitted_at_ns;
+    return d;
+  }
+
+  void reset(py::array_t<double,
+                         py::array::c_style | py::array::forcecast> body_T_world,
+             py::array_t<double,
+                         py::array::c_style | py::array::forcecast> velocity,
+             py::array_t<double,
+                         py::array::c_style | py::array::forcecast> accel_bias,
+             py::array_t<double,
+                         py::array::c_style | py::array::forcecast> gyro_bias) {
+    if (body_T_world.ndim() != 2 || body_T_world.shape(0) != 4 ||
+        body_T_world.shape(1) != 4) {
+      throw OkvisInitException(
+          "Okvis2Backend.reset: body_T_world must be a 4x4 float64 array");
+    }
+    if (velocity.size() != 3 || accel_bias.size() != 3 || gyro_bias.size() != 3) {
+      throw OkvisInitException(
+          "Okvis2Backend.reset: velocity / *_bias must be length-3 float64 arrays");
+    }
+    auto T = body_T_world.unchecked<2>();
+    for (py::ssize_t i = 0; i < 4; ++i) {
+      for (py::ssize_t j = 0; j < 4; ++j) {
+        seed_body_T_world_(i, j) = T(i, j);
+      }
+    }
+    auto v = velocity.unchecked<1>();
+    auto ab = accel_bias.unchecked<1>();
+    auto gb = gyro_bias.unchecked<1>();
+    seed_velocity_ = Eigen::Vector3d(v(0), v(1), v(2));
+    seed_accel_bias_ = Eigen::Vector3d(ab(0), ab(1), ab(2));
+    seed_gyro_bias_ = Eigen::Vector3d(gb(0), gb(1), gb(2));
+
+    state_ = HealthState::Init;
+    consecutive_lost_ = 0;
+    {
+      std::lock_guard<std::mutex> lk(output_mtx_);
+      latest_output_.reset();
+    }
+    _build_estimator();
+  }
+
+  py::dict health() const {
+    py::dict d;
+    d["state"] = std::string(state_to_str(state_));
+    d["consecutive_lost"] = consecutive_lost_;
+    d["bias_norm"] = std::sqrt(
+        seed_accel_bias_.squaredNorm() + seed_gyro_bias_.squaredNorm());
+    return d;
+  }
+
+ private:
+  void _build_estimator() {
+    // Real wiring: instantiate okvis::ThreadedKFVio from yaml_config_,
+    // attach output callback that fills latest_output_ under output_mtx_.
+    //
+    // The skeleton intentionally throws on any actual frame ingest so a
+    // production binary that loads this binding before AZ-332's
+    // estimator wiring lands cannot silently report misleading poses.
+    estimator_built_ = false;
+  }
+
+  bool _drive_estimator(
+      py::array_t<std::uint8_t,
+                  py::array::c_style | py::array::forcecast> /*image*/) {
+    if (!estimator_built_) {
+      // Skeleton path — pybind11 binding compiles and loads but the
+      // OKVIS2 estimator is not yet wired. Tier-2 follow-up wires it up.
+      throw OkvisFatalException(
+          "Okvis2Backend: OKVIS2 estimator not yet wired — this binding "
+          "is the AZ-332 skeleton; tier2 follow-up wires okvis::ThreadedKFVio");
+    }
+    return false;
+  }
+
+  std::string yaml_config_;
+  Eigen::Matrix3d K_ = Eigen::Matrix3d::Identity();
+  Eigen::Matrix4d seed_body_T_world_ = Eigen::Matrix4d::Identity();
+  Eigen::Vector3d seed_velocity_ = Eigen::Vector3d::Zero();
+  Eigen::Vector3d seed_accel_bias_ = Eigen::Vector3d::Zero();
+  Eigen::Vector3d seed_gyro_bias_ = Eigen::Vector3d::Zero();
+  Eigen::Vector3d last_accel_ = Eigen::Vector3d::Zero();
+  Eigen::Vector3d last_gyro_ = Eigen::Vector3d::Zero();
+
+  HealthState state_ = HealthState::Init;
+  int consecutive_lost_ = 0;
+  std::int64_t last_imu_ts_ns_ = -1;
+  std::string pending_frame_id_;
+  std::int64_t pending_ts_ns_ = 0;
+  bool estimator_built_ = false;
+
+  mutable std::mutex output_mtx_;
+  std::optional<EstimatorOutput> latest_output_;
+};
+
+}  // namespace
+
+PYBIND11_MODULE(okvis2_binding, m) {
+  m.doc() =
+      "OKVIS2 pybind11 binding (AZ-332). Wraps okvis::ThreadedKFVio for the "
+      "Python Okvis2Strategy facade. Tier2 follow-up wires the real estimator.";
+
+  py::register_exception<OkvisInitException>(m, "OkvisInitException");
+  py::register_exception<OkvisFatalException>(m, "OkvisFatalException");
+  py::register_exception<OkvisOptimizationException>(
+      m, "OkvisOptimizationException");
+
+  py::class_<Okvis2Backend>(m, "Okvis2Backend")
+      .def(py::init<const std::string&,
+                    py::array_t<double, py::array::c_style | py::array::forcecast>>(),
+           py::arg("yaml_config"), py::arg("camera_intrinsics_3x3"))
+      .def("add_frame", &Okvis2Backend::add_frame, py::arg("frame_id"),
+           py::arg("ts_ns"), py::arg("image"))
+      .def("add_imu", &Okvis2Backend::add_imu, py::arg("ts_ns"),
+           py::arg("accel"), py::arg("gyro"))
+      .def("get_latest_output", &Okvis2Backend::get_latest_output)
+      .def("reset", &Okvis2Backend::reset, py::arg("body_T_world"),
+           py::arg("velocity"), py::arg("accel_bias"), py::arg("gyro_bias"))
+      .def("health", &Okvis2Backend::health);
+}

src/gps_denied_onboard/components/c1_vio/bench/__init__.py

@@ -0,0 +1,6 @@
+"""C1 VIO microbench harness (AZ-332).
+
+The bench scripts are tier2 / Jetson-only — they exercise the real OKVIS2
+binding (or fake binding for cross-platform smoke) and report per-frame
+latency percentiles for C1-PT-01 / NFT-PERF-01.
+"""

src/gps_denied_onboard/components/c1_vio/bench/okvis2.py

@@ -0,0 +1,196 @@
+"""``python -m gps_denied_onboard.components.c1_vio.bench.okvis2`` (AZ-332).
+
+Microbench for :class:`Okvis2Strategy` — reads a fixture directory of
+nav-camera frames + IMU samples and reports per-frame latency
+percentiles for C1-PT-01 (p50 <= 25 ms, p95 <= 80 ms, threshold 120 ms).
+
+The bench produces production behaviour: it constructs the real
+strategy via the AZ-331 factory (so ``BUILD_OKVIS2=ON`` is required),
+feeds real frames through, and measures wall-clock per call. On Tier-2
+this measures OKVIS2's actual estimator latency; on a workstation with
+``BUILD_OKVIS2=OFF`` it refuses to start (Risk-2 — never silently
+benchmark a stub).
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import sys
+import time
+from datetime import datetime, timezone
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from gps_denied_onboard._types.nav import (
+    ImuSample,
+    ImuWindow,
+    NavCameraFrame,
+)
+from gps_denied_onboard.components.c1_vio.config import (
+    C1VioConfig,
+    Okvis2Config,
+)
+from gps_denied_onboard.config.schema import Config, RuntimeConfig
+from gps_denied_onboard.fdr_client.client import make_fdr_client
+from gps_denied_onboard.runtime_root.vio_factory import build_vio_strategy
+
+
+def _percentile(samples_ms: list[float], pct: float) -> float:
+    if not samples_ms:
+        return float("nan")
+    sorted_samples = sorted(samples_ms)
+    idx = min(len(sorted_samples) - 1, int(pct * len(sorted_samples)))
+    return sorted_samples[idx]
+
+
+def _load_fixture(fixture_dir: Path) -> tuple[Any, list[NavCameraFrame], list[ImuWindow]]:
+    """Fixture format (minimal, deterministic):
+
+    .. code::
+
+        fixture_dir/
+          manifest.json    { "frame_count": N, "camera_calibration_path": "..." }
+          frames/0000.npy  uint8 image
+          ...
+          imu/0000.json    {"samples": [{"ts_ns": N, "accel": [..], "gyro": [..]}, ...]}
+          ...
+    """
+    manifest_path = fixture_dir / "manifest.json"
+    if not manifest_path.is_file():
+        raise FileNotFoundError(f"missing manifest.json under {fixture_dir!r}")
+    manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
+
+    frames: list[NavCameraFrame] = []
+    imu_windows: list[ImuWindow] = []
+    frame_count = int(manifest["frame_count"])
+    for i in range(frame_count):
+        img_path = fixture_dir / "frames" / f"{i:04d}.npy"
+        imu_path = fixture_dir / "imu" / f"{i:04d}.json"
+        img = np.load(img_path)
+        imu_blob = json.loads(imu_path.read_text(encoding="utf-8"))
+        samples = tuple(
+            ImuSample(
+                ts_ns=int(s["ts_ns"]),
+                accel_xyz=tuple(s["accel"]),
+                gyro_xyz=tuple(s["gyro"]),
+            )
+            for s in imu_blob["samples"]
+        )
+        if not samples:
+            raise ValueError(
+                f"bench.okvis2: fixture frame {i} ({imu_path}) has no IMU "
+                "samples — bench requires a real IMU window per frame"
+            )
+        ts_start = samples[0].ts_ns
+        ts_end = samples[-1].ts_ns
+        imu_windows.append(ImuWindow(samples=samples, ts_start_ns=ts_start, ts_end_ns=ts_end))
+        frames.append(
+            NavCameraFrame(
+                frame_id=i,
+                timestamp=datetime.fromtimestamp(ts_start * 1e-9, tz=timezone.utc),
+                image=img,
+                camera_calibration_id=str(manifest.get("camera_calibration_id", "bench")),
+            )
+        )
+    return manifest, frames, imu_windows
+
+
+def _make_calibration(intrinsics_path: str | None) -> Any:
+    """Build a CameraCalibration with no body-to-camera (identity)
+    using the bench's calibration JSON if supplied; otherwise raise.
+    """
+    from gps_denied_onboard._types.calibration import CameraCalibration
+
+    if intrinsics_path is None:
+        raise ValueError("bench.okvis2: --camera-calibration is required (real intrinsics)")
+    blob = json.loads(Path(intrinsics_path).read_text(encoding="utf-8"))
+    return CameraCalibration(
+        camera_id=blob.get("camera_id", "bench"),
+        intrinsics_3x3=np.asarray(blob["intrinsics_3x3"], dtype=np.float64),
+        distortion=np.asarray(blob.get("distortion", [0, 0, 0, 0]), dtype=np.float64),
+        body_to_camera_se3=np.eye(4, dtype=np.float64),
+        acquisition_method=blob.get("acquisition_method", "bench-static"),
+        metadata=dict(blob.get("metadata", {})),
+    )
+
+
+def main(argv: list[str] | None = None) -> int:
+    parser = argparse.ArgumentParser(
+        prog="python -m gps_denied_onboard.components.c1_vio.bench.okvis2",
+        description="Microbench for Okvis2Strategy.process_frame (AZ-332 / C1-PT-01).",
+    )
+    parser.add_argument("fixture_dir", type=Path, help="Path to fixture directory")
+    parser.add_argument(
+        "--camera-calibration",
+        type=str,
+        required=True,
+        help="Path to camera calibration JSON",
+    )
+    parser.add_argument(
+        "--warmup",
+        type=int,
+        default=10,
+        help="Number of warmup frames (not counted in percentiles)",
+    )
+    args = parser.parse_args(argv)
+
+    manifest, frames, imu_windows = _load_fixture(args.fixture_dir)
+    calibration = _make_calibration(args.camera_calibration)
+
+    config = Config.with_blocks(
+        c1_vio=C1VioConfig(strategy="okvis2", okvis2=Okvis2Config()),
+        runtime=RuntimeConfig(
+            camera_calibration_path=args.camera_calibration,
+            inference_backend="tensorrt",
+            tier=2,
+        ),
+    )
+    fdr_client = make_fdr_client("c1_vio.okvis2.bench", config)
+    strategy = build_vio_strategy(config, fdr_client=fdr_client)
+
+    durations_ms: list[float] = []
+    for i, (frame, imu) in enumerate(zip(frames, imu_windows, strict=True)):
+        t0 = time.perf_counter()
+        try:
+            strategy.process_frame(frame, imu, calibration)
+        except Exception as exc:
+            print(
+                f"frame {i}: exception {type(exc).__name__}: {exc}",
+                file=sys.stderr,
+            )
+            continue
+        dt_ms = (time.perf_counter() - t0) * 1000.0
+        if i >= args.warmup:
+            durations_ms.append(dt_ms)
+
+    if not durations_ms:
+        print("bench: no successful frames after warmup", file=sys.stderr)
+        return 2
+
+    p50 = _percentile(durations_ms, 0.50)
+    p95 = _percentile(durations_ms, 0.95)
+    p99 = _percentile(durations_ms, 0.99)
+    print(
+        json.dumps(
+            {
+                "fixture_dir": str(args.fixture_dir),
+                "frame_count": manifest.get("frame_count"),
+                "measured": len(durations_ms),
+                "p50_ms": round(p50, 3),
+                "p95_ms": round(p95, 3),
+                "p99_ms": round(p99, 3),
+                "c1_pt_01_target_p50_ms": 25.0,
+                "c1_pt_01_target_p95_ms": 80.0,
+                "c1_pt_01_failure_p95_ms": 120.0,
+            },
+            indent=2,
+        )
+    )
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/gps_denied_onboard/components/c1_vio/config.py

@@ -1,27 +1,91 @@
-"""C1 VIO strategy config block (AZ-331).
+"""C1 VIO strategy config block (AZ-331 + AZ-332).
 
 Registered into ``config.components['c1_vio']`` by the package
 ``__init__.py``. The composition-root factory
 :func:`gps_denied_onboard.runtime_root.vio_factory.build_vio_strategy`
-reads this block to select the strategy and configure the LOST→FATAL
+reads this block to select the strategy and configure the LOST->FATAL
 transition + warm-start convergence budget.
+
+AZ-332 extends this with a nested :class:`Okvis2Config` sub-block
+carrying OKVIS2-specific knobs (sliding-window size, parallax-driven
+keyframe threshold, RANSAC inlier ratio, max optimisation iterations,
+degraded-feature threshold, per-frame debug log). Only consulted when
+``strategy == "okvis2"``.
 """
 
 from __future__ import annotations
 
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from typing import Final
 
 from gps_denied_onboard.config.schema import ConfigError
 
 __all__ = [
-    "C1VioConfig",
     "KNOWN_STRATEGIES",
+    "C1VioConfig",
+    "Okvis2Config",
 ]
 
-KNOWN_STRATEGIES: Final[frozenset[str]] = frozenset(
-    {"okvis2", "vins_mono", "klt_ransac"}
-)
+KNOWN_STRATEGIES: Final[frozenset[str]] = frozenset({"okvis2", "vins_mono", "klt_ransac"})
+
+
+@dataclass(frozen=True)
+class Okvis2Config:
+    """OKVIS2-specific knobs (AZ-332).
+
+    ``keyframe_window_size`` is the sliding-window keyframe count K
+    per D-C5-3 — must be in [10, 20]. Lower values lose accuracy;
+    higher values exceed the C1-PT-01 per-frame budget on Tier-2.
+
+    ``keyframe_parallax_threshold_px`` is the parallax-driven keyframe
+    decision; default 3.0 px (OKVIS2 upstream default).
+
+    ``ransac_inlier_ratio`` is the RANSAC inlier-ratio threshold below
+    which the frontend declares the frame untrackable; default 0.5.
+
+    ``max_optimization_iters`` caps the per-frame Levenberg-Marquardt
+    iterations to bound worst-case latency; default 4 (OKVIS2 default).
+
+    ``degraded_feature_threshold`` is the per-frame tracked-feature
+    count below which ``health_snapshot`` reports DEGRADED; default 30.
+
+    ``per_frame_debug_log`` enables a DEBUG log line per ``process_frame``
+    — OFF by default (would flood at 3 Hz steady-state).
+    """
+
+    keyframe_window_size: int = 15
+    keyframe_parallax_threshold_px: float = 3.0
+    ransac_inlier_ratio: float = 0.5
+    max_optimization_iters: int = 4
+    degraded_feature_threshold: int = 30
+    per_frame_debug_log: bool = False
+
+    def __post_init__(self) -> None:
+        if not (10 <= self.keyframe_window_size <= 20):
+            raise ConfigError(
+                "Okvis2Config.keyframe_window_size must be in [10, 20] "
+                f"(D-C5-3 budget); got {self.keyframe_window_size}"
+            )
+        if self.keyframe_parallax_threshold_px <= 0.0:
+            raise ConfigError(
+                "Okvis2Config.keyframe_parallax_threshold_px must be > 0; "
+                f"got {self.keyframe_parallax_threshold_px}"
+            )
+        if not (0.0 < self.ransac_inlier_ratio <= 1.0):
+            raise ConfigError(
+                "Okvis2Config.ransac_inlier_ratio must be in (0.0, 1.0]; "
+                f"got {self.ransac_inlier_ratio}"
+            )
+        if self.max_optimization_iters < 1:
+            raise ConfigError(
+                "Okvis2Config.max_optimization_iters must be >= 1; "
+                f"got {self.max_optimization_iters}"
+            )
+        if self.degraded_feature_threshold < 1:
+            raise ConfigError(
+                "Okvis2Config.degraded_feature_threshold must be >= 1; "
+                f"got {self.degraded_feature_threshold}"
+            )
 
 
 @dataclass(frozen=True)
@@ -39,25 +103,26 @@ class C1VioConfig:
 
     ``warm_start_max_frames`` is the convergence budget after
     :meth:`VioStrategy.reset_to_warm_start`; default 5.
+
+    ``okvis2`` carries OKVIS2-specific knobs (AZ-332); consulted only
+    when ``strategy == "okvis2"``.
     """
 
     strategy: str = "klt_ransac"
     lost_frame_threshold: int = 9
     warm_start_max_frames: int = 5
+    okvis2: Okvis2Config = field(default_factory=Okvis2Config)
 
     def __post_init__(self) -> None:
         if self.strategy not in KNOWN_STRATEGIES:
             raise ConfigError(
-                f"C1VioConfig.strategy={self.strategy!r} not in "
-                f"{sorted(KNOWN_STRATEGIES)}"
+                f"C1VioConfig.strategy={self.strategy!r} not in {sorted(KNOWN_STRATEGIES)}"
             )
         if self.lost_frame_threshold < 1:
             raise ConfigError(
-                f"C1VioConfig.lost_frame_threshold must be >= 1; "
-                f"got {self.lost_frame_threshold}"
+                f"C1VioConfig.lost_frame_threshold must be >= 1; got {self.lost_frame_threshold}"
             )
         if self.warm_start_max_frames < 1:
             raise ConfigError(
-                f"C1VioConfig.warm_start_max_frames must be >= 1; "
-                f"got {self.warm_start_max_frames}"
+                f"C1VioConfig.warm_start_max_frames must be >= 1; got {self.warm_start_max_frames}"
             )

src/gps_denied_onboard/components/c1_vio/okvis2.py

@@ -0,0 +1,488 @@
+"""`Okvis2Strategy` — production-default C1 VIO (AZ-332).
+
+Python facade over the OKVIS2 C++ tightly-coupled keyframe-based VIO
+core, accessed via the pybind11 binding at
+``_native.okvis2_binding.Okvis2Backend`` (compiled by
+``cpp/okvis2/CMakeLists.txt``, gated by ``BUILD_OKVIS2=ON``).
+
+Conforms to the AZ-331 :class:`VioStrategy` Protocol; consumes the
+runtime ``Config`` + an :class:`FdrClient`; constructs its other
+dependencies (logger, camera calibration, preintegrator) internally
+from ``config`` so the strategy class matches the composition-root
+factory shape::
+
+    strategy_cls(config: Config, *, fdr_client: FdrClient)
+
+Risk-2 mitigation: the native binding is imported **lazily inside the
+constructor**, not at module top level. Importing this module with
+``BUILD_OKVIS2=OFF`` (no compiled ``.so``) is safe — the factory's
+build-flag gate catches that path before the constructor runs.
+
+AC mapping (see ``_docs/02_tasks/todo/AZ-332_c1_okvis2_strategy.md``):
+
+- AC-1 : :meth:`current_strategy_label` returns ``"okvis2"``.
+- AC-2 : :meth:`process_frame` returns :class:`VioOutput` with
+  ``frame_id`` echoed; covariance SPD; ``imu_bias`` non-None.
+- AC-3 : all backend / Eigen / std::runtime_error rewrap into
+  :class:`VioError` family with ``__cause__`` chain.
+- AC-4 : :meth:`reset_to_warm_start` clears state + seeds hint; second
+  consecutive call does not raise.
+- AC-5 : :meth:`health_snapshot` returns INIT initially, TRACKING after
+  ``warm_start_max_frames`` (default 5) successful frames.
+- AC-6 : DEGRADED on feature loss; covariance Frobenius norm strictly
+  increases; ``process_frame`` still returns :class:`VioOutput` (not raise).
+- AC-7 : after ``lost_frame_threshold`` (default 9) consecutive failed
+  frames, raises :class:`VioFatalError`; state == LOST.
+- AC-8 : ``BUILD_OKVIS2=OFF`` does not load this module (enforced by
+  AZ-331 factory; covered in
+  ``tests/unit/c1_vio/test_protocol_conformance.py``).
+- AC-9 / NFR-perf : tier2 — Jetson + Derkachi-class fixture; tests
+  marked ``@pytest.mark.tier2``.
+- AC-10 : exactly one ``vio.health`` FDR record per state transition;
+  no spam on steady-state.
+"""
+
+from __future__ import annotations
+
+import math
+from datetime import datetime, timezone
+from typing import TYPE_CHECKING, Any, Final, Literal
+
+import numpy as np
+
+from gps_denied_onboard._types.nav import (
+    FeatureQuality,
+    ImuBias,
+    VioHealth,
+    VioOutput,
+    VioState,
+)
+from gps_denied_onboard.clock.wall_clock import WallClock
+from gps_denied_onboard.components.c1_vio.errors import (
+    VioFatalError,
+    VioInitializingError,
+)
+from gps_denied_onboard.fdr_client.records import CURRENT_SCHEMA_VERSION, FdrRecord
+from gps_denied_onboard.logging import get_logger
+
+if TYPE_CHECKING:
+    import numpy.typing as npt
+
+    from gps_denied_onboard._types.calibration import CameraCalibration
+    from gps_denied_onboard._types.nav import (
+        ImuWindow,
+        NavCameraFrame,
+        WarmStartPose,
+    )
+    from gps_denied_onboard.clock import Clock
+    from gps_denied_onboard.components.c1_vio.config import Okvis2Config
+    from gps_denied_onboard.config import Config
+    from gps_denied_onboard.fdr_client.client import FdrClient
+
+__all__ = ["Okvis2Strategy"]
+
+
+_STRATEGY_LABEL: Final[Literal["okvis2"]] = "okvis2"
+_PRODUCER_ID: Final[str] = "c1_vio.okvis2"
+_LOGGER_COMPONENT: Final[str] = "c1_vio.okvis2"
+_BIAS_NORM_FLOOR: Final[float] = 0.0
+
+
+def _now_iso() -> str:
+    return datetime.now(timezone.utc).isoformat()
+
+
+def _bias_norm(bias: ImuBias) -> float:
+    """L2 norm of the concatenated 6-vector ``(accel || gyro)``."""
+    accel = np.asarray(bias.accel_bias, dtype=np.float64)
+    gyro = np.asarray(bias.gyro_bias, dtype=np.float64)
+    return float(math.sqrt(float(np.dot(accel, accel) + np.dot(gyro, gyro))))
+
+
+def _se3_from_4x4(matrix: npt.NDArray[Any]) -> Any:
+    """Build a ``gtsam.Pose3`` from a 4x4 row-major matrix.
+
+    Imported lazily so this module can be imported without gtsam in
+    headless tooling paths (tests + facade-only smoke).
+    """
+    import gtsam
+
+    return gtsam.Pose3(np.asarray(matrix, dtype=np.float64))
+
+
+class Okvis2Strategy:
+    """Production-default :class:`VioStrategy` for E-C1 (AZ-332).
+
+    Constructor matches the AZ-331 composition-root factory shape::
+
+        Okvis2Strategy(config: Config, *, fdr_client: FdrClient)
+
+    Other dependencies (calibration, preintegrator-substrate, logger,
+    OKVIS2 sub-config) are resolved internally from ``config``.
+
+    Concurrency: single-threaded by Protocol invariant. One instance
+    per camera-ingest writer thread; concurrent ``process_frame`` calls
+    are undefined behaviour.
+    """
+
+    def __init__(
+        self,
+        config: Config,
+        *,
+        fdr_client: FdrClient,
+        clock: Clock | None = None,
+    ) -> None:
+        c1_block = config.components["c1_vio"]
+        if c1_block.strategy != _STRATEGY_LABEL:
+            raise VioFatalError(
+                f"Okvis2Strategy constructed with config.strategy="
+                f"{c1_block.strategy!r}; expected {_STRATEGY_LABEL!r}. "
+                "The AZ-331 factory is the only sanctioned constructor."
+            )
+
+        self._config = config
+        self._fdr = fdr_client
+        self._clock: Clock = clock if clock is not None else WallClock()
+        self._logger = get_logger(_LOGGER_COMPONENT)
+        self._lost_frame_threshold: int = c1_block.lost_frame_threshold
+        self._warm_start_max_frames: int = c1_block.warm_start_max_frames
+        self._okvis2_cfg: Okvis2Config = c1_block.okvis2
+        self._calibration: CameraCalibration | None = None
+        self._frames_since_warmup: int = 0
+        self._consecutive_lost: int = 0
+        self._latest_bias: ImuBias = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0))
+        self._reported_state: VioState = VioState.INIT
+        self._last_emitted_state: VioState | None = None
+
+        # Lazy import of the native binding — Risk-2 mitigation (I-5).
+        # Failure here is the BUILD_OKVIS2=OFF path the AZ-331 factory's
+        # `StrategyNotAvailableError` is meant to prevent; if a caller
+        # bypasses the factory and reaches this constructor with the
+        # native lib absent, we surface a fatal init error.
+        try:
+            from gps_denied_onboard.components.c1_vio._native import (
+                okvis2_binding,
+            )
+        except ImportError as exc:
+            raise VioFatalError(
+                "Okvis2Strategy: native binding "
+                "(gps_denied_onboard.components.c1_vio._native.okvis2_binding) "
+                "is not importable. Production binary must be built with "
+                "BUILD_OKVIS2=ON."
+            ) from exc
+
+        self._binding_module = okvis2_binding
+        self._backend = self._construct_backend()
+
+    # ------------------------------------------------------------------
+    # Public Protocol surface.
+
+    def process_frame(
+        self,
+        frame: NavCameraFrame,
+        imu: ImuWindow,
+        calibration: CameraCalibration,
+    ) -> VioOutput:
+        """Hot-path call — one per nav-camera frame.
+
+        Steps:
+
+        1. Push every IMU sample in the window into the backend; the
+           strict-monotonic guard lives on the C++ side.
+        2. Submit the frame.
+        3. If the backend produced an output, classify health and
+           build the :class:`VioOutput` DTO.
+        4. If no output: tick the lost-frame counter; emit a state
+           transition record if needed.
+        """
+        self._calibration = calibration
+        frame_id_str = str(frame.frame_id)
+        emitted_at_ns = self._clock.monotonic_ns()
+
+        try:
+            self._push_imu_window(imu)
+            produced = self._backend.add_frame(
+                frame_id_str, _frame_ts_ns(frame), _frame_image(frame)
+            )
+        except self._binding_module.OkvisInitException as exc:
+            self._emit_transition(VioState.INIT, frame_id_str)
+            raise VioInitializingError(
+                f"OKVIS2 backend reports INIT while processing frame {frame_id_str!r}: {exc}"
+            ) from exc
+        except self._binding_module.OkvisOptimizationException as exc:
+            # Treat as a degraded frame: emit no VioOutput from this
+            # path — callers expect either a VioOutput or a VioError;
+            # we choose error here so C5 can fall back, matching AC-3.
+            self._tick_lost(frame_id_str)
+            if self._reported_state == VioState.LOST:
+                self._emit_transition(VioState.LOST, frame_id_str)
+                raise VioFatalError(
+                    f"OKVIS2 backend exhausted lost-frame budget at {frame_id_str!r}: {exc}"
+                ) from exc
+            self._emit_transition(self._reported_state, frame_id_str)
+            raise VioInitializingError(
+                f"OKVIS2 backend optimisation failure at {frame_id_str!r}: {exc}"
+            ) from exc
+        except self._binding_module.OkvisFatalException as exc:
+            self._emit_transition(VioState.LOST, frame_id_str)
+            raise VioFatalError(
+                f"OKVIS2 backend fatal exception at {frame_id_str!r}: {exc}"
+            ) from exc
+        except (RuntimeError, ValueError) as exc:
+            # Catch-all for unmapped backend exceptions. Re-classify as
+            # fatal — we explicitly forbid raw library exceptions across
+            # the public boundary.
+            raise VioFatalError(
+                f"OKVIS2 backend raised an unmapped exception at {frame_id_str!r}: {exc}"
+            ) from exc
+
+        if not produced:
+            # Frame consumed but no estimator update yet — INIT path
+            # while OKVIS2 warms up its keyframe window.
+            self._emit_transition(VioState.INIT, frame_id_str)
+            raise VioInitializingError(
+                f"Okvis2Strategy: backend has not yet emitted an "
+                f"estimator update at {frame_id_str!r}"
+            )
+
+        raw = self._backend.get_latest_output()
+        if raw is None:
+            raise VioFatalError(
+                f"Okvis2Strategy: backend reported a new output for "
+                f"{frame_id_str!r} but get_latest_output() returned None"
+            )
+
+        vio_output = self._build_vio_output(raw, emitted_at_ns)
+        self._consecutive_lost = 0
+        new_state = self._classify_state(vio_output.feature_quality)
+        if new_state != self._reported_state:
+            self._reported_state = new_state
+            self._emit_transition(new_state, frame_id_str)
+
+        if new_state in (VioState.INIT, VioState.TRACKING):
+            self._frames_since_warmup += 1
+
+        if self._okvis2_cfg.per_frame_debug_log:
+            self._logger.debug(
+                "okvis2.process_frame",
+                extra={
+                    "component": _LOGGER_COMPONENT,
+                    "kind": "vio.tick",
+                    "frame_id": frame_id_str,
+                    "kv": {
+                        "state": self._reported_state.value,
+                        "tracked": vio_output.feature_quality.tracked,
+                        "mre_px": vio_output.feature_quality.mre_px,
+                        "emitted_at_ns": vio_output.emitted_at_ns,
+                    },
+                },
+            )
+
+        return vio_output
+
+    def reset_to_warm_start(self, hint: WarmStartPose) -> None:
+        """Destructive re-init from an F8-reboot warm-start hint.
+
+        Idempotent across consecutive calls (AC-4) — a second call
+        without an intervening ``process_frame`` reseats the backend
+        again without raising.
+        """
+        try:
+            body_T_world = np.asarray(hint.body_T_world.matrix(), dtype=np.float64)
+        except AttributeError as exc:
+            raise VioFatalError(
+                "Okvis2Strategy.reset_to_warm_start: hint.body_T_world is "
+                "not a gtsam.Pose3 (missing .matrix())"
+            ) from exc
+
+        velocity = np.asarray(hint.velocity_b, dtype=np.float64)
+        accel_bias = np.asarray(hint.bias.accel_bias, dtype=np.float64)
+        gyro_bias = np.asarray(hint.bias.gyro_bias, dtype=np.float64)
+
+        try:
+            self._backend.reset(body_T_world, velocity, accel_bias, gyro_bias)
+        except self._binding_module.OkvisInitException as exc:
+            raise VioFatalError(f"OKVIS2 backend rejected warm-start reset: {exc}") from exc
+        except (RuntimeError, ValueError) as exc:
+            raise VioFatalError(
+                f"OKVIS2 backend raised an unmapped exception during reset: {exc}"
+            ) from exc
+
+        self._latest_bias = hint.bias
+        self._frames_since_warmup = 0
+        self._consecutive_lost = 0
+        self._reported_state = VioState.INIT
+        self._emit_transition(VioState.INIT, frame_id="")
+
+    def health_snapshot(self) -> VioHealth:
+        """Most-recent health state — no backend call (cheap)."""
+        return VioHealth(
+            state=self._reported_state,
+            consecutive_lost=self._consecutive_lost,
+            bias_norm=_bias_norm(self._latest_bias),
+        )
+
+    def current_strategy_label(self) -> Literal["okvis2", "vins_mono", "klt_ransac"]:
+        return _STRATEGY_LABEL
+
+    # ------------------------------------------------------------------
+    # Internal helpers.
+
+    def _construct_backend(self) -> Any:
+        """Build the backend from config — calibration path is optional
+        because the unit-test fake-binding path skips real intrinsics.
+
+        Tests inject a fake module at ``sys.modules`` before construction
+        (see ``tests/unit/c1_vio/conftest.py``); the fake's
+        ``Okvis2Backend`` accepts whatever this method passes.
+        """
+        K = self._load_camera_intrinsics()
+        yaml_config = self._render_yaml_config()
+        try:
+            return self._binding_module.Okvis2Backend(yaml_config, K)
+        except self._binding_module.OkvisInitException as exc:
+            raise VioFatalError(f"Okvis2Strategy: backend init failed: {exc}") from exc
+
+    def _load_camera_intrinsics(self) -> np.ndarray:
+        """Load 3x3 camera intrinsics from the calibration path.
+
+        Returns the identity matrix when the runtime block has no
+        path configured — the unit-test path overrides this via the
+        fake binding's ctor anyway, and a production binary refusing
+        to start on a missing calibration is preferable to silently
+        emitting wrong poses (handled by the YAML loader downstream).
+        """
+        path = self._config.runtime.camera_calibration_path
+        if not path:
+            return np.eye(3, dtype=np.float64)
+        try:
+            import json
+
+            with open(path, encoding="utf-8") as fh:
+                blob = json.load(fh)
+        except (OSError, ValueError) as exc:
+            raise VioFatalError(
+                f"Okvis2Strategy: failed to load camera calibration from {path!r}: {exc}"
+            ) from exc
+        K_raw = blob.get("intrinsics_3x3")
+        if K_raw is None:
+            raise VioFatalError(
+                f"Okvis2Strategy: calibration file {path!r} is missing the 'intrinsics_3x3' field"
+            )
+        K = np.asarray(K_raw, dtype=np.float64)
+        if K.shape != (3, 3):
+            raise VioFatalError(f"Okvis2Strategy: intrinsics_3x3 must be 3x3; got shape {K.shape}")
+        return K
+
+    def _render_yaml_config(self) -> str:
+        """Render the Okvis2Config sub-block into an OKVIS2 YAML snippet.
+
+        OKVIS2 reads a YAML config string at construction. Only the knobs
+        AZ-332 exposes are rendered; OKVIS2-internal defaults cover the
+        rest.
+        """
+        cfg = self._okvis2_cfg
+        return (
+            "# AZ-332 — generated OKVIS2 config (see Okvis2Config in c1_vio/config.py)\n"
+            f"keyframe_window_size: {cfg.keyframe_window_size}\n"
+            f"keyframe_parallax_threshold_px: {cfg.keyframe_parallax_threshold_px}\n"
+            f"ransac_inlier_ratio: {cfg.ransac_inlier_ratio}\n"
+            f"max_optimization_iters: {cfg.max_optimization_iters}\n"
+        )
+
+    def _push_imu_window(self, imu: ImuWindow) -> None:
+        for sample in imu.samples:
+            self._backend.add_imu(
+                sample.ts_ns,
+                np.asarray(sample.accel_xyz, dtype=np.float64),
+                np.asarray(sample.gyro_xyz, dtype=np.float64),
+            )
+
+    def _build_vio_output(self, raw: dict[str, Any], emitted_at_ns: int) -> VioOutput:
+        try:
+            pose = _se3_from_4x4(raw["pose_T_world_body"])
+            cov = np.asarray(raw["pose_covariance_6x6"], dtype=np.float64)
+            bias = ImuBias(
+                accel_bias=tuple(float(x) for x in raw["accel_bias"]),  # type: ignore[arg-type]
+                gyro_bias=tuple(float(x) for x in raw["gyro_bias"]),  # type: ignore[arg-type]
+            )
+            feature_quality = FeatureQuality(
+                tracked=int(raw["tracked_features"]),
+                new=int(raw["new_features"]),
+                lost=int(raw["lost_features"]),
+                mean_parallax=float(raw["mean_parallax"]),
+                mre_px=float(raw["mre_px"]),
+            )
+            backend_ts = int(raw.get("emitted_at_ns") or emitted_at_ns)
+        except (KeyError, TypeError, ValueError) as exc:
+            raise VioFatalError(f"Okvis2Strategy: backend output is malformed: {exc}") from exc
+
+        if cov.shape != (6, 6):
+            raise VioFatalError(
+                f"Okvis2Strategy: pose_covariance_6x6 has shape {cov.shape}; expected (6, 6)"
+            )
+
+        self._latest_bias = bias
+        return VioOutput(
+            frame_id=raw["frame_id"],
+            relative_pose_T=pose,
+            pose_covariance_6x6=cov,
+            imu_bias=bias,
+            feature_quality=feature_quality,
+            emitted_at_ns=backend_ts,
+        )
+
+    def _classify_state(self, fq: FeatureQuality) -> VioState:
+        if self._reported_state == VioState.INIT and (
+            self._frames_since_warmup + 1 < self._warm_start_max_frames
+        ):
+            return VioState.INIT
+        if fq.tracked < self._okvis2_cfg.degraded_feature_threshold:
+            return VioState.DEGRADED
+        return VioState.TRACKING
+
+    def _tick_lost(self, frame_id: str) -> None:
+        self._consecutive_lost += 1
+        if self._consecutive_lost >= self._lost_frame_threshold:
+            self._reported_state = VioState.LOST
+        elif self._reported_state == VioState.TRACKING:
+            self._reported_state = VioState.DEGRADED
+
+    def _emit_transition(self, new_state: VioState, frame_id: str) -> None:
+        if self._last_emitted_state == new_state:
+            return
+        self._last_emitted_state = new_state
+        record = FdrRecord(
+            schema_version=CURRENT_SCHEMA_VERSION,
+            ts=_now_iso(),
+            producer_id=_PRODUCER_ID,
+            kind="vio.health",
+            payload={
+                "state": new_state.value,
+                "consecutive_lost": self._consecutive_lost,
+                "bias_norm": _bias_norm(self._latest_bias),
+                "strategy_label": _STRATEGY_LABEL,
+                "frame_id": frame_id,
+            },
+        )
+        self._fdr.enqueue(record)
+
+
+def _frame_ts_ns(frame: NavCameraFrame) -> int:
+    """Convert ``NavCameraFrame.timestamp`` to monotonic-ns.
+
+    Uses the datetime's UTC epoch nanoseconds so the value is
+    monotonically increasing across frames (frame source guarantees
+    strictly increasing timestamps per the FrameSource contract).
+    """
+    return int(frame.timestamp.timestamp() * 1e9)
+
+
+def _frame_image(frame: NavCameraFrame) -> np.ndarray:
+    """Coerce the frame's image into a contiguous uint8 ndarray."""
+    arr = np.ascontiguousarray(frame.image, dtype=np.uint8)
+    if arr.ndim < 2 or arr.ndim > 3:
+        raise VioFatalError(
+            f"Okvis2Strategy: NavCameraFrame.image must be 2-D or 3-D; got {arr.ndim}-D"
+        )
+    return arr

src/gps_denied_onboard/fdr_client/records.py

@@ -40,6 +40,13 @@ KNOWN_PAYLOAD_KEYS: Final[dict[str, frozenset[str]]] = {
     "vio.tick": frozenset(
         {"frame_id", "R", "t", "P", "last_anchor_age_ms", "mre_px", "imu_bias_norm"}
     ),
+    # AZ-332 / E-C1: emitted on every VioStrategy state transition
+    # (INIT->TRACKING->DEGRADED->LOST etc.). One record per transition;
+    # steady-state frames emit nothing on this kind. `frame_id` is the
+    # frame the transition was decided on (may be empty for INIT->...).
+    "vio.health": frozenset(
+        {"state", "consecutive_lost", "bias_norm", "strategy_label", "frame_id"}
+    ),
     "state.tick": frozenset({"frame_id", "fused_pose", "covariance_2x2", "estimator_label"}),
     "tile_match": frozenset({"frame_id", "tile_id", "score", "match_count", "ransac_inliers"}),
     "overrun": frozenset({"producer_id", "dropped_count"}),