# C4 — Pose Estimation

## 1. High-Level Overview

**Purpose**: convert `MatchResult` (2D-3D correspondences) into a `PoseEstimate` — WGS84 position + 6×6 covariance + provenance label + `last_satellite_anchor_age_ms` — using OpenCV `solvePnPRansac` (IPPE) wrapped in GTSAM `Marginals` for native 6×6 posterior covariance recovery (D-C4-2 = (b)). Under thermal throttle, auto-degrades to Jacobian-based covariance (D-C4-2 = (a)) per the D-CROSS-LATENCY-1 hybrid.

**Architectural Pattern**: single concrete implementation `OpenCVGtsamPoseEstimator` behind the `PoseEstimator` interface. The pose estimator and the state estimator (C5) **share the GTSAM substrate**; the C4 factor is added directly to C5's iSAM2 graph rather than computed in isolation.

**Upstream dependencies**:
- C3.5 → `MatchResult` (refined or passthrough).
- C5 StateEstimator — supplies the GTSAM iSAM2 handle so C4 can add its factor in-graph (architecture principle: shared substrate per ADR-003).
- Camera calibration artifact — for intrinsics + distortion + body-to-camera extrinsics.
- C7 InferenceRuntime — only indirectly via the LightGlue inliers fed in from C3 / C3.5; C4 itself is OpenCV+GTSAM, not GPU-bound.

**Downstream consumers**:
- C5 StateEstimator (consumes `PoseEstimate`).

## 2. Internal Interfaces

### Interface: `PoseEstimator`

| Method | Input | Output | Async | Error Types |
|--------|-------|--------|-------|-------------|
| `estimate` | `MatchResult, CameraCalibration, ThermalState` | `PoseEstimate` | No | `PnpFailureError`, `CovarianceDegradedWarning` |
| `current_covariance_mode` | `()` | `CovarianceMode` enum {MARGINALS, JACOBIAN} | No | — |

**Input DTOs**:
```
MatchResult:                   see C3 / C3.5
CameraCalibration:             see C5
ThermalState:                  see C7 (telemetry from jetson-stats)
```

**Output DTOs**:
```
PoseEstimate:
  frame_id:                       uuid
  position_wgs84:                 LatLonAlt — degrees, degrees, metres MSL
  orientation_world_T_body:       Quat (w, x, y, z)
  covariance_6x6:                 Matrix6 — position (3x3) + orientation (3x3) sub-matrices
  covariance_mode:                CovarianceMode {MARGINALS, JACOBIAN}
  source_label:                   enum {satellite_anchored, visual_propagated, dead_reckoned}
  last_satellite_anchor_age_ms:   int — bin input for AC-1.3
  emitted_at:                     monotonic_ns
```

## 3. External API Specification

Not applicable.

## 4. Data Access Patterns

Stateless w.r.t. persistent storage; reads camera calibration once at construction.

## 5. Implementation Details

**Algorithmic Complexity**: `solvePnPRansac` is `O(I · trials)` in inlier count and RANSAC trials; `Marginals.marginalCovariance(pose_key)` is `O(K^3)` in keyframe-window size for the steady-state path (D-C5-3 K=10–20). Jacobian-degraded mode is `O(I)`.

**State Management**:
- Stateless w.r.t. flight history (C5 owns history).
- Holds the GTSAM `Marginals` factor handle and the OpenCV `solvePnPRansac` configuration.
- Holds a reference to the shared GTSAM iSAM2 graph owned by C5 — does **not** own it.

**Key Dependencies**:

| Library | Version | Purpose |
|---------|---------|---------|
| OpenCV | ≥ 4.12.0 (CVE-2025-53644 mitigation) | `solvePnPRansac` with `SOLVEPNP_IPPE` flag; D-C4-1 = (b) |
| GTSAM (Python + C++) | per Plan-phase pin | `Marginals.marginalCovariance(pose_key)` for native 6×6 covariance |
| Eigen | matches GTSAM | Lie-algebra math |

**Error Handling Strategy**:
- `PnpFailureError`: RANSAC convergence failure or degenerate match geometry. Emit no `PoseEstimate`; C5 falls back to VIO-only with provenance label `visual_propagated`.
- `CovarianceDegradedWarning`: thermal-throttle telemetry crossed the configurable threshold; auto-switch to Jacobian-based covariance (D-CROSS-LATENCY-1). Emit `PoseEstimate` with `covariance_mode = JACOBIAN`. NOT a fatal condition.
- The thermal-throttle decision is per-frame; once telemetry returns below threshold, switch back to MARGINALS on the next frame.

## 6. Extensions and Helpers

| Helper | Purpose | Used By |
|--------|---------|---------|
| `SE3Utils` | shared with C1, C5 | C1, C4, C5 |
| `WgsConverter` | local-tangent-plane ↔ WGS84 latitude/longitude/altitude | C4, C8 |
| `RansacFilter` | shared RANSAC + reprojection residual | C3, C3.5, C4 |

## 7. Caveats & Edge Cases

**Known limitations**:
- Posterior covariance accuracy depends on the GTSAM substrate being healthy. C5's iSAM2 instability propagates into C4's covariance honesty.
- Jacobian-degraded covariance is a known accuracy trade (~5–10% loss per ADR-006); accepted under thermal throttle, never accepted on the steady-state path.

**Potential race conditions**:
- Concurrent calls to `estimate` would race on the shared GTSAM graph. Single-threaded hot path; composition root binds C4 + C5 to the same thread.

**Performance bottlenecks**:
- `Marginals.marginalCovariance(pose_key)` is the dominant cost in steady state (~30–90 ms). The D-CROSS-LATENCY-1 hybrid trades this for ~5–15 ms Jacobian under thermal throttle.

## 8. Dependency Graph

**Must be implemented after**: C3.5 (input), C5 (shared GTSAM substrate; circular at the design level — both must be co-developed but C5's iSAM2 graph must be constructable without C4 calling into it).

**Can be implemented in parallel with**: C1, C6 — independent paths.

**Blocks**: C5 (graph factor add depends on C4), F3 / F6 / F7.

## 9. Logging Strategy

| Log Level | When | Example |
|-----------|------|---------|
| ERROR | `PnpFailureError` | `C4 PnP failure on frame=12345; mode=marginals; falling back to visual_propagated` |
| WARN | `CovarianceDegradedWarning`; thermal throttle entered | `C4 covariance degraded to JACOBIAN; thermal_throttle=true; clock=mhz=750` |
| INFO | Strategy ready | `C4 ready: estimator=opencv_gtsam, default_covariance=MARGINALS` |
| DEBUG | per-frame inlier count + residual + chosen mode | `C4 frame=12345 inliers=412 residual=0.9px mode=MARGINALS` |

**Log format**: structured JSON.
**Log storage**: stdout / journald / FDR via C13 (ERROR + WARN always; DEBUG only via FDR per-frame estimate row).