[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/todo/AZ-358_c4_opencv_gtsam_marginals.md

@@ -1,194 +0,0 @@
-# C4 OpenCVGtsamPoseEstimator — Marginals (steady-state) path
-
-**Task**: AZ-358_c4_opencv_gtsam_marginals
-**Name**: C4 `OpenCVGtsamPoseEstimator` — `solvePnPRansac` + GTSAM `Marginals` factor add (steady-state path)
-**Description**: Implement `OpenCVGtsamPoseEstimator`, the production-default `PoseEstimator`. STEADY-STATE PATH ONLY (`thermal_state.throttle == False`): runs OpenCV `solvePnPRansac` with `SOLVEPNP_IPPE` on the inlier correspondences from `MatchResult.per_candidate[best_candidate_idx].inlier_correspondences`; on success, adds a `GenericProjectionFactorCal3DS2` to C5's shared iSAM2 graph (via the `ISam2GraphHandle.get_pose_key(frame_id)` API); recovers the posterior 6×6 covariance via `gtsam.Marginals(graph, values).marginalCovariance(pose_key)`; converts the local-tangent-plane pose to WGS84 via the shared `WgsConverter`; assembles a `PoseEstimate` with `covariance_mode = MARGINALS`, `source_label = SATELLITE_ANCHORED`. RANSAC convergence failure or degenerate geometry raises `PnpFailureError` (per Invariant 9). When `thermal_state.throttle == True`, raises `NotImplementedError("Jacobian path owned by Hybrid task")` — replaced by AZ-? (Hybrid task) when that lands.
-**Complexity**: 5 points
-**Dependencies**: AZ-355 (Protocol + DTOs + factory + `ISam2GraphHandle` Protocol stub), AZ-381 (E-C5 — supplies the concrete `ISam2GraphHandle` impl + iSAM2 graph; co-developed per ADR-003), AZ-282 (RANSAC helper, used internally for residual recomputation if needed), AZ-279 (`WgsConverter`), AZ-277 (`SE3Utils`), AZ-269 (config), AZ-266 (logging), AZ-272 (FDR record schema), AZ-263
-**Component**: c4_pose (epic AZ-259 / E-C4)
-**Tracker**: AZ-358
-**Epic**: AZ-259 (E-C4)
-
-### Document Dependencies
-
-- `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md` — producer/consumer split; Marginals task scope.
-- `_docs/02_document/components/06_c4_pose/description.md` — § 1 (D-C4-1=(b) IPPE; D-C4-2=(b) Marginals); § 5 error handling; § 7 `Marginals.marginalCovariance(pose_key)` is the dominant cost (~30–90 ms).
-- `_docs/02_document/components/06_c4_pose/tests.md` — C4-IT-01 (WGS84 accuracy), C4-IT-02 (SPD covariance), C4-IT-04 (shared-graph integration).
-- `_docs/02_document/architecture.md` — ADR-003 (shared GTSAM substrate), ADR-009 (interface-first DI).
-- `_docs/02_document/contracts/shared_helpers/wgs_converter.md`.
-- `_docs/02_document/contracts/shared_helpers/ransac_filter.md`.
-
-## Problem
-
-Without this task, C5 has no source of pose anchors with native 6×6 covariance — meaning every frame would be `visual_propagated` and AC-1.4 (95% covariance + source label) cannot be satisfied. The Marginals path is also the production-default per D-C4-2 = (b); the Jacobian fallback is degraded-mode only. Defining the steady-state path FIRST (before the Hybrid task) is also necessary so the Hybrid task has a working baseline against which to measure the Jacobian-degraded accuracy delta (~5–10% per ADR-006).
-
-## Outcome
-
-- `src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py` defining:
-  - `OpenCVGtsamPoseEstimator` class implementing the `PoseEstimator` Protocol.
-  - Constructor: `__init__(self, config, ransac_filter, wgs_converter, se3_utils, isam2_graph_handle, fdr_client)`.
-  - `_last_covariance_mode: CovarianceMode` (private; reset every `estimate` call).
-  - `estimate(match_result, calibration, thermal_state)`:
-    1. **If `thermal_state.throttle == True`**: raise `NotImplementedError("Jacobian path owned by Hybrid task; install AZ-? to enable.")`. Code review hook for the Hybrid task to delete this branch and replace with the actual implementation.
-    2. **Steady-state path** (`thermal_state.throttle == False`):
-       a. Extract inlier 2D points + 3D world points from `match_result.per_candidate[match_result.best_candidate_idx].inlier_correspondences` + the candidate's `tile_id` → look up tile world coordinates via the calibration's georeferencing.
-       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()`.
-       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)`.
-       j. Assemble `PoseEstimate(frame_id, position_wgs84, orientation, covariance_6x6, MARGINALS, SATELLITE_ANCHORED, last_anchor_age_ms_from_isam2_handle, monotonic_ns())`.
-       k. `self._last_covariance_mode = MARGINALS`.
-       l. INFO log on first-frame ready; DEBUG log per frame `kind="c4.pose.frame_done"` with `{frame_id, inliers, residual, mode}`.
-       m. FDR `pose.frame_done` record.
-    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.
-  - `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.
-
-## Scope
-
-### Included
-- `OpenCVGtsamPoseEstimator` Marginals path (steady-state).
-- `solvePnPRansac` with `SOLVEPNP_IPPE`.
-- GTSAM factor add to C5's iSAM2 graph via `ISam2GraphHandle`.
-- `Marginals.marginalCovariance(pose_key)` for native 6×6 covariance recovery.
-- WGS84 conversion via shared `WgsConverter`.
-- SPD-invariant defensive check.
-- `PnpFailureError` raise on convergence failure or degenerate geometry.
-- `NotImplementedError` placeholder for the Jacobian path (replaced by Hybrid task).
-- `ISam2GraphHandle` Protocol extension (`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`).
-- Composition-root wiring path.
-- Unit tests covering: PnP success path on synthetic correspondences; PnP failure (degenerate geometry) → `PnpFailureError`; SPD covariance assertion; WGS84 conversion correctness against `WgsConverter` test vectors; thermal-throttle → `NotImplementedError`.
-
-### Excluded
-- The Jacobian fallback path — owned by the Hybrid task.
-- The C5 iSAM2 graph implementation — owned by AZ-260; this task consumes via Protocol.
-- C4-IT-01..04 + C4-PT-01 — deferred to E-BBT (AZ-262).
-- The `ThermalState` source — owned by AZ-302.
-- The camera calibration loader (intrinsics + distortion + extrinsics) — owned by C5 / shared.
-
-## Acceptance Criteria
-
-**AC-1: PnP success on synthetic correspondences**
-Given a 50-point inlier set with known ground-truth pose
-When `estimate(...)` runs
-Then the returned `position_wgs84` is within 1 m of ground truth (synthetic; real-world tolerances per C4-IT-01).
-
-**AC-2: PnP RANSAC failure → `PnpFailureError`**
-Given an inlier set with all-collinear points (degenerate geometry)
-When `estimate(...)` is called
-Then `PnpFailureError` is raised; ONE ERROR log; ONE FDR `pose.frame_done` record with `error: true`.
-
-**AC-3: SPD covariance invariant**
-Given a successful PnP + Marginals run
-When the resulting `covariance_6x6` is checked
-Then `np.linalg.cholesky(covariance_6x6)` succeeds (matrix is positive-definite); the matrix is symmetric to 1e-10 tolerance.
-
-**AC-4: `covariance_mode == MARGINALS` on success**
-Every successful `estimate` returns `PoseEstimate.covariance_mode == CovarianceMode.MARGINALS` AND `current_covariance_mode() == MARGINALS`.
-
-**AC-5: `source_label == SATELLITE_ANCHORED` on success (Invariant 7)**
-Every successful `estimate` returns `PoseEstimate.source_label == PoseSourceLabel.SATELLITE_ANCHORED`. C4 NEVER emits `VISUAL_PROPAGATED`.
-
-**AC-6: WGS84 conversion uses shared `WgsConverter`**
-Given a known local-tangent-plane pose AND a known origin
-When `estimate` runs
-Then the WGS84 conversion exactly matches `WgsConverter.local_to_wgs84(pose)` test vectors. (This is implementation hygiene — verifies no inline math.)
-
-**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.
-
-**AC-8: Thermal throttle → `NotImplementedError`**
-Given `thermal_state.throttle = True`
-When `estimate(...)` is called
-Then `NotImplementedError("Jacobian path owned by Hybrid task; install AZ-? to enable.")` is raised.
-
-**AC-9: Non-SPD covariance defensive raise**
-Given a stubbed `ISam2GraphHandle.compute_marginals()` returning a non-SPD matrix (synthetically corrupted)
-When `estimate(...)` runs
-Then `PnpFailureError("non-SPD covariance from Marginals; numerical instability")` is raised; ERROR log emitted.
-
-**AC-10: Composition-root wiring**
-Given `config.pose.strategy = "opencv_gtsam"` AND a valid `ISam2GraphHandle`
-When `compose_root(config)` runs
-Then an `OpenCVGtsamPoseEstimator` is wired; ONE INFO log `kind="c4.pose.ready"` with `{strategy: "opencv_gtsam", default_covariance: "MARGINALS"}` is emitted; the strategy holds the SAME `RansacFilter`, `WgsConverter`, `SE3Utils` instances as C3 / C5 (identity-shared).
-
-**AC-11: FDR `pose.frame_done` record shape**
-Every `estimate` call (success OR `PnpFailureError`) emits exactly ONE FDR record with documented fields: `{frame_id, inliers, residual, mode, covariance_norm, position_wgs84, error}`.
-
-## Non-Functional Requirements
-
-**Performance**
-- `estimate` p95 (MARGINALS, K=15) ≤ 90 ms target / 130 ms hard limit (per C4-PT-01 / AC-4.1).
-- `solvePnPRansac` portion p95 ≤ 30 ms.
-- `Marginals.marginalCovariance(pose_key)` portion p95 ≤ 60 ms (the dominant cost per description.md § 7).
-
-**Compatibility**
-- OpenCV ≥ 4.12.0 (CVE-2025-53644 mitigation per description.md).
-- GTSAM Python bindings — version per Plan-phase pin.
-
-**Reliability**
-- `PnpFailureError` is the ONLY exception escaping (Invariant 9). All other failure modes (degenerate geometry, non-SPD covariance, etc.) MAP to `PnpFailureError`.
-- SPD invariant defensive check covers GTSAM numerical instability; if it triggers, an upstream issue exists in C5's graph health.
-
-## Unit Tests
-
-| AC Ref | What to Test | Required Outcome |
-|--------|--------------|------------------|
-| AC-1 | Synthetic PnP success | Position within 1 m of GT |
-| AC-2 | Degenerate geometry → `PnpFailureError` | Exception raised; ERROR log; FDR error record |
-| AC-3 | SPD covariance | Cholesky succeeds; symmetric to 1e-10 |
-| 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-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 |
-| AC-11 | FDR record shape | Exactly one per call; documented fields |
-
-## Constraints
-
-- **Single-threaded** by contract (Invariant 1); same thread as C5.
-- **Steady-state path ONLY** — `thermal_state.throttle == True` raises `NotImplementedError`. The Hybrid task replaces this branch.
-- **`solvePnPRansac` flag MUST be `SOLVEPNP_IPPE`** per D-C4-1 = (b).
-- **`Marginals` is the covariance-recovery primitive** — not Jacobian-based math (the Hybrid task's job).
-- **No inline math for WGS84 conversion** — must use `WgsConverter` (AC-6).
-- **`ISam2GraphHandle` extension MUST be applied to AZ-?'s Protocol stub** in lockstep — both tasks update the same `_isam2_handle.py` file. This is documented as a co-developed scope per ADR-003.
-
-## Risks & Mitigation
-
-**Risk 1: GTSAM `Marginals` is non-thread-safe + the iSAM2 graph is shared with C5**
-- *Mitigation*: single-thread invariant (Invariant 1); composition root binds C4 + C5 to the same thread. The handle's `update()` + `compute_marginals()` are called sequentially within `estimate`; C5's update path runs in a different code section but on the same thread.
-
-**Risk 2: `solvePnPRansac` in OpenCV 4.12 has a known subtle behaviour change for `SOLVEPNP_IPPE`** that could affect convergence on the Derkachi fixture
-- *Mitigation*: pin OpenCV version per Plan-phase pin. C4-IT-01 verifies WGS84 accuracy on Derkachi; if the verdict regresses, escalate.
-
-**Risk 3: The `ISam2GraphHandle` extension creates a chicken-and-egg with AZ-260 (C5)**
-- *Mitigation*: ADR-003 acknowledges this. The Protocol extension is owned by THIS task; the concrete impl is owned by AZ-260; both are co-developed. The Protocol task (AZ-?) ships the minimal `get_pose_key` surface; this task extends to `add_factor`/`update`/`compute_marginals`/`last_anchor_age_ms`. AZ-260 implements all four.
-
-**Risk 4: `Marginals.marginalCovariance(pose_key)` cost (~30–90 ms) blows the AC-4.1 budget under load**
-- *Mitigation*: the Hybrid task addresses this via the Jacobian fallback under thermal throttle. This task's NFR target is 90 ms p95 — within budget; the ~5–10% accuracy loss of the Jacobian path is the trade documented in ADR-006.
-
-## 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.
-- **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).
-
-## Contract
-
-This task implements the steady-state portion of `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md`.
-The Hybrid task (TBD) replaces the `NotImplementedError` branch with the actual Jacobian implementation.

_docs/02_tasks/todo/AZ-361_c4_jacobian_thermal_hybrid.md

@@ -1,187 +0,0 @@
-# C4 D-CROSS-LATENCY-1 Hybrid — Jacobian fallback + thermal-state-driven mode switch
-
-**Task**: AZ-361_c4_jacobian_thermal_hybrid
-**Name**: C4 D-CROSS-LATENCY-1 hybrid — Jacobian-degraded covariance + per-frame thermal-state-driven mode switch
-**Description**: Extend `OpenCVGtsamPoseEstimator` (TBD AZ-? Marginals task) with the D-CROSS-LATENCY-1 hybrid: when `thermal_state.throttle == True`, REPLACE the `Marginals.marginalCovariance(pose_key)` path with a Jacobian-derived 6×6 covariance computed directly from the OpenCV `solvePnPRansac` outputs (`rvec`, `tvec`, inlier residuals) using the standard PnP Jacobian + reprojection-residual variance. The pose estimate itself is still `solvePnPRansac` output; ONLY the covariance recovery path differs. The mode-switch decision is made PER FRAME at the start of `estimate(...)` based on `thermal_state.throttle`. Switching MARGINALS → JACOBIAN or back happens immediately on the next call (Invariant 4 — mode-switch latency ≤ 1 frame). The Jacobian path also emits `CovarianceDegradedWarning` via `warnings.warn(...)` once per 60 s window (filterwarnings-based rate-limiting). Replaces the AZ-? Marginals task's `NotImplementedError("Jacobian path owned by Hybrid task...")` branch with the actual implementation.
-**Complexity**: 3 points
-**Dependencies**: AZ-358 (Marginals path + class scaffold), AZ-355 (`CovarianceDegradedWarning` + `CovarianceMode` enum), AZ-302_c7_thermal_publisher (`ThermalState` source), AZ-277_se3_utils, AZ-279_wgs_converter, AZ-269_config_loader, AZ-266_log_module, AZ-272_fdr_record_schema, AZ-263
-**Component**: c4_pose (epic AZ-259 / E-C4)
-**Tracker**: AZ-361
-**Epic**: AZ-259 (E-C4)
-
-### Document Dependencies
-
-- `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md` — Hybrid task scope.
-- `_docs/02_document/components/06_c4_pose/description.md` — § 1 D-C4-2 = (a) Jacobian fallback under throttle; § 5 `CovarianceDegradedWarning` semantics; § 7 ~5–15 ms Jacobian cost.
-- `_docs/02_document/components/06_c4_pose/tests.md` — C4-IT-03 (D-CROSS-LATENCY-1 mode switch within 1 frame), C4-PT-01 (JACOBIAN p95 ≤ 15 ms).
-- `_docs/02_document/architecture.md` — ADR-006 (~5–10% accuracy loss accepted under throttle), R10.
-
-## Problem
-
-Without this task, the system has only the Marginals path; under thermal throttle (sustained Jetson high-temperature mode), `Marginals.marginalCovariance(pose_key)` consumes ~60 ms of the AC-4.1 latency budget AND GPU/CPU contention with C7 inference makes the overall pipeline miss the 400 ms p95 budget. The D-CROSS-LATENCY-1 hybrid trades ~5–10% covariance accuracy (per ADR-006) for ~5–15 ms Jacobian-path latency, restoring the latency budget. AC-NEW-5 (operating envelope; thermal-throttle-driven covariance degradation hybrid) requires this path to exist and to switch per-frame within 1 frame of the thermal flag flipping.
-
-## Outcome
-
-- `src/gps_denied_onboard/components/c4_pose/opencv_gtsam_estimator.py` — REMOVE the `NotImplementedError` branch; ADD the Jacobian path.
-- New private method `_estimate_marginals_path(...)` extracted from the existing Marginals body (refactor; no behaviour change).
-- New private method `_estimate_jacobian_path(...)`:
-  1. Run `cv2.solvePnPRansac` (same call as Marginals path) — pose extraction is identical.
-  2. From the inlier 2D points + 3D world points + reprojected coordinates, compute the per-point reprojection residuals.
-  3. Compute the PnP Jacobian `J` (6×N) at the converged pose using OpenCV's `cv2.projectPoints` with derivative output (or via `SE3Utils.numerical_jacobian` if OpenCV does not expose the analytical Jacobian for IPPE).
-  4. Compute residual variance σ² as `mean(residuals²)` (isotropic noise model — documented as the simplification the Jacobian path makes per ADR-006).
-  5. Compute the 6×6 information matrix `Λ = (1/σ²) · Jᵀ J`.
-  6. Compute covariance `Σ = inv(Λ + ε·I)` with ε = 1e-9 (ridge regularisation for numerical stability when `Λ` is ill-conditioned).
-  7. Verify SPD: `np.linalg.cholesky(Σ)` succeeds; on failure raise `PnpFailureError("non-SPD Jacobian covariance; numerical instability")` (defensive).
-  8. Convert pose to WGS84 via `WgsConverter.local_to_wgs84(pose)` — SAME conversion path as Marginals.
-  9. Assemble `PoseEstimate(..., covariance_mode = JACOBIAN, source_label = SATELLITE_ANCHORED)`.
-  10. `self._last_covariance_mode = JACOBIAN`.
-  11. **`warnings.warn(CovarianceDegradedWarning("Jacobian covariance engaged; thermal_throttle=true"), stacklevel=2)`** — first time per 60 s window; rate-limited via `_jacobian_warn_window` (timestamp of last emission). Subsequent invocations within the window use `warnings.simplefilter("once")` semantics — emit only the first one.
-  12. WARN log `kind="c4.pose.covariance_degraded"` with `{frame_id, thermal_state}` — emitted ONCE per 60 s window (rate-limited).
-  13. FDR `pose.frame_done` record with `mode: "jacobian"`.
-- `estimate(...)` becomes a dispatcher: read `thermal_state.throttle` at entry; call `_estimate_marginals_path(...)` or `_estimate_jacobian_path(...)`. NO state buffering — strictly per-call.
-- Note: the Jacobian path does NOT add a factor to C5's iSAM2 graph (per design — under throttle, the system runs lighter; C5 receives a `PoseEstimate` but no graph factor add). DOCUMENT this trade explicitly: under throttle, the iSAM2 graph stops growing; recovery happens automatically when the thermal flag flips back.
-
-> **Cross-task interaction with AZ-260 (C5)**. C5's iSAM2 update path needs to handle the case where C4 emits a `PoseEstimate` without a corresponding factor add (Jacobian path). C5's `add_pose_anchor(pose_estimate)` MUST inspect `pose_estimate.covariance_mode` and skip the graph-resync work for `JACOBIAN`. This requirement is captured in AZ-260's task spec; this task does NOT modify C5 — it only documents the requirement.
-
-## Scope
-
-### Included
-- Refactor of existing `OpenCVGtsamPoseEstimator.estimate(...)` into a dispatcher + two private path methods.
-- New `_estimate_jacobian_path(...)` implementation.
-- Per-frame thermal-state-driven dispatch.
-- `CovarianceDegradedWarning` emission via `warnings.warn` (NOT raise).
-- 60 s rate-limiting on the WARN log AND the warnings.warn emission.
-- SPD-invariant defensive check on Jacobian covariance.
-- Removal of `NotImplementedError` from AZ-? Marginals task's body.
-- Documentation update to the contract / description: explicit note that the Jacobian path does NOT add to C5's iSAM2 graph.
-- Unit tests covering: thermal flag flip → mode switch within 1 frame; Jacobian covariance is SPD; Jacobian covariance produces accuracy WITHIN ~5–10% of Marginals on a synthetic baseline; `CovarianceDegradedWarning` emitted via `warnings.warn` not raise; rate-limiting of the warning + WARN log.
-
-### Excluded
-- The Marginals path — already shipped by AZ-?.
-- C5's iSAM2 update-path adjustment (`add_pose_anchor` mode inspection) — owned by AZ-260; this task only documents the requirement.
-- The `ThermalState` source — owned by AZ-302.
-- C4-IT-03 + C4-PT-01 component-internal acceptance tests — deferred to E-BBT (AZ-262); unit tests in this task cover the per-method invariants.
-
-## Acceptance Criteria
-
-**AC-1: Per-frame mode dispatch**
-Given an alternating `thermal_state.throttle` sequence (False, True, False, True, ...) over 10 frames
-When `estimate(...)` is called for each frame
-Then `current_covariance_mode()` returns the correct mode after EACH call (no buffering, no hysteresis).
-
-**AC-2: Mode-switch latency ≤ 1 frame (Invariant 4)**
-Given the thermal flag flips between two consecutive `estimate(...)` calls
-When the second call runs
-Then the new mode IS the new flag's mode (no carry-over from the previous call).
-
-**AC-3: Jacobian covariance is SPD**
-Given a successful Jacobian path run
-Then `np.linalg.cholesky(covariance_6x6)` succeeds; the matrix is symmetric to 1e-10 tolerance.
-
-**AC-4: `covariance_mode == JACOBIAN` on Jacobian path**
-When the Jacobian path runs successfully
-Then `PoseEstimate.covariance_mode == CovarianceMode.JACOBIAN` AND `current_covariance_mode() == JACOBIAN`.
-
-**AC-5: `source_label == SATELLITE_ANCHORED` on success regardless of path (Invariant 7)**
-Both Marginals and Jacobian paths emit `SATELLITE_ANCHORED` on success.
-
-**AC-6: `CovarianceDegradedWarning` emitted via `warnings.warn`**
-When the Jacobian path runs
-Then `warnings.warn(CovarianceDegradedWarning("..."))` is called; verified via `warnings.catch_warnings()` test harness. The warning is NOT raised as an exception.
-
-**AC-7: `warnings.warn` rate-limited to ONE per 60 s window**
-Given the Jacobian path runs at 3 Hz for 70 s (total 210 calls)
-When the warnings emitted are counted
-Then exactly 2 warnings are emitted (one for window 0–60 s, one for window 60–120 s).
-
-**AC-8: WARN log rate-limited similarly**
-Same scenario as AC-7 → exactly 2 WARN log records `kind="c4.pose.covariance_degraded"`.
-
-**AC-9: Marginals path unchanged by refactor**
-Given a Marginals-only test fixture (already exercised in AZ-?)
-When the AZ-? tests are re-run after this task's refactor
-Then ALL AZ-? AC-1..AC-11 still pass without modification.
-
-**AC-10: Non-SPD Jacobian covariance defensive raise**
-Given an inlier set producing a near-singular `JᵀJ` (synthetically degenerate)
-When the Jacobian path runs
-Then `PnpFailureError("non-SPD Jacobian covariance; numerical instability")` is raised; ERROR log emitted.
-
-**AC-11: Jacobian accuracy within ~5–10% of Marginals on synthetic baseline**
-Given 100 synthetic frames AND ground-truth pose
-When BOTH paths are run on the same input (test harness toggles `thermal_state.throttle`)
-Then the Jacobian path's RMSE is within 1.10× the Marginals path's RMSE (10% tolerance per ADR-006). Informational; does NOT block this AC if the actual ratio is between 1.0 and 1.10.
-
-**AC-12: Jacobian path skips iSAM2 factor add**
-Given the Jacobian path runs
-Then NO `isam2_graph_handle.add_factor(...)` call AND NO `isam2_graph_handle.update()` call is made; only `last_anchor_age_ms()` is read (for the `last_satellite_anchor_age_ms` field).
-
-**AC-13: FDR `pose.frame_done` distinguishes path**
-The `mode` field in the FDR record matches the path: `"marginals"` or `"jacobian"`.
-
-## Non-Functional Requirements
-
-**Performance**
-- `_estimate_jacobian_path` p95 ≤ 15 ms target / 25 ms hard limit (per C4-PT-01).
-- Mode-switch latency: zero — the dispatch is a single `if` at call entry.
-
-**Compatibility**
-- OpenCV ≥ 4.12.0 (same as AZ-?).
-- The Jacobian computation MUST work for both analytical (if OpenCV exposes it for IPPE) AND numerical (`SE3Utils.numerical_jacobian`) paths; choose analytical when available, fall back to numerical.
-
-**Reliability**
-- SPD-invariant defensive check covers the Jacobian path.
-- `CovarianceDegradedWarning` rate-limiting prevents log flooding under sustained throttle.
-
-## Unit Tests
-
-| AC Ref | What to Test | Required Outcome |
-|--------|--------------|------------------|
-| AC-1 | Alternating thermal flag → alternating mode | Each call's mode matches the flag |
-| AC-2 | Mode-switch within 1 frame | New mode on the next call |
-| AC-3 | Jacobian SPD | Cholesky succeeds |
-| AC-4 | Jacobian mode reporting | Both `PoseEstimate` and `current_covariance_mode()` |
-| AC-5 | Source label always SATELLITE_ANCHORED | Both paths |
-| AC-6 | `warnings.warn` not raise | Captured via `warnings.catch_warnings()` |
-| AC-7 | Warning rate-limited | 2 warnings over 70 s |
-| AC-8 | WARN log rate-limited | 2 records over 70 s |
-| AC-9 | AZ-? tests still pass | Re-run all 11 ACs after refactor |
-| AC-10 | Near-singular Jacobian → defensive raise | `PnpFailureError` |
-| AC-11 | Jacobian within 10% of Marginals | RMSE ratio ≤ 1.10 |
-| AC-12 | Jacobian skips iSAM2 add | No `add_factor`/`update` calls |
-| AC-13 | FDR mode field | `"marginals"` or `"jacobian"` |
-
-## Constraints
-
-- **Single-threaded** by contract; same thread as C5 + the Marginals task.
-- **No buffering** — mode dispatch is per-call; thermal flag is read at call entry.
-- **Jacobian path skips iSAM2 factor add** — design choice per C5's degraded mode requirement.
-- **`CovarianceDegradedWarning` is emitted via `warnings.warn`** — NEVER raised.
-- **Rate-limiting is 60 s** for both the warning and the WARN log.
-- **Refactor of Marginals path MUST be behaviour-preserving** — AZ-?'s tests pass unchanged (AC-9).
-
-## Risks & Mitigation
-
-**Risk 1: OpenCV may not expose an analytical Jacobian for `SOLVEPNP_IPPE`**
-- *Mitigation*: implementation falls back to `SE3Utils.numerical_jacobian` (forward-difference, 1e-6 step). Numerical Jacobian costs ~5 ms additional but stays within the 15 ms target.
-
-**Risk 2: Jacobian-derived covariance is overly optimistic on hard frames**
-- *Mitigation*: ADR-006 documents the ~5–10% accuracy loss; AC-11 verifies within tolerance. AC-NEW-5 (full operating envelope verification) belongs to E-BBT / NFT-LIM-04 — workstation-baseline portion only here.
-
-**Risk 3: The `_jacobian_warn_window` rate-limiting might miss a warning if the first frame after the 60 s rollover is a Marginals frame**
-- *Mitigation*: window is reset on every Jacobian call; rollover boundaries are not special-cased. The intent is "at least one warning per active throttle window of duration ≥ 60 s", which is satisfied.
-
-**Risk 4: Refactoring AZ-?'s `estimate(...)` may introduce subtle behaviour changes**
-- *Mitigation*: AC-9 mandates AZ-?'s full test suite passes unchanged. The refactor is mechanical (extract method); review hook ensures fidelity.
-
-## Runtime Completeness
-
-- **Named capability**: D-CROSS-LATENCY-1 hybrid — Jacobian fallback + per-frame thermal-driven mode switch.
-- **Production code that must exist**: real Jacobian computation (analytical or numerical); real per-frame thermal-driven dispatch; real SPD defensive check; real `warnings.warn` emission with rate-limiting; real WARN log emission with rate-limiting; refactor of Marginals path into a private method; FDR record `mode` field distinguishes paths.
-- **Allowed external stubs**: `FakeISam2GraphHandle`, `FakeWgsConverter`, `FakeSE3Utils`, `FakeFdrClient` — same as AZ-?.
-- **Unacceptable substitutes**: a Marginals call with a "skip the cubic part" hack (would not deliver the latency budget); raising `CovarianceDegradedWarning` instead of emitting via `warnings.warn` (changes the user-facing semantics); skipping the rate limiter (would flood logs under sustained throttle); buffering thermal state across frames (would violate Invariant 4); emitting warnings inside the Marginals path "for symmetry" (no — the warning is documentation that the system is in degraded mode).
-
-## Contract
-
-This task implements the Jacobian / hybrid portion of `_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md`.
-After this task lands, the contract's invariants (especially Invariants 3, 4, 6, 9) are FULLY exercised.