azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 15:01:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-389] C5 orthorectifier emits mid-flight tiles to C6

Browse Source 
Adds an opt-in C5-internal orthorectifier (`_orthorectifier.py`) that
emits at most one tile-aligned JPEG candidate per nav frame to the
C6 `TileStore.write_tile` API.  Quality gates fire before any
OpenCV work: covariance Frobenius, inlier floor, source-label
(`SATELLITE_ANCHORED` only), and once-per-frame rate limit.

Cross-component import rule (AZ-507) is preserved: c5_state never
imports c6_tile_cache.  `runtime_root.state_factory` carries a new
`_C6MidFlightIngestAdapter` that builds the canonical
`TileMetadata` (`ONBOARD_INGEST` / `FRESH` / `PENDING`), hashes
the JPEG, and translates `FreshnessRejectionError` to a `None`
return so the orthorectifier silently swallows freshness
rejection per AC-NEW-3.

Wiring is opt-in via `C5StateConfig.orthorectifier.enabled`;
existing tests/binaries default to disabled and are unaffected.
Both `GtsamIsam2StateEstimator` and `EskfStateEstimator`
participate through new `attach_orthorectifier` /
`set_latest_nav_frame` extension methods (Protocol surface
unchanged).

Tests: 22 new unit tests cover AC-1..AC-9 plus inlier-floor
gate plus the composition-root adapter.  216/216 c5_state and
38/38 runtime-root + compose tests pass.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-16 09:02:33 +03:00
parent 811ddc8aa7
commit c5ffc14fe9
9 changed files with 1952 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
_docs/02_tasks/todo/AZ-389_c5_orthorectifier_c6.md
-101
View File
@@ -1,101 +0,0 @@
# C5 Orthorectifier → C6 mid-flight tile gen sub-path
**Task**: AZ-389_c5_orthorectifier_c6
**Name**: C5 internal orthorectifier — produces mid-flight tile candidates for C6 via existing `TileStore.write_tile` + `ONBOARD_INGEST` source
**Description**: Implement the orthorectifier sub-path inside C5: when a frame has converged in the iSAM2 graph (≥1 satellite anchor + visual consistency), apply the camera intrinsics + extrinsics + the C5-known pose to orthorectify the nav-camera frame into a tile-aligned image patch; persist it to C6 as a `TileSource.ONBOARD_INGEST` tile via the existing `TileStore.write_tile(tile_blob, metadata)` API (AZ-303). The orthorectifier is C5-internal (per epic spec § Scope: "orthorectifier (lives within C5 as an internal subcomponent)"); it consumes the converged pose + nav frame from a per-frame buffer; it emits at most ONE tile per frame (gated by quality thresholds: `cov_norm < threshold` AND `inlier_count > floor`). Triggered after a successful `current_estimate()` call when quality conditions hold AND `source_label == SATELLITE_ANCHORED`. Per AC-NEW-3 the emission is opportunistic: a `FreshnessRejectionError` from C6's freshness gate is caught and dropped (DEBUG log only).
**Complexity**: 3 points
**Dependencies**: AZ-384 (`current_estimate` body + cov norm), AZ-385 (only emit candidates when source_label == SATELLITE_ANCHORED), AZ-303 (`TileStore.write_tile` + `TileMetadata` + `TileQualityMetadata` + `TileSource.ONBOARD_INGEST`), AZ-263, AZ-269, AZ-266, AZ-272 (FDR)
**Component**: c5_state (epic AZ-260 / E-C5)
**Tracker**: AZ-389
**Epic**: AZ-260 (E-C5)
### Document Dependencies
- `_docs/02_document/contracts/c5_state/state_estimator_protocol.md`.
- `_docs/02_document/components/07_c5_state/description.md` — orthorectifier mention; § 1 downstream "C6 (mid-flight tile gen via orthorectifier)".
- `_docs/02_document/contracts/c6_tile_cache/tile_store.md` — `write_tile` API (the four-method baseline).
### History
The original v1.0.0 spec referenced a separate `tile_store.put_mid_flight_candidate(MidFlightTileCandidate)` API that does not exist; investigation against `c6_tile_cache/_types.py` and `interface.py` showed `TileSource.ONBOARD_INGEST` + `TileMetadata.quality_metadata` + `write_tile`'s built-in `FreshnessRejectionError` semantic already cover the entire mid-flight ingest path. AZ-559 was filed and immediately closed Won't Fix; the spec is rewritten here against the actual API surface.
## Problem
Without this task, the system never emits mid-flight tile candidates → C6's cache never grows in flight → AC-NEW-3 (mid-flight tile gen) is unachievable.
## Outcome
- `src/gps_denied_onboard/components/c5_state/_orthorectifier.py` defining:
- `Orthorectifier` class (component-internal; not in `__all__`).
- Method: `try_emit_candidate(frame, pose_estimate, cov_6x6, inlier_count, mre_px, source_label) -> TileId | None`.
- Quality gates: `cov_norm < cov_threshold` AND `inlier_count > inlier_floor` AND `source_label == SATELLITE_ANCHORED`.
- Orthorectification math: project nav-camera frame to tile plane via camera intrinsics + extrinsics + pose; nearest-neighbour or bilinear sampling.
- JPEG encoding of the orthorectified patch (via OpenCV `cv2.imencode`).
- Constructs a `TileMetadata` with `source = TileSource.ONBOARD_INGEST`, `voting_status = VotingStatus.PENDING`, `quality_metadata = TileQualityMetadata(estimator_label, covariance_2x2_horizontal_subblock, last_anchor_age_ms, mre_px, imu_bias_norm)`, `flight_id`, `companion_id`, `freshness_label = FreshnessLabel.FRESH` (the gate runs at insert time).
- Calls `tile_store.write_tile(jpeg_bytes, tile_metadata)`.
- Catches `FreshnessRejectionError` per AC-NEW-3 (opportunistic) and returns `None`; logs DEBUG `"c5.state.mid_flight_candidate_freshness_rejected"`.
- Returns the persisted `TileId` on success.
- Hook in `GtsamIsam2StateEstimator.current_estimate()` post-emission (or post-`add_pose_anchor` — implementer choice; gated to fire AT MOST once per frame).
- ESKF estimator: also has the hook (mid-flight tile gen is independent of state-estimator strategy).
- Configurable thresholds in `config.state.orthorectifier.{cov_norm_threshold, inlier_floor}`.
- Defensive: skip emission silently if quality gates fail (NOT a degraded-mode error; tile gen is opportunistic per AC-NEW-3).
- DEBUG log on every emission attempt; INFO log on first emission per flight.
- Unit tests: known pose + frame → expected orthorectified output; quality-gate skip behaviour; emission rate-limit (once per frame); `FreshnessRejectionError` swallowed silently.
## Scope
### Included
- `Orthorectifier` impl.
- Hook in `current_estimate` for both estimators.
- Quality-gate logic.
- Configurable thresholds.
- Unit tests.
### Excluded
- Any new C6 API — the existing `write_tile` + `TileSource.ONBOARD_INGEST` + `TileMetadata` covers everything (closed AZ-559 confirms).
- C6's downstream tile-cache eviction integration — owned by AZ-308.
- The orthorectification kernel optimisation — production-acceptable kernel uses NumPy or OpenCV `cv2.warpPerspective`; CUDA optimisation is a feature-cycle improvement.
## Acceptance Criteria
**AC-1: Orthorectification correctness** — synthetic camera pose + planar tile → output pixels match expected projection within 1-pixel tolerance.
**AC-2: Quality gate skip — covariance** — `cov_norm > threshold` → no tile written; DEBUG log only.
**AC-3: Source label gate** — `source_label != SATELLITE_ANCHORED` → no emission.
**AC-4: Once-per-frame rate limit** — even if `current_estimate` is called multiple times for the same frame, at most ONE tile is written.
**AC-5: Both estimators participate** — iSAM2 + ESKF both attempt candidate emission via the same `Orthorectifier` instance (or an equivalent per-estimator instance — implementer choice).
**AC-6: Composition wiring** — the orthorectifier is constructed inside the estimator at `__init__` time; `tile_store: TileStore` is constructor-injected.
**AC-7: First-emission INFO log** — `kind="c5.state.first_mid_flight_candidate"` with `{frame_id, tile_id, cov_norm}`.
**AC-8: Defensive skip on missing inputs** — if `frame` or `pose_estimate` is None, skip silently with DEBUG log (NOT an error).
**AC-9: Freshness rejection caught** — when `tile_store.write_tile` raises `FreshnessRejectionError`, the orthorectifier returns `None` and emits a DEBUG log; no exception propagates to `current_estimate`'s callers (replay protocol Invariant: opportunistic emission per AC-NEW-3).
## Non-Functional Requirements
- `try_emit_candidate` p95 ≤ 30 ms (orthorectification kernel cost, including JPEG encode).
- Memory ≤ 50 MB resident (frame buffer + working memory).
## Constraints
- Component-internal (not in C5 `__all__`).
- Once-per-frame rate limit.
- Quality gates are mandatory; AC-NEW-3 gain is contingent on emitted candidates being high-quality.
- The `TileQualityMetadata.covariance_2x2` field carries the **horizontal-position 2x2 sub-block** of the C5 pose covariance (not the full 6x6); the orthorectifier uses the full 6x6 for its OWN gate (`cov_norm < threshold`) but persists only the 2x2 sub-block in `TileQualityMetadata` per the existing schema.
- `inlier_count` is NOT a field on `TileQualityMetadata`; the orthorectifier uses it for the gate but persists `mre_px` (mean reprojection error) which serves the same downstream consumer (C6 voting status updater).
## Risks & Mitigation
- **Risk: Orthorectification produces low-quality tiles under degenerate pose** — quality gates filter; if still problematic, AZ-308 cache-eviction policy filters at storage time.
- **Risk: `cov_norm` (Frobenius norm of 6x6) vs `covariance_2x2` (horizontal sub-block) mismatch confuses readers** — *Mitigation*: docstring on the orthorectifier explicitly distinguishes the two uses; the gate operates on the 6x6 norm; the sub-block is only persisted for downstream voting-status readers.
## Runtime Completeness
- **Named capability**: orthorectifier → mid-flight tile candidate emission via `TileStore.write_tile`.
- **Production code**: real orthorectification kernel (NumPy or OpenCV), real quality gates, real `tile_store.write_tile` call against the production `PostgresFilesystemStore` in the airborne composition root.
- **Unacceptable substitutes**: emitting raw nav-frame pixels (not orthorectified); skipping the quality gates (AC-NEW-3 corruption); inventing a parallel `put_mid_flight_candidate` path when `write_tile` already exists.