# 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).
