mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 10:41:14 +00:00
Decompose Step 6 snapshot: 140 task specs + contract docs
Closes out greenfield Step 6 (Decompose) for all 14 components (C1-C13 + cross-cutting helpers/replay). Covers tasks AZ-266..AZ-446 plus the _dependencies_table.md and component contract documents. State file updated to greenfield Step 7 (Implement), not_started. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,194 @@
|
||||
# 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.
|
||||
Reference in New Issue
Block a user