# Contract: `PoseEstimator` Protocol

**Owner**: c4_pose (epic AZ-259 / E-C4)
**Producer task**: AZ-355 (Protocol + DTO + factory + composition)
**Consumer tasks**: AZ-358 (`OpenCVGtsamPoseEstimator` Marginals path), AZ-361 (D-CROSS-LATENCY-1 hybrid: Jacobian fallback + thermal-state-driven mode switch). Downstream c5_state (epic AZ-260) which consumes `PoseEstimate`.
**Version**: 1.0.0
**Status**: draft, awaiting Producer task implementation
**Last Updated**: 2026-05-10
**Module-layout home**: `src/gps_denied_onboard/components/c4_pose/interface.py` (Protocol), `src/gps_denied_onboard/components/c4_pose/__init__.py` (re-exports), `src/gps_denied_onboard/runtime_root/pose_factory.py` (factory)

## Purpose

Defines the public interface for the C4 pose estimator: `estimate(match_result, calibration, thermal_state) -> PoseEstimate` produces a WGS84 position + 6×6 covariance + provenance label by running OpenCV `solvePnPRansac` (`SOLVEPNP_IPPE`) and recovering the posterior 6×6 covariance via GTSAM `Marginals.marginalCovariance(pose_key)` against C5's shared iSAM2 graph. Under thermal throttle (D-CROSS-LATENCY-1 / ADR-006), the implementation switches per-frame to Jacobian-derived covariance accepting ~5–10% accuracy loss to preserve the AC-4.1 latency budget. `current_covariance_mode()` exposes the per-frame decision for FDR provenance and AC-NEW-5 verification.

There is exactly ONE concrete implementation (`OpenCVGtsamPoseEstimator`); the Protocol exists for ADR-009 (interface-first DI) so consumers (C5, runtime root) hold a typed reference rather than the concrete class. ADR-002 build-time exclusion does NOT apply (one strategy only) — but lazy-import via the factory remains the entry-point pattern for symmetry with C2 / C2.5 / C3 / C3.5.

The shared `RansacFilter` (AZ-282), `WgsConverter` (AZ-279), and `SE3Utils` (AZ-277) helpers are constructor-injected. The C5 iSAM2 graph handle is constructor-injected from the runtime root; C4 NEVER owns the graph (ADR-003 shared substrate).

## Public API

### Protocol: `PoseEstimator`

```python
from typing import Protocol, runtime_checkable
from gps_denied_onboard._types import (
    MatchResult, CameraCalibration, ThermalState, PoseEstimate, CovarianceMode,
)


@runtime_checkable
class PoseEstimator(Protocol):
    """Single-pose estimator producing WGS84 + 6×6 covariance + provenance label. Stateless per-frame except for the constructor-injected shared GTSAM substrate (owned by C5)."""

    def estimate(
        self,
        match_result: MatchResult,
        calibration: CameraCalibration,
        thermal_state: ThermalState,
    ) -> PoseEstimate:
        """Run PnP → factor add → covariance recovery. Per-frame thermal decision: `thermal_state.throttle == True` → Jacobian path (cheap, ~5–10% accuracy loss); `False` → Marginals path (production-default).

        Raises:
            PnpFailureError: RANSAC convergence failure or degenerate match geometry. C5 falls back to VIO-only with `source_label = "visual_propagated"`. NEVER converted to a fallback PoseEstimate; C5 is the place where the fallback decision is taken.
        """
        ...

    def current_covariance_mode(self) -> CovarianceMode:
        """Return the mode used for the LAST `estimate` call: `CovarianceMode.MARGINALS` or `CovarianceMode.JACOBIAN`. Used by C5 for FDR provenance and by C4-IT-03 to verify the per-frame switch."""
        ...
```

**Invariants**:

