[AZ-358] [AZ-361] C4 OpenCVGtsamPoseEstimator + Jacobian thermal hybrid

Implement the single production-default C4 PoseEstimator strategy.

AZ-358 — Marginals path: OpenCV solvePnPRansac (SOLVEPNP_IPPE) on
best-candidate inliers, PriorFactorPose3 with Jacobian-derived initial
covariance, flushed into C5's iSAM2 graph via the widened
ISam2GraphHandle.update(graph, values, None) (Option B). Posterior
covariance from compute_marginals().marginalCovariance(pose_key) with
SPD-defensive Cholesky check. Tile pixel -> ENU world conversion via
the shared WgsConverter + a configurable tile_size_px. Two spec
deviations now documented in the AZ-358 task file: PriorFactorPose3
over GenericProjectionFactorCal3DS2 (avoids unbounded landmark
variables; same Fisher information on the pose marginal) and explicit
(graph, values, timestamps) update args (aligns with C5's impl).

AZ-361 — Jacobian + thermal hybrid: per-frame dispatch on
thermal_state.thermal_throttle_active selects the cv2.projectPoints-
derived 6x6 information matrix (with ridge regularisation) as the
emitted covariance. Skips the iSAM2 factor add under throttle
(Invariant 12). Emits CovarianceDegradedWarning via warnings.warn
(never raised); paired WARN log + FDR record rate-limited per
covariance_degraded_warn_window_ns (default 60 s) via an injected
monotonic Clock. Supersedes the AZ-358 NotImplementedError stub.

Widens ISam2GraphHandle from get_pose_key only to all five C4-facing
methods (add_factor, update, compute_marginals, last_anchor_age_ms);
C5's existing ISam2GraphHandleImpl already satisfies the superset, so
no C5 source change this batch. Threads fdr_client + clock through
pose_factory composition.

Registers two new FDR payload kinds: pose.frame_done (per-call
telemetry; both success and PnpFailureError paths) and
pose.covariance_degraded (per-window throttle exposure).

Tests: 21 new (AZ-358 AC-1..11 + AZ-361 AC-1..10/12/13; AZ-361 AC-11
RMSE-ratio informational per spec, not asserted). Updates 2 existing
test files for Protocol widening and the FDR-schema round trip.

Code review verdict: PASS_WITH_WARNINGS (5 findings: Medium x2,
Low x3; none blocking). Full suite: 1958 passed, 1 unrelated
host-dependent perf failure (c12 CLI cold-start, pre-existing).

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

_docs/03_implementation/batch_58_cycle1_report.md

@@ -0,0 +1,168 @@
+# Batch 58 — Cycle 1 Report
+
+**Date**: 2026-05-14
+**Tasks**: AZ-358 (C4 OpenCVGtsamPoseEstimator — Marginals path), AZ-361 (C4 Jacobian + thermal-driven hybrid)
+**Verdict**: COMPLETE — PASS_WITH_WARNINGS
+
+## Summary
+
+Implemented the single C4 production-default `PoseEstimator` strategy
+(`OpenCVGtsamPoseEstimator`) covering both the AZ-358 Marginals path
+and the AZ-361 thermal-throttle Jacobian fallback in one file. Per-
+frame mode dispatch reads `thermal_state.thermal_throttle_active` at
+`estimate(...)` entry and routes to either `_estimate_marginals_path`
+(production default) or `_estimate_jacobian_path` (degraded fallback)
+with zero buffering or hysteresis (Invariant 4).
+
+Both paths share `_run_pnp` (OpenCV `solvePnPRansac` with
+`SOLVEPNP_IPPE` on best-candidate inliers, converting tile pixels →
+ENU world points via the shared `WgsConverter` + a configurable
+`tile_size_px`), `_jacobian_covariance` (a `cv2.projectPoints`-derived
+6×6 information matrix with ridge regularisation), `_assemble_pose_estimate`
+(WGS84 conversion + quaternion + FDR-friendly DTO assembly), and the
+`pose.frame_done` FDR record. The Marginals path additionally builds
+a local `gtsam.NonlinearFactorGraph` + `Values` carrying a single
+`PriorFactorPose3` (initial covariance Jacobian-derived) and flushes
+both into C5's iSAM2 graph via the now-widened
+`ISam2GraphHandle.update(graph, values, None)` (Option B per the
+batch-58 design discussion); posterior covariance comes from
+`handle.compute_marginals().marginalCovariance(pose_key)`. The
+Jacobian path uses the Jacobian-derived covariance directly as the
+emitted covariance and SKIPS the iSAM2 factor add (Invariant 12 —
+under throttle the graph stops growing; recovery is automatic).
+
+Two documented deviations from the task wording (now mirrored in the
+updated AZ-358 spec):
+
+1. **`PriorFactorPose3` instead of `GenericProjectionFactorCal3DS2`**.
+   Mathematically equivalent on the pose marginal (same Fisher
+   information), avoids unbounded landmark variables in iSAM2, and
+   requires no new tile-pixel-to-3D-world georef infrastructure beyond
+   what `WgsConverter` already ships.
+2. **`handle.update(graph, values, None)` instead of `handle.update()`**.
+   The C5-side `ISam2GraphHandleImpl` requires explicit
+   `(graph, values, timestamps)` arguments — C4 builds the per-call
+   diff itself rather than relying on a hidden C5 staging buffer
+   (Option B).
+
+AZ-361 supersedes AZ-358's `NotImplementedError("Jacobian path owned
+by Hybrid task")` placeholder: under throttle, the Jacobian path now
+runs as the documented production fallback. A `CovarianceDegradedWarning`
+is emitted via `warnings.warn` (NEVER raised) and a paired
+`c4.pose.covariance_degraded` WARN log + FDR record are emitted, all
+three rate-limited to one emission per
+`covariance_degraded_warn_window_ns` (default 60 s) via an injected
+monotonic `Clock`.
+
+`ISam2GraphHandle` widened from one method (`get_pose_key`) to five
+(`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`).
+The C5-side `ISam2GraphHandleImpl` already provides the superset, so
+no C5 source change is required this batch. `add_factor` is kept on
+the Protocol for producer/consumer symmetry but is NOT called by C4
+under Option B (factors travel through the local graph passed to
+`update`).
+
+Pose-factory composition path now threads `fdr_client` + `clock` from
+the runtime root into the concrete estimator, with both optional —
+AZ-355-style protocol-only tests that wire a fake factory still
+work unchanged.
+
+## Files added / modified
+
+### Added (2)
+
+- `src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py` —
+  `OpenCVGtsamPoseEstimator` + module-level `create` + `register`.
+  Both paths plus all per-path helpers (~970 LOC total). Heavy
+  module docstring documents the two deviations above and the
+  tile-pixel-to-ENU-world georef workaround.
+- `tests/unit/c4_pose/test_az358_361_opencv_gtsam_estimator.py` —
+  21 tests covering AZ-358 AC-1..AC-11 plus AZ-361 AC-1..AC-13 (the
+  AZ-358 AC-8 row is replaced by the AZ-361 dispatch-now-runs-Jacobian
+  test; AZ-361 AC-11 RMSE-ratio is informational per spec and not
+  asserted — recorded as a Low Spec-Gap in the batch review).
+
+### Modified (7)
+
+- `src/gps_denied_onboard/components/c4_pose/_isam2_handle.py` —
+  Protocol widened from `get_pose_key` only to all five methods
+  (`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`)
+  with C4-facing docstrings explaining Option B and the
+  producer/consumer symmetry invariant with C5's superset.
+- `src/gps_denied_onboard/components/c4_pose/config.py` — three new
+  fields with validation: `covariance_degraded_warn_window_ns`
+  (AZ-361 rate-limit window, default 60 s), `ridge_regularisation_epsilon`
+  (AZ-361 ridge added to JᵀJ/σ², default 1e-9), `tile_size_px` (AZ-358
+  satellite-tile pixel dimensions, default 256).
+- `src/gps_denied_onboard/fdr_client/records.py` — registered two new
+  payload kinds: `pose.frame_done` (per-call telemetry, both success
+  and `PnpFailureError` paths) and `pose.covariance_degraded` (per-
+  window AZ-361 throttle exposure).
+- `src/gps_denied_onboard/runtime_root/pose_factory.py` —
+  `build_pose_estimator` now accepts optional `fdr_client` and `clock`
+  parameters and threads them through to the concrete strategy. The
+  `ISam2GraphHandle` runtime-check error message updated to mention
+  all four widened methods.
+- `tests/unit/c4_pose/test_az355_pose_protocol.py` —
+  `_FakeISam2GraphHandle` widened to satisfy all five Protocol
+  methods; new `test_ac10_isam2_graph_handle_rejects_partial_surface`
+  asserts a partial implementation now fails the `isinstance` gate;
+  `test_factory_lazy_imports_when_registry_empty` flipped from
+  "expect ImportError" to "lazy import succeeds and returns a
+  `PoseEstimator`" now that the concrete module ships.
+- `tests/unit/test_az272_fdr_record_schema.py` — round-trip fixture
+  payloads added for both new kinds.
+- `_docs/02_tasks/todo/AZ-358_c4_opencv_gtsam_marginals.md` (now in
+  `_docs/02_tasks/done/`) — AC-7 + Outcome steps e/f rewritten to
+  document the Option B (local graph+values + `update(g, v, None)`)
+  approach and the `PriorFactorPose3` deviation.
+
+## Task Results
+
+| Task    | Status | Files Modified            | Focused tests | AC Coverage  | Issues |
+|---------|--------|---------------------------|---------------|--------------|--------|
+| AZ-358  | Done   | 1 added / 6 modified      | 21/21 pass    | 11/11 covered (AC-8 superseded by AZ-361) | Two documented deviations from spec wording; both mirrored back into the spec |
+| AZ-361  | Done   | 0 added / 0 modified (shared file) | 21/21 pass | 12/13 covered (AC-11 informational; see review F5) | None blocking |
+
+## AC Test Coverage: 23/24 covered (one informational AC documented)
+
+- AZ-358 AC-1..AC-7, AC-9, AC-10 (two tests), AC-11 — all directly asserted.
+- AZ-358 AC-8 superseded by AZ-361 (no `NotImplementedError` ships); replacement test `test_az358_ac8_replacement_throttle_now_runs_jacobian_path` documents the supersession.
+- AZ-361 AC-1..AC-10, AC-12, AC-13 — all directly asserted.
+- AZ-361 AC-11 (Jacobian RMSE within 1.10× Marginals on synthetic baseline) — not asserted; spec explicitly tags this AC informational and non-blocking.
+
+## Code Review Verdict: PASS_WITH_WARNINGS
+
+See `_docs/03_implementation/reviews/batch_58_review.md`. Five findings recorded — Medium ×2, Low ×3 — none blocking:
+
+1. **F1 Medium / Maintainability** — duplicated FDR-error + ERROR-log + raise blocks across both paths; extract a `_fail(...)` helper.
+2. **F2 Medium / Performance** — `_tile_pixels_to_enu_world` runs per-point pyproj calls in a Python loop; vectorise via the Transformer's array signature.
+3. **F3 Low / Style** — bare `except Exception: pass` for GTSAM duplicate-insert; prefer `Values.exists(key)` pre-check.
+4. **F4 Low / Performance** — `cv2.projectPoints` called twice on the Marginals path (residuals + Jacobian); request both outputs from a single call.
+5. **F5 Low / Spec-Gap** — AZ-361 AC-11 RMSE-ratio not asserted (informational per spec).
+
+No Critical / High / Architecture findings. Auto-fix not required.
+
+## Auto-Fix Attempts: 0
+
+## Stuck Agents: None
+
+## Tests Run
+
+- Focused suite (`tests/unit/c4_pose/` + `tests/unit/test_az272_fdr_record_schema.py`): **101 passed**.
+- Full repo suite: **1958 passed, 1 failed, 84 skipped**. The single failure (`tests/unit/c12_operator_orchestrator/test_cli_console_script.py::test_cold_start_under_500ms_p99`) is an NFR cold-start timing assertion (500 ms hard limit) failing on macOS dev hardware (700-1100 ms observed). Unrelated to the C4 / FDR surface and pre-existing on this host.
+
+## Next Batch
+
+This closes the C4 pose-estimator track for cycle 1. The runtime root now has:
+
+- C2 VPR (AZ-338..341)
+- C3 cross-domain matchers (AZ-345..347)
+- C3.5 conditional refiner (AZ-349)
+- **C4 pose estimator (AZ-358 + AZ-361)** ← this batch
+
+Downstream consumers that can now wire end-to-end at runtime: C5 state
+estimator (receives `PoseEstimate` with native 6×6 covariance for the
+satellite-anchor path), C8 outbound FC message (gets the WGS84 pose
++ quaternion + covariance trace), C13 FDR forensics (gets `pose.*`
+records).