gps-denied-onboard/_docs/02_document/components/06_c4_pose/description.md

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