1. **Single-threaded by contract** — bound to the SAME ingest thread as C5 (composition root enforces; shared GTSAM substrate per ADR-003 is non-thread-safe).
2. **Stateless w.r.t. flight history for `estimate`** — relies solely on inputs + the shared iSAM2 graph (which carries history but is C5-owned).
3. **Per-frame mode decision** — `thermal_state.throttle` is read at call entry; the choice between Marginals/Jacobian is made on EVERY call independently. NO hysteresis, NO smoothing, NO operator-tooling override at this layer (R10 covers operator tuning at a higher layer via `config`).
4. **Mode-switch latency ≤ 1 frame** — switching from JACOBIAN to MARGINALS or back happens immediately on the next `estimate` call when the thermal flag flips. C4-IT-03 verifies.
5. **`PoseEstimate.covariance_6x6` is always SPD** — both paths produce SPD matrices; non-SPD is a bug. C4-IT-02 verifies.
6. **`PoseEstimate.covariance_mode` matches the path actually taken** — never reports MARGINALS while computing Jacobian.
7. **`source_label` is set by C4 to `"satellite_anchored"`** unconditionally on success; C5 is the component that may downgrade it to `"visual_propagated"` or `"dead_reckoned"` when the gate decides. C4 never emits `"visual_propagated"` from `estimate` directly.
8. **`last_satellite_anchor_age_ms` is provided BY C5 and PASSED THROUGH** — C4 receives the current value via the runtime root + caches it; on emit, the value reflects the time since C5's last anchor add. C4 does not compute this metric independently.
9. **`PnpFailureError` is the ONLY non-warning exception escaping `estimate`** — `CovarianceDegradedWarning` is a Python `Warning` (filterwarnings-compatible), NOT an exception.

### DTOs (in `_types/pose.py`)

```python
from dataclasses import dataclass
from enum import Enum
from uuid import UUID
import numpy as np


class CovarianceMode(Enum):
    MARGINALS = "marginals"
    JACOBIAN = "jacobian"


class PoseSourceLabel(Enum):
    SATELLITE_ANCHORED = "satellite_anchored"
    VISUAL_PROPAGATED = "visual_propagated"
    DEAD_RECKONED = "dead_reckoned"


@dataclass(frozen=True, slots=True)
class LatLonAlt:
    """WGS84 position. lat/lon in degrees, alt in metres MSL."""
    lat_deg: float
    lon_deg: float
    alt_m_msl: float


@dataclass(frozen=True, slots=True)
class Quat:
    """Unit quaternion (w, x, y, z); scalar-first."""
    w: float
    x: float
    y: float
    z: float


@dataclass(frozen=True, slots=True)
class PoseEstimate:
    """Pose estimate emitted by C4 to C5."""
    frame_id: UUID
    position_wgs84: LatLonAlt
    orientation_world_T_body: Quat
    covariance_6x6: np.ndarray  # shape (6, 6) float64; SPD; position (3x3) | orientation (3x3) blocks
    covariance_mode: CovarianceMode
    source_label: PoseSourceLabel  # C4 always emits SATELLITE_ANCHORED on success
    last_satellite_anchor_age_ms: int
    emitted_at: int  # monotonic_ns
```

### Error hierarchy (in `c4_pose/errors.py`)

```python
class PoseEstimatorError(Exception):
    """Base class."""


class PnpFailureError(PoseEstimatorError):
    """RANSAC convergence failure or degenerate match geometry. NEVER converted to a fallback PoseEstimate by C4 itself; C5 owns the fallback decision."""


class CovarianceDegradedWarning(Warning):
    """Per-frame thermal-state-driven Jacobian-path engagement. NOT an exception. Emitted via `warnings.warn(...)` at the start of every Jacobian-path frame; users SHOULD filter to one warning per 60 s window via `warnings.simplefilter("once")` to avoid log flooding."""
```

### Composition-root factory

