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.

Cycle-1 operational reality: the airborne binary wires C4 through _STRATEGY_REGISTRY + register_airborne_strategies() (AZ-591) with a single strategy slot (opencv_gtsam — C4PoseConfig.KNOWN_POSE_STRATEGIES = {"opencv_gtsam"}). Constructor injection flows through the pre_constructed dict passed to compose_root(config, pre_constructed=...) (AZ-618 umbrella → AZ-623 c5 helpers phase + AZ-625 eager iSAM2 handle phase). The c4_pose slot lists ("c282_ransac_filter", "c5_wgs_converter", "c5_se3_utils", "c5_isam2_graph_handle") in AIRBORNE_REQUIRED_PRE_CONSTRUCTED_KEYS; c13_fdr and clock are optional. The c5_isam2_graph_handle slot is the shared GTSAM substrate seam — build_pre_constructed eagerly invokes build_state_estimator once (AZ-625 / Phase E.5) so the (StateEstimator, ISam2GraphHandle) tuple is constructed BEFORE either the C4 or C5 wrapper runs (C4 runs first in topo order via _C4_POSE_DEPENDS_ON = ("c1_vio", "c3_matcher"), then C5 short-circuits on the prebuilt estimator via the internal _c5_prebuilt_estimator key). The cross-seam identity invariant (c4_pose._isam2_handle is c5_state._isam2_handle) is verified by AC-625.3. Missing required keys raise AirborneBootstrapError at composition time, naming the consumer and missing key.

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 (cv2)	>=4.11.0.86,<4.12 (cycle-1 relaxed pin; D-CROSS-CVE-1 deferred — see _docs/_process_leftovers/2026-05-11_d_cross_cve_1_opencv_pin_deferred.md)	solvePnPRansac with SOLVEPNP_IPPE flag in opencv_gtsam_estimator.py; 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).