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