```python
# In src/gps_denied_onboard/runtime_root/pose_factory.py

def build_pose_estimator(
    config: AppConfig,
    ransac_filter: RansacFilter,
    wgs_converter: WgsConverter,
    se3_utils: SE3Utils,
    isam2_graph_handle: ISam2GraphHandle,  # owned by C5, constructor-injected
) -> PoseEstimator:
    """Construct the configured C4 estimator at composition-root time. Currently only `"opencv_gtsam"` is defined; the Protocol exists for ADR-009.

    Raises:
        PoseEstimatorConfigError: invalid config; missing camera calibration; invalid `isam2_graph_handle`.
    """
    ...
```

Strategy resolution table:

| `config.pose.strategy` | Module path | Class | Notes |
|---|---|---|---|
| `"opencv_gtsam"` | `gps_denied_onboard.components.c4_pose.opencv_gtsam_estimator` | `OpenCVGtsamPoseEstimator` | production-default; only strategy. |

Config-load-time validation:

- `config.pose.strategy` (enum, default `"opencv_gtsam"`).
- `config.pose.ransac_iterations` (int, default 200).
- `config.pose.ransac_reprojection_threshold_px` (float, default 4.0).
- `config.pose.thermal_throttle_threshold_celsius` (float, default 75.0) — informational only; the actual `ThermalState.throttle` decision is owned by C7, not C4.

## Test expectations summarised by Invariant

| Invariant | Test name | Assertion |
|---|---|---|
| 1 | thread-binding | composition root binds to the same thread as C5; second binding raises `RuntimeError`. |
| 2 | stateless reorder | shuffle 10 frames → same outputs (modulo iSAM2 graph state which is C5-owned). |
| 3 | per-frame mode decision | thermal flag flipped between consecutive frames → mode flips immediately. |
| 4 | mode-switch latency | switch happens on the NEXT `estimate` call after the flag changes (no buffering). |
| 5 | covariance SPD | every emitted `covariance_6x6` is symmetric AND positive-definite (Cholesky succeeds). |
| 6 | mode reporting honesty | when Jacobian path runs, `covariance_mode == JACOBIAN` AND `current_covariance_mode()` returns `JACOBIAN`. |
| 7 | source_label = SATELLITE_ANCHORED on success | C4 always emits SATELLITE_ANCHORED; downgrade is C5's job. |
| 8 | `last_satellite_anchor_age_ms` pass-through | matches the last value from C5's broadcast. |
| 9 | only `PnpFailureError` escapes | `CovarianceDegradedWarning` is via `warnings.warn` not `raise`. |

## What this contract does NOT define

- The OpenCV `solvePnPRansac` configuration — owned by the producer task.
- The GTSAM `Marginals` factor add path — owned by the Marginals task.
- The Jacobian covariance derivation — owned by the hybrid task.
- The C5 iSAM2 graph internals — owned by E-C5 (AZ-260).
- The `ThermalState` source — owned by E-C7 (AZ-249 / AZ-302).

## Producer-task / consumer-task split

- **Protocol task (TBD)**: Protocol, `PoseEstimate` + `LatLonAlt` + `Quat` + `CovarianceMode` + `PoseSourceLabel` DTOs, error hierarchy, factory, config schema extension.
- **Marginals task (TBD)**: `OpenCVGtsamPoseEstimator` core (PnP + IPPE + GTSAM `Marginals` factor add against C5's iSAM2 graph). Steady-state path only; fails fast if `thermal_state.throttle` is True (raises `NotImplementedError` until the hybrid task lands).
- **Hybrid task (TBD)**: D-CROSS-LATENCY-1 — Jacobian fallback + per-frame thermal-state-driven mode switch. Adds the JACOBIAN code path; replaces the Marginals task's `NotImplementedError` with the actual Jacobian implementation; verifies AC-NEW-5 (workstation portion).

## Versioning + change policy

- Protocol method-signature changes are MAJOR version bumps (lockstep update of consumers).
- DTO field additions are MINOR; field removals are MAJOR.
- Adding a third covariance mode (e.g., a learned-prior covariance) is a feature-cycle change; it adds an entry to `CovarianceMode` without changing the Protocol surface.
