mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 18:41:13 +00:00
[AZ-355] C4 PoseEstimator Protocol + factory + DTOs + composition
Land the foundational C4 surface AZ-358 (Marginals) and AZ-361 (Hybrid) build on top of: - PoseEstimator Protocol (@runtime_checkable): estimate(...) + current_covariance_mode(). - Error hierarchy: PoseEstimatorError, PnpFailureError, PoseEstimatorConfigError; CovarianceDegradedWarning as a Warning subclass (warnings.warn path, not raised). - ISam2GraphHandle Protocol stub (READ-ONLY view, get_pose_key only) decoupled from C5's concrete ISam2GraphHandleImpl. - C4PoseConfig (frozen dataclass) + register on c4_pose import. - runtime_root/pose_factory.build_pose_estimator with lazy-import fallback; INFO log c4.pose.strategy_loaded; shares ingest-thread binding with C5 per ADR-003. DTO restructuring (cross-cutting): retire the legacy raw-4x4 PoseEstimate(int frame_id, datetime timestamp, pose_se3, ...) and ship the contract shape PoseEstimate(UUID, LatLonAlt, Quat, np.ndarray, CovarianceMode, PoseSourceLabel, last_satellite_anchor_age_ms, emitted_at). C5 add_pose_anchor in both gtsam_isam2 + eskf_baseline migrated in lockstep via WGS84->ENU + Quat->R helpers; test fixtures updated. VIO output stays on the raw shape until AZ-331 (C1 protocol) lands. LatLonAlt upgraded to slots=True per AC-2. ThermalState stub added to _types/thermal.py so the Protocol typechecks pre-AZ-302. Tests: 25 new in tests/unit/c4_pose/test_az355_pose_protocol.py covering AC-1..AC-10 + factory wiring + config validation; full repo: 685 passed, 2 pre-existing CI-only skips. Jira transition deferred: MCP "Not connected"; leftover entry in _docs/_process_leftovers/2026-05-11_jira_transition_az355_deferred.md. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,189 @@
|
||||
# Batch 20 — AZ-355 C4 PoseEstimator Protocol + Factory + DTOs + Composition
|
||||
|
||||
**Date**: 2026-05-11
|
||||
**Tracker**: Jira AZ-355 (Epic AZ-259 / E-C4)
|
||||
**Cycle**: 1
|
||||
**Status**: complete; tests + lint + format green; Jira transition deferred (see leftovers).
|
||||
|
||||
## Scope landed
|
||||
|
||||
AZ-355 ships the foundational C4 surface that AZ-358 (Marginals) and
|
||||
AZ-361 (Hybrid) will fill in. Concrete impl of
|
||||
`OpenCVGtsamPoseEstimator` is explicitly out of scope.
|
||||
|
||||
### Public surface
|
||||
|
||||
* `src/gps_denied_onboard/components/c4_pose/interface.py` — the
|
||||
`PoseEstimator` Protocol (`@runtime_checkable`). Two methods:
|
||||
`estimate(match_result, calibration, thermal_state) -> PoseEstimate`
|
||||
and `current_covariance_mode() -> CovarianceMode`.
|
||||
* `src/gps_denied_onboard/components/c4_pose/errors.py` — error
|
||||
hierarchy: `PoseEstimatorError` (base) → `PnpFailureError`,
|
||||
`PoseEstimatorConfigError`; plus `CovarianceDegradedWarning` as a
|
||||
`Warning` subclass (NOT an `Exception` — `warnings.warn(...)` is
|
||||
the only emission path).
|
||||
* `src/gps_denied_onboard/components/c4_pose/_isam2_handle.py` —
|
||||
minimal READ-ONLY view of C5's iSAM2 graph exposing only
|
||||
`get_pose_key(frame_id) -> int`. Decoupled from C5's concrete
|
||||
`ISam2GraphHandleImpl` so C4 never imports C5 internals.
|
||||
* `src/gps_denied_onboard/components/c4_pose/config.py` —
|
||||
`C4PoseConfig` (frozen dataclass) with `strategy`,
|
||||
`ransac_iterations`, `ransac_reprojection_threshold_px`,
|
||||
`thermal_throttle_threshold_celsius`. `__post_init__` validates
|
||||
the values via `ConfigError`.
|
||||
* `src/gps_denied_onboard/runtime_root/pose_factory.py` —
|
||||
`build_pose_estimator(config, *, ransac_filter, wgs_converter,
|
||||
se3_utils, isam2_graph_handle) -> PoseEstimator`. Lazy-import
|
||||
fallback via `importlib.import_module(...)` mirrors the C5 / C8
|
||||
factories. Emits one INFO log `c4.pose.strategy_loaded` per
|
||||
successful build.
|
||||
* `src/gps_denied_onboard/components/c4_pose/__init__.py` — public
|
||||
re-exports per AC-8 + registers `C4PoseConfig` against the
|
||||
global config registry on import.
|
||||
* `src/gps_denied_onboard/runtime_root/__init__.py` — re-exports
|
||||
`build_pose_estimator`, `register_pose_estimator`,
|
||||
`clear_pose_registry`, `list_registered_pose_strategies`.
|
||||
|
||||
### DTO restructuring (cross-cutting)
|
||||
|
||||
The C4 contract pins `PoseEstimate` at:
|
||||
|
||||
```python
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class PoseEstimate:
|
||||
frame_id: UUID
|
||||
position_wgs84: LatLonAlt
|
||||
orientation_world_T_body: Quat
|
||||
covariance_6x6: np.ndarray # SPD, 6x6
|
||||
covariance_mode: CovarianceMode
|
||||
source_label: PoseSourceLabel
|
||||
last_satellite_anchor_age_ms: int
|
||||
emitted_at: int # monotonic_ns
|
||||
```
|
||||
|
||||
The legacy `PoseEstimate(int frame_id, datetime timestamp, 4x4 pose_se3,
|
||||
covariance_6x6, str covariance_mode, mre_px)` shape that C5 consumed
|
||||
in earlier batches has been retired in this batch (user-approved scope
|
||||
expansion).
|
||||
|
||||
C5 consumers (`gtsam_isam2_estimator.add_pose_anchor`,
|
||||
`eskf_baseline.add_pose_anchor`) and their test fixtures are migrated
|
||||
in lockstep:
|
||||
|
||||
* `pose.emitted_at` (int ns) replaces `_datetime_to_ns(pose.timestamp)`.
|
||||
* `pose.covariance_mode.value` (enum → str) replaces the old loose
|
||||
string.
|
||||
* A new `_pose_estimate_to_matrix(pose)` private helper on each
|
||||
estimator converts `LatLonAlt + Quat` back into a 4x4 SE(3) using
|
||||
the injected ENU origin (default `(0, 0, 0)` for synthetic
|
||||
fixtures). Round-trip is numerically clean because
|
||||
`WgsConverter.local_enu_to_latlonalt` and `latlonalt_to_local_enu`
|
||||
are inverses at the metre scale.
|
||||
* The VIO consumer code path (`pose.timestamp` / `pose.pose_se3`)
|
||||
is unchanged — `VioOutput` still carries the raw form because the
|
||||
C1 contract has not migrated yet.
|
||||
|
||||
### Forward-declared types
|
||||
|
||||
`src/gps_denied_onboard/_types/thermal.py` is a new module holding the
|
||||
minimal `ThermalState(throttle: bool)` DTO. AZ-302 (C7 ThermalState
|
||||
publisher) is responsible for the full producer surface; this stub
|
||||
exists solely so the C4 Protocol typechecks without a `TYPE_CHECKING`
|
||||
indirection cycle. The contract pins only the `throttle` field.
|
||||
|
||||
### Geo DTO upgrade
|
||||
|
||||
`_types/geo.LatLonAlt` is upgraded to `frozen=True, slots=True` per
|
||||
the C4 contract AC-2. The field set is unchanged
|
||||
(`lat_deg, lon_deg, alt_m`); no consumer needed a code change.
|
||||
|
||||
## Architectural notes
|
||||
|
||||
* **ADR-001 / ADR-009 alignment** — single concrete strategy
|
||||
(`opencv_gtsam`) behind a `@runtime_checkable` Protocol. The
|
||||
factory is the only production path that resolves the strategy
|
||||
name; tests pre-register fakes via `register_pose_estimator`.
|
||||
* **Single-thread invariant (AC-9)** — the C4 estimator shares C5's
|
||||
ingest thread via the existing `bind_state_ingest_thread` helper
|
||||
on the state factory. A second binding from a different thread
|
||||
raises `StateIngestThreadAlreadyBoundError`. The same helper is
|
||||
reused; no duplicate thread-binding state on the pose factory.
|
||||
* **`CovarianceDegradedWarning` semantics (AC-4)** — Python's class
|
||||
hierarchy has `Warning < Exception < BaseException`, so a strict
|
||||
`not issubclass(CovarianceDegradedWarning, Exception)` would be
|
||||
False; the contract's intent is behavioural — `warnings.warn`
|
||||
does NOT raise, so `try / except Exception` around `warnings.warn`
|
||||
does NOT catch the warning. The test covers the behavioural
|
||||
contract and the AC text is documented in the test docstring so
|
||||
the discrepancy is visible to future maintainers.
|
||||
|
||||
## Test coverage
|
||||
|
||||
`tests/unit/c4_pose/test_az355_pose_protocol.py` — 25 tests across
|
||||
AC-1..AC-10 plus bonus factory / config tests.
|
||||
|
||||
| AC | Test focus | Outcome |
|
||||
|----|------------|---------|
|
||||
| AC-1 | `runtime_checkable` Protocol — full fake passes; partial fake fails | green |
|
||||
| AC-2 | `LatLonAlt`, `Quat`, `PoseEstimate` frozen + slotted | green |
|
||||
| AC-3 | Enum membership for `CovarianceMode` + `PoseSourceLabel` | green |
|
||||
| AC-4 | `CovarianceDegradedWarning` Warning semantics + behavioural test | green |
|
||||
| AC-5 | `PnpFailureError` IS-A `Exception`, caught by `except Exception` | green |
|
||||
| AC-6 | Factory rejects unknown strategy + non-conforming graph handle; ERROR log emitted | green |
|
||||
| AC-7 | Factory accepts `opencv_gtsam`; one INFO log `c4.pose.strategy_loaded` with structured fields | green |
|
||||
| AC-8 | Public `__all__` includes the required surface; internals excluded | green |
|
||||
| AC-9 | `bind_state_ingest_thread` rejects second different-thread binding; idempotent on same thread | green |
|
||||
| AC-10 | `ISam2GraphHandle` `runtime_checkable` — fake with `get_pose_key` passes; bare class fails | green |
|
||||
| Bonus | Factory wires deps through to the strategy | green |
|
||||
| Bonus | Lazy-import fallback raises `PoseEstimatorConfigError` when concrete module is absent | green |
|
||||
| Bonus | `C4PoseConfig` rejects bad strategy / zero ransac iterations at `__post_init__` | green |
|
||||
| Bonus | `PoseEstimate.frame_id` is `UUID`; `emitted_at` round-trips as int ns | green |
|
||||
|
||||
C5 regression suite — 160 unit tests (test_az381..test_az388) green
|
||||
after the `PoseEstimate` migration. Full repo: 685 passed, 2 skipped
|
||||
(pre-existing CI-only skips for `cmake` and `actionlint`).
|
||||
|
||||
## Quality gates
|
||||
|
||||
* `ruff check` on every changed file — clean.
|
||||
* `ruff format` applied; 3 files reformatted.
|
||||
* `ReadLints` on the changed surface — no diagnostics.
|
||||
* Full `pytest` — green (685 passed, 2 skipped).
|
||||
|
||||
## Known follow-ups
|
||||
|
||||
* **AZ-358 (Marginals)** — needs to wire the concrete C5
|
||||
`ISam2GraphHandleImpl` to expose `get_pose_key` so the C4 Protocol
|
||||
isinstance check passes at composition time. Currently the C5 impl
|
||||
has `key_for_frame` on the estimator itself, not on the handle.
|
||||
Either:
|
||||
1. Add `get_pose_key` to `ISam2GraphHandleImpl` delegating to
|
||||
`estimator.key_for_frame`, or
|
||||
2. Have the runtime root construct an adapter object that
|
||||
implements only the C4 Protocol over the C5 handle.
|
||||
|
||||
Option 1 is the cheaper path; flag at AZ-358 kick-off.
|
||||
* **`ThermalState` full surface** — AZ-302 (C7) will add captured_at,
|
||||
temperature reading, thermal-zone source. The stub here pins only
|
||||
`throttle: bool`; AZ-302 must keep that field.
|
||||
* **`CovarianceDegradedWarning` filter policy** — the contract
|
||||
recommends `warnings.simplefilter("once", CovarianceDegradedWarning)`
|
||||
to avoid log flood. AZ-361 (Hybrid) owns the actual emit path and
|
||||
should install the filter near the composition root.
|
||||
|
||||
## Cross-task constraint surfaced
|
||||
|
||||
The PoseEstimate migration deliberately keeps `VioOutput` on the
|
||||
raw `datetime timestamp` + `4x4 pose_se3` shape (C1 contract has not
|
||||
migrated yet). When the C1 protocol task (AZ-331) lands it may bring
|
||||
`VioOutput` in line — at which point `_datetime_to_ns(vio.timestamp)`
|
||||
+ `_pose_se3_to_*(vio.pose_se3)` calls in C5 should be migrated in
|
||||
lockstep. Recorded for AZ-331's planning.
|
||||
|
||||
## Tracker
|
||||
|
||||
Jira MCP still returns `"Not connected"` at the time of writing.
|
||||
The status transition `AZ-355: To Do → Done` is recorded in
|
||||
`_docs/_process_leftovers/2026-05-11_jira_transition_az355_deferred.md`.
|
||||
The previous `_docs/_process_leftovers/2026-05-11_jira_transition_az386_deferred.md`
|
||||
remains pending — both will be replayed when the MCP is reconnected.
|
||||
Reference in New Issue
Block a user