mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 04:51:12 +00:00
[AZ-776] Open-loop ESKF composition profile via c4_pose.enabled
ADR-012: add c4_pose.enabled (default True) and enforce the (c4_pose.enabled, c5_state.strategy) 2x2 pairing matrix at compose time. When enabled=false, compose_root removes c4_pose from the selection map and build_pre_constructed omits c5_isam2_graph_handle. Replay protocol Invariant 13 owns the gate. Tier-2 conftest YAML writes the open-loop profile; un-xfails AC-1/2/5 and both AC-6 variants in Derkachi (AC-3 stays xfailed for AZ-777). 319/319 runtime_root + c4_pose + c5_state tests green. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -713,4 +713,41 @@ Two facts surfaced during the Step 7 (Implement) batch loop that contradicted th
|
||||
- AZ-405 grows slightly: it now also owns the `replay_input/` coordinator (the natural home for the auto-sync logic + the time-offset application).
|
||||
- AZ-404 (E2E replay test) is unchanged in scope but reworded: it asserts mode-agnosticism (Invariant 1) and runs against the unified airborne image — no fourth-image entrypoint to verify.
|
||||
- C8 gains a thin `MavlinkTransport` Protocol seam introduced by AZ-400: `SerialMavlinkTransport` (live) and `NoopMavlinkTransport` (replay) implement it. This is a no-op restructure of the existing C8 transport code; the encoders are unchanged. The Protocol seam is the architectural mechanism for Invariant 5 (encoders are byte-identical).
|
||||
- Demo↔field fidelity is now structurally guaranteed: the same binary runs in both contexts; any drift between them is a behavioural-test failure, not an SBOM-diff failure.
|
||||
- Demo↔field fidelity is now structurally guaranteed: the same binary runs in both contexts; any drift between them is a behavioural-test failure, not an SBOM-diff failure.
|
||||
|
||||
### ADR-012 — Open-loop ESKF composition profile via `c4_pose.enabled = false` (AZ-776)
|
||||
|
||||
**Context**: ADR-009 wires the C4 pose estimator and the C5 state estimator through a shared GTSAM iSAM2 substrate — C4 adds its PnP factor directly to C5's iSAM2 graph (ADR-003). The `c4_pose` slot in `runtime_root/airborne_bootstrap.py` lists `c5_isam2_graph_handle` as a required `pre_constructed` key (AZ-625), and the `OpenCVGtsamPoseEstimator` constructor consumes that handle. This wiring was sound for the steady-state GTSAM-iSAM2 build of C5.
|
||||
|
||||
When C5 ships a second strategy — `eskf` (ESKF baseline, AZ-588) — the substrate is **not** an iSAM2 graph: ESKF integrates an IMU-driven covariance forward closed-form, with no factor graph behind it. Its `create()` factory returns `(estimator, None)` for the second tuple element (the iSAM2 handle slot). Two facts surfaced from this:
|
||||
|
||||
1. **`c4_pose` cannot be the gate.** C4 owns satellite-anchored pose estimation. ESKF runs satellite-free open-loop. Forcing `c4_pose` into the composition when no satellite anchoring is wired means C4 either crashes at construction (no iSAM2 handle) or, worse, gets a fake handle that pretends to anchor poses that nothing produces — a silent passthrough that violates the "Real Results, Not Simulated Ones" meta-rule.
|
||||
2. **The replay Tier-2 smoke profile needs an honest minimum.** The AZ-265 replay path's mandatory simple baseline is KLT/RANSAC VIO + ESKF state estimator without any satellite re-anchoring (AZ-777 will add the satellite path on top via the Derkachi C6 reference tile cache). Without an explicit composition profile that excludes C4, every Tier-2 test that wants to exercise the simple baseline either crashes at compose time or has to monkey-patch the registry — both are anti-patterns for an architectural seam.
|
||||
|
||||
**Decision**:
|
||||
|
||||
1. **`C4PoseConfig.enabled: bool = True` is the user-facing switch for the open-loop ESKF profile.** Default ON preserves the ADR-003 steady-state airborne path. Setting `enabled=False` instructs `compose_root` to remove `c4_pose` from the selection map before topological ordering — the wrapper never runs, the consumer never sees a handle, and the wiring stays honest.
|
||||
2. **`compose_root` enforces the C4↔C5 pairing matrix at compose time.** The validation gate lives in `_validate_c4_c5_composition_profile` (called from `compose_root` before `_compose`) and rejects the two off-diagonal cells of the 2×2 (`c4_pose.enabled`, `c5_state.strategy`) matrix with a `CompositionError` naming both blocks. The two valid combinations are:
|
||||
- `c4_pose.enabled=True` + `c5_state.strategy="gtsam_isam2"` — the ADR-003 / ADR-009 steady-state airborne path.
|
||||
- `c4_pose.enabled=False` + `c5_state.strategy="eskf"` — the open-loop ESKF profile (Tier-2 smoke baseline; satellite anchoring deferred to AZ-777).
|
||||
The two **invalid** combinations are rejected with explicit error text:
|
||||
- `enabled=False` + `gtsam_isam2` (an iSAM2 graph with no PnP anchors converges to drift-prone visual-only odometry; the production deployment intent is that gtsam_isam2 always coexists with C4).
|
||||
- `enabled=True` + `eskf` (ESKF has no graph for C4 to anchor against; this is the AZ-776 root-cause pairing the user reported).
|
||||
3. **`build_pre_constructed` honours `c4_pose.enabled`.** When disabled, `c5_isam2_graph_handle` is **omitted** from the `pre_constructed` dict — the handle is a C4 consumer requirement, and removing C4 from the selection map removes the requirement. The ESKF estimator itself is still built and cached in the internal `_c5_prebuilt_estimator` slot (so the C5 wrapper short-circuits onto the prebuilt instance), but the iSAM2-shaped seam disappears from the cross-component contract.
|
||||
4. **Component selection is the only thing that changes.** The composition root's existing `_compose` mechanics — topological ordering, lazy strategy resolution, build-flag gating — are unchanged. The new `skip_slugs` parameter (a `frozenset[str]`) is the minimal seam that lets `compose_root` instruct `_compose` to drop the disabled component(s); there is no second composition path, no `compose_eskf` function, no mode-aware branch outside the validation gate.
|
||||
|
||||
**Alternatives considered**:
|
||||
|
||||
1. **Make `c4_pose` a "soft" dependency of C5 (introspect the strategy at C5 construction time, skip C4 wiring only when `strategy == "eskf"`).** Rejected: this leaks C5-strategy specifics into C4's interface (`PoseEstimator` would have to grow a "you may not be wired" affordance), violates ADR-009 interface-first, and re-introduces the very mode-aware branches Invariant 1 of the replay protocol forbids.
|
||||
2. **Make `compose_root` derive `c4_pose.enabled` automatically from `c5_state.strategy` (no user-facing flag).** Rejected: the C4↔C5 coupling is a deliberate design pairing, not a mechanical derivation. Future research strategies (e.g. a non-iSAM2 GTSAM variant, or a satellite-anchored ESKF) may want different combinations; the explicit flag keeps the configuration honest and audit-able.
|
||||
3. **Keep the wiring as-is and rely on the registry mechanism to skip C4.** Rejected: `C4PoseConfig` registers itself with the global config registry at module import (via `register_component_block` in `components/c4_pose/__init__.py`), which means even an empty `c4_pose:` block in YAML instantiates the block with defaults and pulls C4 into the selection map. The flag is the only honest opt-out without removing the registration call (which would break the steady-state path).
|
||||
4. **Build a synthetic `NullIsam2GraphHandle` that satisfies the Protocol but no-ops on update.** Rejected as the textbook example of the "Real Results, Not Simulated Ones" anti-pattern: it would let C4 run on top of ESKF with no anchoring, producing pose estimates that look real but have no factor-graph grounding. The composition-time gate is the honest answer.
|
||||
|
||||
**Consequences**:
|
||||
|
||||
- `tests/e2e/replay/conftest.py` writes `c4_pose: { enabled: false }` into the Tier-2 replay `config.yaml`, alongside the existing `c1_vio: klt_ransac` + `c5_state: eskf` block. This is the open-loop profile the replay binary uses for the AZ-265 / AZ-776 simple-baseline tests.
|
||||
- `tests/e2e/replay/test_derkachi_1min.py` un-xfails AC-1 (clean exit + per-frame JSONL), AC-2 (schema), AC-5 (determinism), AC-6 realtime, and AC-6 ASAP — these tests only required compose-time success to pass and AZ-776 lands that. AC-3 (≤ 100 m for ≥ 80 % of ticks) **remains** xfailed for AZ-777: ESKF integrates open-loop and drifts unbounded without C2/C3/C4 satellite re-anchoring; the ≤ 100 m threshold cannot be met by physics until the Derkachi C6 reference tile cache lands.
|
||||
- `_docs/02_document/contracts/replay/replay_protocol.md` gains a new "Open-loop ESKF composition profile" sub-section in **Composition root extension** plus a new **Invariant 13** ("C4↔C5 pairing matrix is enforced at compose time") that the AZ-776 unit tests own.
|
||||
- `_docs/02_document/components/06_c4_pose/description.md` gains an "Enabled flag" sub-section that points at this ADR; the rest of the component contract is unchanged.
|
||||
- The unit-test surface at `tests/unit/runtime_root/test_az776_open_loop_eskf_composition.py` owns the seven invariants AZ-776 introduces: `C4PoseConfig.enabled` default-true, AC-1 (open-loop ESKF composes without C4), AC-2 (default GTSAM profile still includes C4), AC-3a + AC-3b (the two forbidden pairings raise `CompositionError`), and the two `pre_constructed` behaviours (`c5_isam2_graph_handle` omitted when C4 disabled, present when C4 enabled). The full suite passes in ~4 s.
|
||||
- The composition root's contract surface in `runtime_root/__init__.py` gains one public helper (`CompositionError` was already public; the new `skip_slugs` parameter to `_compose` is module-private). No public CLI flag is added — operators set `c4_pose.enabled = false` in YAML.
|
||||
@@ -8,6 +8,8 @@
|
||||
|
||||
**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.
|
||||
|
||||
**Enabled flag (AZ-776 / ADR-012)**: `C4PoseConfig.enabled: bool = True` is the user-facing switch that controls C4's participation in the composition graph. Default ON preserves the ADR-003 steady-state airborne path. Setting `c4_pose.enabled = false` in YAML removes C4 from the component selection map at compose time — the wrapper never runs, the consumer never sees an iSAM2 handle, and `build_pre_constructed` omits `c5_isam2_graph_handle` from the `pre_constructed` dict. The flag exists to support the open-loop ESKF composition profile (the AZ-265 replay Tier-2 smoke baseline) where C5 runs as the `eskf` strategy with no factor graph for C4 to anchor against. `compose_root` enforces the 2×2 pairing matrix between `c4_pose.enabled` and `c5_state.strategy` at compose time and rejects the off-diagonal cells (`enabled=False` + `gtsam_isam2`, `enabled=True` + `eskf`) with a `CompositionError`. See ADR-012 (architecture.md) and Invariant 13 in `_docs/02_document/contracts/replay/replay_protocol.md`.
|
||||
|
||||
**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).
|
||||
|
||||
@@ -213,6 +213,33 @@ Side notes:
|
||||
- `set_takeoff_origin` (AZ-490 / ADR-010) is invoked identically in replay: the operator's pre-flight C10 Manifest is the source of truth in both modes. The tlog's first GPS fix is the **fallback**, gated through the same Principle #11 bounded-delta check.
|
||||
- `BUILD_FAISS_INDEX` is ON in the airborne binary (live and replay alike). C2 in replay queries the **real** C6 `FaissDescriptorIndex`, populated by the pre-flight C10 build. This is the architectural change vs. v1.0.0 of this contract.
|
||||
|
||||
### Composition profile: open-loop ESKF (AZ-776 / ADR-012)
|
||||
|
||||
The replay binary supports a second composition profile alongside the production GTSAM-iSAM2 path: **open-loop ESKF**. It is the Tier-2 smoke baseline for the AZ-265 replay flow and the only profile that can run end-to-end against the Derkachi clip today (the satellite-anchored profile waits on AZ-777's C6 reference tile cache).
|
||||
|
||||
The user-facing switch is one YAML field on the C4 block:
|
||||
|
||||
```
|
||||
c4_pose:
|
||||
enabled: false
|
||||
c5_state:
|
||||
strategy: eskf
|
||||
```
|
||||
|
||||
When `c4_pose.enabled = false`:
|
||||
- `compose_root` removes `c4_pose` from the component selection map before topological ordering; the C4 wrapper never runs, the `OpenCVGtsamPoseEstimator` is never instantiated, and `pose_estimator.estimate(refined)` in the per-frame loop above is a no-op (C5 receives IMU + VIO inputs only, no PnP anchor).
|
||||
- `build_pre_constructed` omits `c5_isam2_graph_handle` from the `pre_constructed` dict (no consumer requires it). The ESKF estimator is still pre-built and cached in the internal `_c5_prebuilt_estimator` slot exactly as in the gtsam_isam2 path; the C5 wrapper short-circuits onto the prebuilt instance per AZ-625.
|
||||
- The replay per-frame loop is otherwise unchanged — C1 VIO still runs, C5 still ingests, the JSONL sink still emits per video frame. Position drifts open-loop without satellite re-anchoring; AZ-777 closes that half of the loop by adding C2/C3/C4 against the C6 tile cache.
|
||||
|
||||
The 2×2 pairing matrix between `c4_pose.enabled` and `c5_state.strategy` is enforced at compose time (Invariant 13 below). The two valid cells are:
|
||||
|
||||
| `c4_pose.enabled` | `c5_state.strategy` | Profile | Status |
|
||||
|---|---|---|---|
|
||||
| `true` | `gtsam_isam2` | Steady-state airborne (ADR-003 / ADR-009) | Production |
|
||||
| `false` | `eskf` | Open-loop ESKF (this section) | Replay Tier-2 smoke baseline |
|
||||
|
||||
The two **invalid** cells (`true` + `eskf` and `false` + `gtsam_isam2`) raise `CompositionError` from `compose_root` with explicit error text naming both blocks. The Tier-2 replay fixture (`tests/e2e/replay/conftest.py`) writes the second valid cell. The first cell is the live + airborne default.
|
||||
|
||||
## Invariants
|
||||
|
||||
1. **Mode-agnostic C1–C7, C13**: production components MUST NOT contain `if config.mode == "replay":` branches. Mode-specific behaviour lives in the strategies (FrameSource / FcAdapter / MavlinkTransport / ReplaySink / Clock). Verified by an explicit grep guard in CI (the AZ-404 E2E test owns this assertion).
|
||||
@@ -227,6 +254,7 @@ Side notes:
|
||||
10. **Determinism**: same `(video, tlog, config, time_offset_ms, pace=ASAP)` input → same JSONL output within ≤ 1e-6 float drift in position fields (AC-5).
|
||||
11. **MAVLink signing key required in replay**: the airborne binary refuses to run without `--mavlink-signing-key PATH` in both modes. In replay the operator supplies a dummy file (well-formed key bytes; no real channel to verify against). This preserves Invariant 5 — the encoders' signing code path runs identically in both modes.
|
||||
12. **Real C6 cache in replay**: the airborne binary in replay mode reads the same pre-built C6 tile cache the operator built via the normal pre-flight C10/C11/C12 flow. There is no replay-specific cache shape. Verified by the AZ-404 E2E fixture, which runs the operator's pre-flight flow before invoking the replay CLI.
|
||||
13. **C4↔C5 pairing matrix is enforced at compose time** (AZ-776 / ADR-012): `compose_root` rejects the two off-diagonal cells of the (`c4_pose.enabled`, `c5_state.strategy`) matrix with a `CompositionError` naming both blocks. `enabled=False` + `gtsam_isam2` and `enabled=True` + `eskf` are forbidden. The two valid cells are `enabled=True` + `gtsam_isam2` (production steady-state per ADR-003 / ADR-009) and `enabled=False` + `eskf` (open-loop ESKF — replay Tier-2 smoke baseline; satellite anchoring deferred to AZ-777). Verified by `tests/unit/runtime_root/test_az776_open_loop_eskf_composition.py` AC-3a and AC-3b.
|
||||
|
||||
## Producer / Consumer Split
|
||||
|
||||
|
||||
@@ -6,9 +6,9 @@ step: 10
|
||||
name: Implement
|
||||
status: in_progress
|
||||
sub_step:
|
||||
phase: 0
|
||||
name: awaiting-invocation
|
||||
detail: "cycle 3 implement: AZ-776 first (no deps), then AZ-777 (depends on AZ-776)"
|
||||
phase: 7
|
||||
name: batch-loop
|
||||
detail: "batch 103 cycle3: AZ-776 implemented, awaiting commit + transition; AZ-777 next"
|
||||
retry_count: 0
|
||||
cycle: 3
|
||||
tracker: jira
|
||||
|
||||
@@ -27,6 +27,16 @@ class C4PoseConfig:
|
||||
|
||||
Fields per the C4 contract §"Config-load-time validation":
|
||||
|
||||
* ``enabled`` — AZ-776 composition-graph flag. When ``False`` the
|
||||
composition root strips ``c4_pose`` from the airborne component
|
||||
graph entirely (open-loop ESKF profile per ADR-012). Default
|
||||
``True`` preserves the full GTSAM pipeline that ADR-003 mandates
|
||||
as the steady-state airborne path. Forbidden pairings (rejected
|
||||
by ``compose_root`` with :class:`CompositionError`):
|
||||
``enabled=False`` + ``c5_state.strategy="gtsam_isam2"`` (iSAM2
|
||||
requires a C4 anchor); ``enabled=True`` +
|
||||
``c5_state.strategy="eskf"`` (ESKF has no iSAM2 graph for C4 to
|
||||
anchor against).
|
||||
* ``strategy`` — selects the concrete estimator. Currently only
|
||||
``"opencv_gtsam"`` is defined.
|
||||
* ``ransac_iterations`` — OpenCV ``solvePnPRansac`` iteration
|
||||
@@ -56,6 +66,7 @@ class C4PoseConfig:
|
||||
handling would land in a future config extension).
|
||||
"""
|
||||
|
||||
enabled: bool = True
|
||||
strategy: str = "opencv_gtsam"
|
||||
ransac_iterations: int = 200
|
||||
ransac_reprojection_threshold_px: float = 4.0
|
||||
|
||||
@@ -336,6 +336,7 @@ def _compose(
|
||||
allowed_tiers: frozenset[StrategyTier],
|
||||
extra_required_env: Iterable[str],
|
||||
pre_constructed: Mapping[str, Any] | None = None,
|
||||
skip_slugs: frozenset[str] = frozenset(),
|
||||
) -> tuple[dict[str, Any], tuple[str, ...]]:
|
||||
"""Shared composition path used by ``compose_root`` / ``compose_operator``.
|
||||
|
||||
@@ -345,9 +346,17 @@ def _compose(
|
||||
strategies (``frame_source``, ``fc_adapter``, ``clock``,
|
||||
``mavlink_transport``, ``replay_sink``) so any C1-C7 factory that
|
||||
declares a dependency on one finds it already populated.
|
||||
|
||||
``skip_slugs`` removes component slugs from the composition graph
|
||||
even when ``config.components`` contains a block for them. AZ-776
|
||||
uses this to drop ``c4_pose`` from the airborne open-loop ESKF
|
||||
profile; the slug is filtered out of the topological walk and the
|
||||
corresponding wrapper factory is never invoked.
|
||||
"""
|
||||
_check_required_env(extra_required=extra_required_env)
|
||||
selections = _resolve_component_strategies(config, allowed_tiers)
|
||||
for skipped in skip_slugs:
|
||||
selections.pop(skipped, None)
|
||||
resolved: dict[str, _Registration] = {
|
||||
slug: _resolve_strategy(slug, strategy, allowed_tiers)
|
||||
for slug, strategy in selections.items()
|
||||
@@ -423,6 +432,92 @@ def _read_strategy_attr(block: Any) -> Any:
|
||||
return None
|
||||
|
||||
|
||||
def _read_c4_pose_enabled(config: Config) -> bool:
|
||||
"""True iff the airborne ``c4_pose`` slot is enabled in the composition graph.
|
||||
|
||||
AZ-776: ``c4_pose.enabled = False`` selects the open-loop ESKF
|
||||
composition profile (ADR-012). Defaults to ``True`` when the
|
||||
block is absent or carries no ``enabled`` attribute, preserving
|
||||
the legacy GTSAM pipeline (ADR-003 steady-state path).
|
||||
"""
|
||||
components = getattr(config, "components", None) or {}
|
||||
if not isinstance(components, Mapping):
|
||||
return True
|
||||
block = components.get("c4_pose")
|
||||
if block is None:
|
||||
return True
|
||||
enabled = getattr(block, "enabled", True)
|
||||
return bool(enabled)
|
||||
|
||||
|
||||
def _read_c5_state_strategy(config: Config) -> str:
|
||||
"""Return ``config.components['c5_state'].strategy`` or the default.
|
||||
|
||||
Defaults to ``"gtsam_isam2"`` when the block is absent — the same
|
||||
fallback :func:`airborne_bootstrap._resolve_c5_state_strategy`
|
||||
uses for the eager :func:`build_state_estimator` invocation.
|
||||
"""
|
||||
components = getattr(config, "components", None) or {}
|
||||
if not isinstance(components, Mapping):
|
||||
return "gtsam_isam2"
|
||||
block = components.get("c5_state")
|
||||
if block is None:
|
||||
return "gtsam_isam2"
|
||||
strategy = getattr(block, "strategy", "gtsam_isam2")
|
||||
return str(strategy) if strategy else "gtsam_isam2"
|
||||
|
||||
|
||||
def _validate_c4_c5_composition_profile(config: Config) -> None:
|
||||
"""Reject incompatible ``c4_pose.enabled`` / ``c5_state.strategy`` pairings.
|
||||
|
||||
AZ-776 / ADR-012 defines two valid composition profiles for the
|
||||
airborne binary:
|
||||
|
||||
* **Full GTSAM** (steady-state, ADR-003): ``c4_pose.enabled=True``
|
||||
+ ``c5_state.strategy="gtsam_isam2"``. C4 anchors land into C5's
|
||||
iSAM2 graph; C5 produces smoothed estimates.
|
||||
* **Open-loop ESKF** (mandatory simple baseline, ADR-012):
|
||||
``c4_pose.enabled=False`` + ``c5_state.strategy="eskf"``. C4 is
|
||||
stripped from the graph; C5 ESKF integrates VIO + IMU
|
||||
open-loop. No iSAM2 graph handle is produced or consumed.
|
||||
|
||||
The two off-diagonal pairings are physically impossible:
|
||||
|
||||
* ``c4_pose.enabled=False`` + ``c5_state.strategy="gtsam_isam2"``:
|
||||
iSAM2 requires a C4 pose anchor to bound drift; without C4 the
|
||||
graph never receives a pose factor and the smoother is unbounded.
|
||||
* ``c4_pose.enabled=True`` + ``c5_state.strategy="eskf"``: C4
|
||||
needs the iSAM2 graph handle to inject pose factors, but ESKF
|
||||
returns ``handle=None`` by design (no graph). The C4 wrapper
|
||||
would crash on the ``None`` handle.
|
||||
|
||||
Either off-diagonal raises :class:`CompositionError` with a clear
|
||||
next-step naming both fields.
|
||||
"""
|
||||
c4_enabled = _read_c4_pose_enabled(config)
|
||||
c5_strategy = _read_c5_state_strategy(config)
|
||||
if not c4_enabled and c5_strategy == "gtsam_isam2":
|
||||
raise CompositionError(
|
||||
"compose_root: c4_pose.enabled=False is incompatible with "
|
||||
"c5_state.strategy='gtsam_isam2' — iSAM2 requires C4 pose "
|
||||
"anchors to bound smoother drift. Use either the full "
|
||||
"GTSAM profile (c4_pose.enabled=true, c5_state.strategy="
|
||||
"'gtsam_isam2') or the open-loop ESKF profile "
|
||||
"(c4_pose.enabled=false, c5_state.strategy='eskf'). See "
|
||||
"ADR-012 (open-loop ESKF composition profile)."
|
||||
)
|
||||
if c4_enabled and c5_strategy == "eskf":
|
||||
raise CompositionError(
|
||||
"compose_root: c4_pose.enabled=True is incompatible with "
|
||||
"c5_state.strategy='eskf' — ESKF has no iSAM2 graph for "
|
||||
"C4 to anchor against (handle=None by design), so the C4 "
|
||||
"wrapper cannot run. Set c4_pose.enabled=false to select "
|
||||
"the open-loop ESKF profile, or change c5_state.strategy "
|
||||
"to 'gtsam_isam2' for the full GTSAM pipeline. See "
|
||||
"ADR-012 (open-loop ESKF composition profile)."
|
||||
)
|
||||
|
||||
|
||||
def compose_root(
|
||||
config: Config,
|
||||
*,
|
||||
@@ -445,6 +540,14 @@ def compose_root(
|
||||
finds it already populated. C1-C7+C13 strategies are wired
|
||||
identically to live mode (replay protocol Invariant 1).
|
||||
|
||||
AZ-776 composition profile (ADR-012): when
|
||||
``config.components['c4_pose'].enabled`` is ``False`` the function
|
||||
strips ``c4_pose`` from the airborne component graph (open-loop
|
||||
ESKF profile). The forbidden off-diagonal pairings
|
||||
(``enabled=False`` + ``c5_state.strategy='gtsam_isam2'``;
|
||||
``enabled=True`` + ``c5_state.strategy='eskf'``) raise
|
||||
:class:`CompositionError` before any factory runs.
|
||||
|
||||
The ``pre_constructed`` kwarg (AZ-591) lets the caller seed
|
||||
``constructed`` with infrastructure objects (e.g. fdr_client,
|
||||
descriptor_index, inference_runtime) before any registered factory
|
||||
@@ -461,6 +564,8 @@ def compose_root(
|
||||
have to satisfy the full OpenCV / pymavlink / FDR side-effects of
|
||||
the real strategies.
|
||||
"""
|
||||
_validate_c4_c5_composition_profile(config)
|
||||
skip_slugs = frozenset() if _read_c4_pose_enabled(config) else frozenset({"c4_pose"})
|
||||
extra_env = ("MAVLINK_SIGNING_KEY",) if config.mode == "live" else ()
|
||||
if config.mode == "replay":
|
||||
replay_factory = replay_components_factory or build_replay_components
|
||||
@@ -478,6 +583,7 @@ def compose_root(
|
||||
allowed_tiers=frozenset({"airborne", "shared"}),
|
||||
extra_required_env=extra_env,
|
||||
pre_constructed=seeded,
|
||||
skip_slugs=skip_slugs,
|
||||
)
|
||||
merged: dict[str, Any] = dict(replay_components)
|
||||
merged.update(components)
|
||||
|
||||
@@ -1135,6 +1135,33 @@ def _build_c3_feature_extractor(config: Config) -> FeatureExtractor:
|
||||
return OpenCvOrbExtractor()
|
||||
|
||||
|
||||
def _c4_pose_disabled(config: Config) -> bool:
|
||||
"""True iff the airborne open-loop ESKF composition profile is active.
|
||||
|
||||
AZ-776 / ADR-012: when ``config.components["c4_pose"].enabled`` is
|
||||
explicitly ``False`` the composition root strips ``c4_pose`` from
|
||||
the component graph and the C4 wrapper never runs. The eager
|
||||
``(estimator, handle)`` build inside :func:`build_pre_constructed`
|
||||
therefore has no consumer for the iSAM2 graph handle slot
|
||||
(``c5_isam2_graph_handle``); leaving the slot absent makes the
|
||||
"no C4" property visible at the pre-bootstrap layer too.
|
||||
|
||||
Returns ``False`` when the block is absent or when ``enabled`` is
|
||||
missing / true — the full GTSAM profile (ADR-003 steady state)
|
||||
remains the default airborne pipeline. Replay-mode component-block
|
||||
omission is handled separately by
|
||||
:func:`_replay_omits_component_block`.
|
||||
"""
|
||||
components = getattr(config, "components", None) or {}
|
||||
if not isinstance(components, Mapping):
|
||||
return False
|
||||
block = components.get("c4_pose")
|
||||
if block is None:
|
||||
return False
|
||||
enabled = getattr(block, "enabled", True)
|
||||
return not bool(enabled)
|
||||
|
||||
|
||||
def _replay_omits_component_block(config: Config, block_name: str) -> bool:
|
||||
"""True iff replay-mode :class:`Config` has no ``components[block_name]`` entry.
|
||||
|
||||
@@ -1301,7 +1328,14 @@ def build_pre_constructed(config: Config) -> dict[str, Any]:
|
||||
fdr_client=constructed["c13_fdr"],
|
||||
tile_store=constructed.get("c6_tile_store"),
|
||||
)
|
||||
constructed["c5_isam2_graph_handle"] = handle
|
||||
# AZ-776 / ADR-012 open-loop ESKF profile: c4_pose.enabled=False
|
||||
# strips c4_pose from the composition graph, so the
|
||||
# c5_isam2_graph_handle slot has no consumer. ESKF returns
|
||||
# handle=None by design; omitting the slot makes the open-loop
|
||||
# property visible at the pre_constructed layer and refuses the
|
||||
# invariant "handle is None means C4 will crash on read".
|
||||
if not _c4_pose_disabled(config):
|
||||
constructed["c5_isam2_graph_handle"] = handle
|
||||
constructed[_C5_PREBUILT_ESTIMATOR_KEY] = estimator
|
||||
return constructed
|
||||
|
||||
|
||||
@@ -136,24 +136,28 @@ def derkachi_replay_inputs(tmp_path_factory: pytest.TempPathFactory) -> Derkachi
|
||||
# treats each top-level mapping as a block whose key is a
|
||||
# registry slug; nesting the slugs under a `components:`
|
||||
# wrapper makes the loader silently drop them (the wrapper
|
||||
# is not a registered slug). See `_docs/_repo` notes on the
|
||||
# ESKF compose-time blocker (AZ-776) for why this matters.
|
||||
# is not a registered slug).
|
||||
#
|
||||
# KLT/RANSAC + ESKF is the minimal pair that runs without
|
||||
# native deps (cv2 + numpy only). The CLI currently exits
|
||||
# non-zero at compose time for this configuration: c4_pose
|
||||
# hard-requires an iSAM2 graph handle that ESKF does not
|
||||
# provide (handle=None by design). AZ-776 tracks the fix.
|
||||
# Until AZ-776 lands, every heavy AC test in
|
||||
# `test_derkachi_1min.py` is xfailed with that ticket in
|
||||
# the reason. C2/C3/C4 satellite anchoring additionally
|
||||
# require AZ-777 (Derkachi C6 reference tile cache).
|
||||
# Open-loop ESKF composition profile (AZ-776 / ADR-012):
|
||||
# `c4_pose.enabled = false` strips C4 from the composition
|
||||
# graph so the airborne binary can run the mandatory simple
|
||||
# baseline (KLT/RANSAC VIO + ESKF state estimator) end-to-end
|
||||
# without a C4 anchor. ESKF has no iSAM2 graph for C4 to
|
||||
# anchor against; the `compose_root` validation gate rejects
|
||||
# the off-diagonal pairings (`enabled=False` + `gtsam_isam2`
|
||||
# or `enabled=True` + `eskf`) with a `CompositionError`.
|
||||
# Position drifts open-loop without C2/C3/C4 satellite
|
||||
# re-anchoring — AZ-777 (Derkachi C6 reference tile cache)
|
||||
# is the follow-up that closes the satellite-anchoring half
|
||||
# of the per-frame loop.
|
||||
"mode: replay\n"
|
||||
"replay:\n"
|
||||
" pace: asap\n"
|
||||
" target_fc_dialect: ardupilot_plane\n"
|
||||
"c1_vio:\n"
|
||||
" strategy: klt_ransac\n"
|
||||
"c4_pose:\n"
|
||||
" enabled: false\n"
|
||||
"c5_state:\n"
|
||||
" strategy: eskf\n"
|
||||
)
|
||||
|
||||
@@ -58,22 +58,6 @@ _HEAVY_SKIP = pytest.mark.skipif(
|
||||
|
||||
@pytest.mark.tier2
|
||||
@_HEAVY_SKIP
|
||||
@pytest.mark.xfail(
|
||||
reason=(
|
||||
"Blocked by AZ-776: the replay compose root cannot wire "
|
||||
"c5_state=eskf because c4_pose hard-requires an iSAM2 graph "
|
||||
"handle that ESKF does not provide (handle=None by design). "
|
||||
"The CLI exits non-zero at compose time before the per-frame "
|
||||
"loop runs, so this test cannot pass against the current "
|
||||
"runtime. Once AZ-776 ships, an open-loop C1+C5(ESKF) "
|
||||
"composition will allow the CLI to exit 0 and this AC-1 "
|
||||
"test (emit one EstimatorOutput per video frame) can pass. "
|
||||
"Full-pipeline accuracy still requires AZ-777 (Derkachi C6 "
|
||||
"reference tile cache) but AC-1 only needs successful exit, "
|
||||
"not anchor-quality, so AZ-776 alone is sufficient."
|
||||
),
|
||||
strict=False,
|
||||
)
|
||||
def test_ac1_exits_0_jsonl_count_match(replay_runner, derkachi_replay_inputs) -> None:
|
||||
"""Real loop emits one EstimatorOutput per video frame, not per GPS fix.
|
||||
|
||||
@@ -135,17 +119,6 @@ _ESTIMATOR_OUTPUT_KEYS = frozenset(
|
||||
|
||||
@pytest.mark.tier2
|
||||
@_HEAVY_SKIP
|
||||
@pytest.mark.xfail(
|
||||
reason=(
|
||||
"Blocked by AZ-776 (replay compose root cannot use "
|
||||
"c5_state=eskf). The CLI exits non-zero before any JSONL "
|
||||
"rows are written, so the schema cannot be validated against "
|
||||
"the current runtime. Schema lives in EstimatorOutput and is "
|
||||
"stable; AC-2 can pass as soon as AZ-776 makes the loop "
|
||||
"actually emit rows."
|
||||
),
|
||||
strict=False,
|
||||
)
|
||||
def test_ac2_jsonl_schema_match(replay_runner) -> None:
|
||||
# Act
|
||||
result = replay_runner(pace="asap")
|
||||
@@ -174,18 +147,13 @@ def test_ac2_jsonl_schema_match(replay_runner) -> None:
|
||||
@pytest.mark.xfail(
|
||||
reason=(
|
||||
"AC-3 requires the C1+C2+C3+C4+C5 satellite-re-anchoring "
|
||||
"pipeline. Two blockers, both tracked: "
|
||||
"(1) AZ-776 — the replay compose root cannot currently wire "
|
||||
"c5_state=eskf at all (c4_pose hard-requires an iSAM2 "
|
||||
"handle ESKF does not provide); the CLI exits non-zero "
|
||||
"before any tick is emitted. "
|
||||
"(2) AZ-777 — once AZ-776 lands, the open-loop C1+C5(ESKF) "
|
||||
"composition will run end-to-end but with NO satellite "
|
||||
"anchoring (no C2/C3/C4) because the Derkachi fixture has "
|
||||
"no reference C6 tile cache. ESKF integrates open-loop, so "
|
||||
"position drifts unbounded over the 8-min flight and the "
|
||||
"≤100m threshold cannot be met by physics. "
|
||||
"AC-3 stays xfail until BOTH AZ-776 and AZ-777 ship."
|
||||
"pipeline. Blocked by AZ-777: with AZ-776 landed, the "
|
||||
"open-loop C1+C5(ESKF) composition now runs end-to-end but "
|
||||
"with NO satellite anchoring (no C2/C3/C4) because the "
|
||||
"Derkachi fixture has no reference C6 tile cache. ESKF "
|
||||
"integrates open-loop, so position drifts unbounded over "
|
||||
"the 8-min flight and the ≤100 m threshold cannot be met "
|
||||
"by physics until the reference tile cache (AZ-777) lands."
|
||||
),
|
||||
strict=False,
|
||||
)
|
||||
@@ -410,17 +378,6 @@ def test_ac4_encoder_byte_equality_via_transport_seam() -> None:
|
||||
|
||||
@pytest.mark.tier2
|
||||
@_HEAVY_SKIP
|
||||
@pytest.mark.xfail(
|
||||
reason=(
|
||||
"Blocked by AZ-776: with the compose root failing for "
|
||||
"c5_state=eskf the CLI exits non-zero on both runs, so "
|
||||
"determinism cannot be observed. Once AZ-776 ships, the "
|
||||
"open-loop C1+C5 path is deterministic by construction "
|
||||
"(KLT/RANSAC uses fixed seeds, ESKF is closed-form) and "
|
||||
"AC-5 should pass."
|
||||
),
|
||||
strict=False,
|
||||
)
|
||||
def test_ac5_determinism_two_runs_diff(replay_runner) -> None:
|
||||
# Act
|
||||
r1 = replay_runner(pace="asap")
|
||||
@@ -450,14 +407,6 @@ def test_ac5_determinism_two_runs_diff(replay_runner) -> None:
|
||||
|
||||
@pytest.mark.tier2
|
||||
@_HEAVY_SKIP
|
||||
@pytest.mark.xfail(
|
||||
reason=(
|
||||
"Blocked by AZ-776: the CLI exits non-zero at compose time, "
|
||||
"so the realtime pacing loop is never reached. Once AZ-776 "
|
||||
"ships, AC-6 realtime can pace the open-loop C1+C5 path."
|
||||
),
|
||||
strict=False,
|
||||
)
|
||||
def test_ac6_pace_realtime_60s_within_5pct(replay_runner) -> None:
|
||||
# Act — cap to 60 s so a full 490-second flight doesn't pin the test
|
||||
# to an 8-minute realtime run; the pacing correctness is validated
|
||||
@@ -476,15 +425,6 @@ def test_ac6_pace_realtime_60s_within_5pct(replay_runner) -> None:
|
||||
|
||||
@pytest.mark.tier2
|
||||
@_HEAVY_SKIP
|
||||
@pytest.mark.xfail(
|
||||
reason=(
|
||||
"Blocked by AZ-776: the CLI exits non-zero at compose time, "
|
||||
"so the ASAP pacing loop is never reached. Once AZ-776 "
|
||||
"ships, AC-6 ASAP can run the open-loop C1+C5 path "
|
||||
"to completion."
|
||||
),
|
||||
strict=False,
|
||||
)
|
||||
def test_ac6_pace_asap_under_30s(replay_runner) -> None:
|
||||
# Act
|
||||
result = replay_runner(pace="asap")
|
||||
|
||||
@@ -0,0 +1,442 @@
|
||||
"""AZ-776 / ADR-012 — Open-loop ESKF composition profile (`c4_pose.enabled`).
|
||||
|
||||
Verifies the contract at
|
||||
``_docs/02_tasks/todo/AZ-776_eskf_open_loop_composition_profile.md``:
|
||||
|
||||
* AC-1 — open-loop profile composes: with
|
||||
``c4_pose.enabled = False`` + ``c5_state.strategy = "eskf"``,
|
||||
:func:`compose_root` produces a runtime whose components dict
|
||||
contains ``c1_vio`` and ``c5_state`` but NOT ``c4_pose``, and the
|
||||
topological walk completes without
|
||||
:class:`CompositionError` / :class:`StrategyNotLinkedError` /
|
||||
:class:`AirborneBootstrapError`.
|
||||
|
||||
* AC-2 — full GTSAM profile unchanged: with default
|
||||
``c4_pose.enabled = True`` + ``c5_state.strategy = "gtsam_isam2"``,
|
||||
:func:`compose_root` still composes ``c4_pose`` exactly as before
|
||||
AZ-776 (no behaviour change for the steady-state airborne path
|
||||
documented by ADR-003).
|
||||
|
||||
* AC-3a — forbidden pairing ``c4_pose.enabled=False`` +
|
||||
``c5_state.strategy="gtsam_isam2"`` raises :class:`CompositionError`
|
||||
naming both fields and ADR-012.
|
||||
|
||||
* AC-3b — forbidden pairing ``c4_pose.enabled=True`` +
|
||||
``c5_state.strategy="eskf"`` raises :class:`CompositionError`
|
||||
naming both fields and ADR-012.
|
||||
|
||||
* AC-Config — :class:`C4PoseConfig.enabled` defaults to ``True``
|
||||
(preserves ADR-003 steady-state path when the operator does not
|
||||
set the flag explicitly).
|
||||
|
||||
* AC-Bootstrap — :func:`build_pre_constructed` omits the
|
||||
``c5_isam2_graph_handle`` slot from the returned dict when
|
||||
``c4_pose.enabled = False`` (the slot has no consumer in the
|
||||
open-loop profile).
|
||||
|
||||
The composition tests stub every per-component wrapper at the
|
||||
:mod:`gps_denied_onboard.runtime_root.airborne_bootstrap` module
|
||||
boundary so the unit suite does not pull in gtsam / opencv /
|
||||
lightglue / FAISS / TensorRT — the same isolation pattern AZ-591
|
||||
established in :mod:`tests.unit.runtime_root.test_az591_airborne_bootstrap`.
|
||||
The :func:`build_pre_constructed` test also stubs
|
||||
:func:`_build_c5_state_estimator_pair` to avoid registering / loading
|
||||
either gtsam or the ESKF NumPy estimator at unit-test time.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import dataclasses
|
||||
from collections.abc import Iterator
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard.components.c4_pose.config import C4PoseConfig
|
||||
from gps_denied_onboard.config import Config
|
||||
from gps_denied_onboard.fdr_client.client import _reset_for_tests as _reset_fdr_client_cache
|
||||
from gps_denied_onboard.runtime_root import (
|
||||
CompositionError,
|
||||
clear_strategy_registry,
|
||||
compose_root,
|
||||
)
|
||||
from gps_denied_onboard.runtime_root import airborne_bootstrap
|
||||
from gps_denied_onboard.runtime_root.airborne_bootstrap import (
|
||||
build_pre_constructed,
|
||||
clear_imu_preintegrator_cache,
|
||||
register_airborne_strategies,
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Fixtures + helpers
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _C1Block:
|
||||
strategy: str = "klt_ransac"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _C4Block:
|
||||
enabled: bool = True
|
||||
strategy: str = "opencv_gtsam"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _C5Block:
|
||||
strategy: str = "gtsam_isam2"
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _isolated_registry() -> Iterator[None]:
|
||||
clear_strategy_registry()
|
||||
clear_imu_preintegrator_cache()
|
||||
_reset_fdr_client_cache()
|
||||
yield
|
||||
clear_strategy_registry()
|
||||
clear_imu_preintegrator_cache()
|
||||
_reset_fdr_client_cache()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def _airborne_env(monkeypatch: pytest.MonkeyPatch) -> None:
|
||||
for name, value in (
|
||||
("GPS_DENIED_FC_PROFILE", "ardupilot_plane"),
|
||||
("GPS_DENIED_TIER", "1"),
|
||||
("DB_URL", "postgresql+psycopg://gps_denied:dev@db:5432/gps_denied"),
|
||||
("CAMERA_CALIBRATION_PATH", "/etc/gps-denied/calib.yml"),
|
||||
("LOG_LEVEL", "INFO"),
|
||||
("LOG_SINK", "console"),
|
||||
("INFERENCE_BACKEND", "pytorch_fp16"),
|
||||
("FDR_PATH", "/var/lib/gps-denied/fdr"),
|
||||
("TILE_CACHE_PATH", "/var/lib/gps-denied/tiles"),
|
||||
("MAVLINK_SIGNING_KEY", "ZZZZZZZZ"),
|
||||
):
|
||||
monkeypatch.setenv(name, value)
|
||||
|
||||
|
||||
def _stub_wrappers(monkeypatch: pytest.MonkeyPatch) -> dict[str, list[str]]:
|
||||
"""Stub every airborne wrapper to return a sentinel + record invocations.
|
||||
|
||||
Mirrors :func:`tests.unit.runtime_root.test_az591_airborne_bootstrap.\
|
||||
test_ac2_compose_root_reaches_completion_with_pre_constructed_infra`
|
||||
so the AZ-776 tests do not pull in gtsam / opencv / lightglue.
|
||||
|
||||
Returns:
|
||||
``invocations``: a dict mapping component slug → list of
|
||||
invocation order (read after :func:`compose_root` returns).
|
||||
"""
|
||||
invocations: dict[str, list[str]] = {"order": []}
|
||||
|
||||
def _make_stub(slug: str) -> Any:
|
||||
def _stub(config: Any, constructed: dict[str, Any]) -> str:
|
||||
del config, constructed
|
||||
invocations["order"].append(slug)
|
||||
return f"<{slug}>"
|
||||
|
||||
return _stub
|
||||
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c1_vio_wrapper", _make_stub("c1_vio"))
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c2_vpr_wrapper", _make_stub("c2_vpr"))
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c2_5_rerank_wrapper", _make_stub("c2_5_rerank"))
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c3_matcher_wrapper", _make_stub("c3_matcher"))
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c3_5_adhop_wrapper", _make_stub("c3_5_adhop"))
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c4_pose_wrapper", _make_stub("c4_pose"))
|
||||
monkeypatch.setattr(airborne_bootstrap, "_c5_state_wrapper", _make_stub("c5_state"))
|
||||
monkeypatch.setattr(
|
||||
airborne_bootstrap,
|
||||
"_AIRBORNE_REGISTRATIONS",
|
||||
(
|
||||
("c1_vio", airborne_bootstrap._C1_VIO_STRATEGIES, airborne_bootstrap._c1_vio_wrapper, ()),
|
||||
("c2_vpr", airborne_bootstrap._C2_VPR_STRATEGIES, airborne_bootstrap._c2_vpr_wrapper, ()),
|
||||
(
|
||||
"c2_5_rerank",
|
||||
airborne_bootstrap._C2_5_RERANK_STRATEGIES,
|
||||
airborne_bootstrap._c2_5_rerank_wrapper,
|
||||
("c2_vpr",),
|
||||
),
|
||||
(
|
||||
"c3_matcher",
|
||||
airborne_bootstrap._C3_MATCHER_STRATEGIES,
|
||||
airborne_bootstrap._c3_matcher_wrapper,
|
||||
(),
|
||||
),
|
||||
(
|
||||
"c3_5_adhop",
|
||||
airborne_bootstrap._C3_5_ADHOP_STRATEGIES,
|
||||
airborne_bootstrap._c3_5_adhop_wrapper,
|
||||
("c3_matcher",),
|
||||
),
|
||||
(
|
||||
"c4_pose",
|
||||
airborne_bootstrap._C4_POSE_STRATEGIES,
|
||||
airborne_bootstrap._c4_pose_wrapper,
|
||||
("c1_vio", "c3_matcher"),
|
||||
),
|
||||
(
|
||||
"c5_state",
|
||||
airborne_bootstrap._C5_STATE_STRATEGIES,
|
||||
airborne_bootstrap._c5_state_wrapper,
|
||||
("c1_vio", "c4_pose"),
|
||||
),
|
||||
),
|
||||
)
|
||||
return invocations
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-Config: C4PoseConfig.enabled defaults to True
|
||||
|
||||
|
||||
def test_c4_pose_config_enabled_defaults_to_true() -> None:
|
||||
# Act
|
||||
block = C4PoseConfig()
|
||||
|
||||
# Assert
|
||||
assert block.enabled is True, (
|
||||
"ADR-003 steady-state airborne path requires c4_pose.enabled "
|
||||
"to default ON; ADR-012 opt-in is via explicit enabled=False"
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-1: open-loop ESKF profile composes without c4_pose
|
||||
|
||||
|
||||
def test_ac1_open_loop_eskf_composes_without_c4_pose(
|
||||
_airborne_env: None,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
# Arrange
|
||||
invocations = _stub_wrappers(monkeypatch)
|
||||
register_airborne_strategies()
|
||||
config = Config.with_blocks(
|
||||
c1_vio=_C1Block(),
|
||||
c4_pose=_C4Block(enabled=False, strategy="opencv_gtsam"),
|
||||
c5_state=_C5Block(strategy="eskf"),
|
||||
)
|
||||
|
||||
# Act
|
||||
root = compose_root(config, pre_constructed={})
|
||||
|
||||
# Assert
|
||||
assert "c1_vio" in root.components
|
||||
assert "c5_state" in root.components
|
||||
assert "c4_pose" not in root.components, (
|
||||
"AZ-776 AC-1: c4_pose.enabled=False MUST exclude c4_pose from "
|
||||
"the composition graph; got components="
|
||||
f"{sorted(root.components.keys())}"
|
||||
)
|
||||
assert "c4_pose" not in invocations["order"], (
|
||||
"AZ-776 AC-1: the c4_pose wrapper MUST NOT be invoked when "
|
||||
"the open-loop ESKF profile is active"
|
||||
)
|
||||
order = invocations["order"]
|
||||
assert order.index("c1_vio") < order.index("c5_state"), (
|
||||
"c1_vio must construct before c5_state — VIO output feeds the "
|
||||
"state estimator"
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-2: full GTSAM profile unchanged (default enabled=True + gtsam_isam2)
|
||||
|
||||
|
||||
def test_ac2_full_gtsam_profile_includes_c4_pose(
|
||||
_airborne_env: None,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
# Arrange
|
||||
invocations = _stub_wrappers(monkeypatch)
|
||||
register_airborne_strategies()
|
||||
config = Config.with_blocks(
|
||||
c1_vio=_C1Block(),
|
||||
c4_pose=_C4Block(enabled=True, strategy="opencv_gtsam"),
|
||||
c5_state=_C5Block(strategy="gtsam_isam2"),
|
||||
)
|
||||
|
||||
# Act
|
||||
root = compose_root(config, pre_constructed={})
|
||||
|
||||
# Assert
|
||||
assert "c4_pose" in root.components, (
|
||||
"ADR-003 steady-state path: c4_pose.enabled=True MUST keep "
|
||||
"c4_pose in the composition graph"
|
||||
)
|
||||
assert "c1_vio" in root.components
|
||||
assert "c5_state" in root.components
|
||||
assert "c4_pose" in invocations["order"]
|
||||
order = invocations["order"]
|
||||
assert order.index("c1_vio") < order.index("c4_pose")
|
||||
assert order.index("c4_pose") < order.index("c5_state")
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-3a: forbidden — c4_pose.enabled=False + c5_state.strategy=gtsam_isam2
|
||||
|
||||
|
||||
def test_ac3a_disabled_c4_with_gtsam_isam2_raises_composition_error(
|
||||
_airborne_env: None,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
# Arrange
|
||||
_stub_wrappers(monkeypatch)
|
||||
register_airborne_strategies()
|
||||
config = Config.with_blocks(
|
||||
c1_vio=_C1Block(),
|
||||
c4_pose=_C4Block(enabled=False, strategy="opencv_gtsam"),
|
||||
c5_state=_C5Block(strategy="gtsam_isam2"),
|
||||
)
|
||||
|
||||
# Act
|
||||
with pytest.raises(CompositionError) as info:
|
||||
compose_root(config, pre_constructed={})
|
||||
|
||||
# Assert
|
||||
msg = str(info.value)
|
||||
assert "c4_pose.enabled=False" in msg
|
||||
assert "c5_state.strategy='gtsam_isam2'" in msg
|
||||
assert "ADR-012" in msg, (
|
||||
"AZ-776 AC-3a: CompositionError message MUST cite ADR-012 so "
|
||||
"the operator can find the documented profile matrix"
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-3b: forbidden — c4_pose.enabled=True + c5_state.strategy=eskf
|
||||
|
||||
|
||||
def test_ac3b_enabled_c4_with_eskf_raises_composition_error(
|
||||
_airborne_env: None,
|
||||
monkeypatch: pytest.MonkeyPatch,
|
||||
) -> None:
|
||||
# Arrange
|
||||
_stub_wrappers(monkeypatch)
|
||||
register_airborne_strategies()
|
||||
config = Config.with_blocks(
|
||||
c1_vio=_C1Block(),
|
||||
c4_pose=_C4Block(enabled=True, strategy="opencv_gtsam"),
|
||||
c5_state=_C5Block(strategy="eskf"),
|
||||
)
|
||||
|
||||
# Act
|
||||
with pytest.raises(CompositionError) as info:
|
||||
compose_root(config, pre_constructed={})
|
||||
|
||||
# Assert
|
||||
msg = str(info.value)
|
||||
assert "c4_pose.enabled=True" in msg
|
||||
assert "c5_state.strategy='eskf'" in msg
|
||||
assert "ADR-012" in msg, (
|
||||
"AZ-776 AC-3b: CompositionError message MUST cite ADR-012 so "
|
||||
"the operator can find the documented profile matrix"
|
||||
)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-Bootstrap: build_pre_constructed omits c5_isam2_graph_handle when c4_pose disabled
|
||||
|
||||
|
||||
def test_pre_constructed_omits_isam2_handle_when_c4_disabled(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Any
|
||||
) -> None:
|
||||
"""When ``c4_pose.enabled=False`` the eager ESKF (estimator, handle)
|
||||
build still runs (so the c5_state wrapper's fast path stays
|
||||
populated), but the handle slot is absent from the returned dict
|
||||
(no consumer in the open-loop profile).
|
||||
"""
|
||||
# Arrange — minimal config with c4_pose.enabled=False + c5_state.strategy=eskf
|
||||
base = Config()
|
||||
runtime = dataclasses.replace(base.runtime, camera_calibration_path=str(tmp_path / "calib.json"))
|
||||
config = dataclasses.replace(base, runtime=runtime).with_blocks(
|
||||
c4_pose=_C4Block(enabled=False, strategy="opencv_gtsam"),
|
||||
c5_state=_C5Block(strategy="eskf"),
|
||||
)
|
||||
|
||||
# Stub every upstream builder so build_pre_constructed reaches the
|
||||
# AZ-776 guard without requiring real FAISS / TensorRT / camera JSON.
|
||||
sentinel_estimator = object()
|
||||
sentinel_handle = None # ESKF returns None for the handle by design
|
||||
|
||||
def _stub_pair(config: Any, **kwargs: Any) -> tuple[Any, Any]:
|
||||
del config, kwargs
|
||||
return sentinel_estimator, sentinel_handle
|
||||
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c6_descriptor_index", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c6_tile_store", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c7_inference", lambda c: object())
|
||||
monkeypatch.setattr(
|
||||
airborne_bootstrap,
|
||||
"_build_c3_lightglue_runtime",
|
||||
lambda c, *, inference_runtime: object(),
|
||||
)
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c3_feature_extractor", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c282_ransac_filter", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_imu_preintegrator", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_se3_utils", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_wgs_converter", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_state_estimator_pair", _stub_pair)
|
||||
# The c13_fdr cache is reset by the autouse fixture; the make_fdr_client
|
||||
# call inside build_pre_constructed runs unstubbed against the in-memory
|
||||
# client cache (no disk I/O).
|
||||
|
||||
# Act
|
||||
constructed = build_pre_constructed(config)
|
||||
|
||||
# Assert — open-loop profile: handle slot absent, estimator slot present
|
||||
assert "c5_isam2_graph_handle" not in constructed, (
|
||||
"AZ-776 / ADR-012: c4_pose.enabled=False MUST omit "
|
||||
"pre_constructed['c5_isam2_graph_handle'] (no C4 consumer); "
|
||||
f"got keys={sorted(constructed.keys())}"
|
||||
)
|
||||
assert constructed["_c5_prebuilt_estimator"] is sentinel_estimator, (
|
||||
"AZ-776: the prebuilt estimator slot still populates so the "
|
||||
"c5_state wrapper's fast path returns the ESKF instance"
|
||||
)
|
||||
|
||||
|
||||
def test_pre_constructed_keeps_isam2_handle_when_c4_enabled(
|
||||
monkeypatch: pytest.MonkeyPatch, tmp_path: Any
|
||||
) -> None:
|
||||
"""Symmetric AC-2 baseline: full GTSAM profile keeps the handle slot."""
|
||||
# Arrange
|
||||
base = Config()
|
||||
runtime = dataclasses.replace(base.runtime, camera_calibration_path=str(tmp_path / "calib.json"))
|
||||
config = dataclasses.replace(base, runtime=runtime).with_blocks(
|
||||
c4_pose=_C4Block(enabled=True, strategy="opencv_gtsam"),
|
||||
c5_state=_C5Block(strategy="gtsam_isam2"),
|
||||
)
|
||||
|
||||
sentinel_estimator = object()
|
||||
sentinel_handle = object() # GTSAM returns a real handle
|
||||
|
||||
def _stub_pair(config: Any, **kwargs: Any) -> tuple[Any, Any]:
|
||||
del config, kwargs
|
||||
return sentinel_estimator, sentinel_handle
|
||||
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c6_descriptor_index", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c6_tile_store", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c7_inference", lambda c: object())
|
||||
monkeypatch.setattr(
|
||||
airborne_bootstrap,
|
||||
"_build_c3_lightglue_runtime",
|
||||
lambda c, *, inference_runtime: object(),
|
||||
)
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c3_feature_extractor", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c282_ransac_filter", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_imu_preintegrator", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_se3_utils", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_wgs_converter", lambda c: object())
|
||||
monkeypatch.setattr(airborne_bootstrap, "_build_c5_state_estimator_pair", _stub_pair)
|
||||
|
||||
# Act
|
||||
constructed = build_pre_constructed(config)
|
||||
|
||||
# Assert
|
||||
assert constructed["c5_isam2_graph_handle"] is sentinel_handle, (
|
||||
"ADR-003 steady-state path: c4_pose.enabled=True MUST keep "
|
||||
"the iSAM2 handle in pre_constructed for the C4 wrapper"
|
||||
)
|
||||
assert constructed["_c5_prebuilt_estimator"] is sentinel_estimator
|
||||
Reference in New Issue
Block a user