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

### 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: `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`
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 | `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 |
| 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 `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 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

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.
