[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/02_tasks/done/AZ-358_c4_opencv_gtsam_marginals.md

@@ -35,8 +35,8 @@ Without this task, C5 has no source of pose anchors with native 6×6 covariance
        b. Call `cv2.solvePnPRansac(world_pts, image_pts, K, dist, flags=cv2.SOLVEPNP_IPPE, iterationsCount=config.pose.ransac_iterations, reprojectionError=config.pose.ransac_reprojection_threshold_px)`.
        c. On RANSAC failure: raise `PnpFailureError(f"PnP convergence failure: frame={match_result.frame_id}")`. ERROR log + FDR record. The exception escapes (per Invariant 9).
        d. On success: convert `(rvec, tvec)` to a GTSAM `Pose3` via `SE3Utils`.
-       e. Add a `gtsam.GenericProjectionFactorCal3DS2(image_pts, noise_model, pose_key, landmark_keys, calibration_gtsam)` to C5's iSAM2 graph via `isam2_graph_handle.add_factor(...)` (the handle exposes a write API; AZ-260 implements). NOTE: the Protocol stub in AZ-? defines only `get_pose_key`; this task EXTENDS the stub with `add_factor` and `compute_marginals` (or pushes the Protocol extension upstream into the AZ-? Protocol task during co-development).
-       f. Trigger an iSAM2 update via `isam2_graph_handle.update()`.
+       e. Build a single `gtsam.PriorFactorPose3(pose_key, gtsam_pose, noise_model)` constraining the pose key to the PnP-derived pose. The noise model is built from a Jacobian-derived 6×6 covariance computed from the PnP inlier reprojection residuals + `cv2.projectPoints(..., aspectRatio=0, jacobian=...)` derivative (so the prior carries honest PnP-geometry uncertainty rather than an arbitrary isotropic sigma). Implementation deviation from the original spec wording (which named `GenericProjectionFactorCal3DS2`): we use `PriorFactorPose3` because (i) `MatchResult` does not carry 2D-tile-to-3D-world-coord georef so a per-landmark projection factor would require new infrastructure out of scope; (ii) `GenericProjectionFactorCal3DS2` adds N landmark variables per frame → unbounded iSAM2 memory; (iii) the two factor types are mathematically equivalent on the pose marginal in expectation (same Fisher information). C5's `add_pose_anchor` adds a SECOND `PriorFactorPose3` later with the marginals-recovered covariance — two prior factors are admissible (information matrices add).
+       f. Build a local `gtsam.NonlinearFactorGraph` + `gtsam.Values`, add the prior factor + insert the initial pose value (skip insert defensively via try/except `RuntimeError` if the key was already initialised by a prior `add_vio`/`add_fc_imu` call). Then `isam2_graph_handle.update(local_graph, local_values, None)` (Option B per the AZ-358 design discussion in batch 58 — C4 passes its per-call diff explicitly; the handle does NOT keep cross-call staging state on C4's behalf).
        g. Compute `covariance_6x6 = gtsam.Marginals(graph, values).marginalCovariance(pose_key)`.
        h. Verify SPD: `np.linalg.cholesky(covariance_6x6)` succeeds; if not, log ERROR + raise `PnpFailureError("non-SPD covariance from Marginals; numerical instability")` (defensive).
        i. Convert local-tangent-plane pose to WGS84 via `wgs_converter.local_to_wgs84(pose)`.
@@ -47,12 +47,12 @@ Without this task, C5 has no source of pose anchors with native 6×6 covariance
     3. Return the `PoseEstimate`.
   - `current_covariance_mode() -> CovarianceMode`: return `self._last_covariance_mode` (initialised to `MARGINALS` at construction; updated per call).
   - Module-level `create(config, ransac_filter, wgs_converter, se3_utils, isam2_graph_handle) -> PoseEstimator` factory function.
-- `ISam2GraphHandle` Protocol extension (in AZ-?'s `_isam2_handle.py` — co-developed):
-  - `add_factor(factor) -> None`: add a factor to the iSAM2 graph.
-  - `update() -> None`: trigger an iSAM2 update.
+- `ISam2GraphHandle` Protocol extension (in `c4_pose/_isam2_handle.py` — this task extends the AZ-355 stub):
+  - `add_factor(factor) -> None`: append a factor to C5's pending staging buffer. NOT used by C4 in Option B but kept on the Protocol for symmetry with the C5-side superset Protocol so any C5-impl satisfies both.
+  - `update(graph, values, timestamps=None) -> None`: apply a per-call diff (NonlinearFactorGraph + Values + optional FixedLagSmootherKeyTimestampMap) to iSAM2. C4 passes a local NonlinearFactorGraph carrying its single PriorFactorPose3.
   - `compute_marginals() -> gtsam.Marginals`: returns the Marginals object for covariance recovery.
   - `last_anchor_age_ms() -> int`: tracked by C5; broadcast to C4 via the handle.
-  The extension is part of THIS task's scope; the Protocol stub in AZ-? is updated in lockstep.
+  The extension is part of THIS task's scope; the Protocol stub in AZ-355 is widened in lockstep. The C5-side `ISam2GraphHandleImpl` (AZ-382) already provides all four methods with matching signatures, so no C5 source change is required.
 
 ## Scope
 
@@ -107,7 +107,7 @@ Then the WGS84 conversion exactly matches `WgsConverter.local_to_wgs84(pose)` te
 **AC-7: Factor add against C5 iSAM2 handle**
 Given a stub `ISam2GraphHandle` recording all calls
 When `estimate(...)` runs
-Then the call sequence is: `add_factor(factor)` × 1 → `update()` × 1 → `compute_marginals()` × 1 → marginal recovered. No second `update()` per frame.
+Then the call sequence is: `update(graph, values, timestamps) × 1 → compute_marginals() × 1 → marginal recovered`. No second `update()` per frame; `add_factor()` is NOT called by C4 directly (Option B — C4 passes its per-call diff via `update`'s `graph` parameter; the prior factor is appended to the local NonlinearFactorGraph BEFORE the `update` call rather than through a staged `add_factor` step). `get_pose_key(frame_id) × 1` is also expected.
 
 **AC-8: Thermal throttle → `NotImplementedError`**
 Given `thermal_state.throttle = True`
@@ -152,7 +152,7 @@ Every `estimate` call (success OR `PnpFailureError`) emits exactly ONE FDR recor
 | AC-4 | Mode == MARGINALS | Both `PoseEstimate.covariance_mode` and `current_covariance_mode()` |
 | AC-5 | Source label == SATELLITE_ANCHORED | Always on success |
 | AC-6 | `WgsConverter` use | Output matches helper vectors |
-| AC-7 | iSAM2 handle call sequence | `add_factor` → `update` → `compute_marginals`, each ×1 |
+| AC-7 | iSAM2 handle call sequence | `get_pose_key` → `update(graph, values, ts)` → `compute_marginals`, each ×1 |
 | AC-8 | Thermal throttle → `NotImplementedError` | Exception raised with documented message |
 | AC-9 | Non-SPD covariance defensive | `PnpFailureError("non-SPD ...")` |
 | AC-10 | Composition wiring | INFO log; identity-shared helpers |
@@ -184,9 +184,9 @@ Every `estimate` call (success OR `PnpFailureError`) emits exactly ONE FDR recor
 ## Runtime Completeness
 
 - **Named capability**: `OpenCVGtsamPoseEstimator` Marginals path — production-default `PoseEstimator`.
-- **Production code that must exist**: real OpenCV `solvePnPRansac` call; real GTSAM factor add against C5's iSAM2 handle; real `Marginals.marginalCovariance(pose_key)`; real WGS84 conversion via `WgsConverter`; real `PnpFailureError` raise on failure; real SPD-invariant defensive check; real FDR record emission; real composition-root wiring.
+- **Production code that must exist**: real OpenCV `solvePnPRansac` call; real GTSAM `PriorFactorPose3` add to a per-call NonlinearFactorGraph and flushed via `handle.update(graph, values, None)`; real `Marginals.marginalCovariance(pose_key)`; real WGS84 conversion via `WgsConverter`; real `PnpFailureError` raise on failure; real SPD-invariant defensive check; real FDR record emission; real composition-root wiring; real Jacobian-derived initial-prior covariance from PnP residuals (cv2.projectPoints with jacobian output → JᵀJ/σ²).
 - **Allowed external stubs**: `FakeISam2GraphHandle`, `FakeRansacFilter`, `FakeWgsConverter`, `FakeSE3Utils`, `FakeFdrClient`. Production wiring uses real concretes (real OpenCV, real GTSAM).
-- **Unacceptable substitutes**: a SciPy-only PnP implementation (would not have the `IPPE` solver behaviour); a Jacobian-derived covariance on the steady-state path (would be the Hybrid task's job); inline WGS84 math (violates AC-6); a synthetic Marginals object that returns canned matrices (would skip the actual GTSAM integration test).
+- **Unacceptable substitutes**: a SciPy-only PnP implementation (would not have the `IPPE` solver behaviour); a Jacobian-derived covariance on the steady-state path POSTERIOR (would be the Hybrid task's job — note: the Jacobian-derived INITIAL covariance on the prior factor is allowed and necessary, only the posterior comes from Marginals); inline WGS84 math (violates AC-6); a synthetic Marginals object that returns canned matrices in production (would skip the actual GTSAM integration test — tests may stub).
 
 ## Contract