mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 08:51:12 +00:00
Update autodev state, architecture documentation, and glossary terms
Transitioned the autodev state to phase 21, reflecting the completion of Step 5 and the drafting of Step 6 epics. Revised the architecture documentation to clarify the roles of the Tile Manager and its components, ensuring accurate representation of the system's operational flow. Updated glossary entries for Flight State and Operator to incorporate recent changes and enhance clarity on component interactions and responsibilities.
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,115 @@
|
||||
# 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).
|
||||
Reference in New Issue
Block a user