diff --git a/_docs/02_tasks/todo/AZ-382_c5_isam2_smoother_wiring.md b/_docs/02_tasks/done/AZ-382_c5_isam2_smoother_wiring.md similarity index 100% rename from _docs/02_tasks/todo/AZ-382_c5_isam2_smoother_wiring.md rename to _docs/02_tasks/done/AZ-382_c5_isam2_smoother_wiring.md diff --git a/_docs/03_implementation/batch_13_cycle1_report.md b/_docs/03_implementation/batch_13_cycle1_report.md new file mode 100644 index 0000000..1f4dc79 --- /dev/null +++ b/_docs/03_implementation/batch_13_cycle1_report.md @@ -0,0 +1,84 @@ +# Batch 13 — Cycle 1 Implementation Report + +**Batch**: 13 of N +**Tasks landed**: AZ-382 (`GtsamIsam2StateEstimator` skeleton — iSAM2 + `IncrementalFixedLagSmoother` wiring + `ISam2GraphHandleImpl` real bodies) +**Cycle**: 1 +**Date**: 2026-05-11 + +## Scope + +| Task | Component | Purpose | +|------|-----------|---------| +| AZ-382 | C5 state estimator | Replaces the AZ-381 NotImplementedError skeleton for the iSAM2 graph handle with real GTSAM calls, and adds the production-default `GtsamIsam2StateEstimator` class that owns the `gtsam.ISAM2` + `gtsam_unstable.IncrementalFixedLagSmoother(K * frame_period_s)` substrate. The estimator's `StateEstimator` Protocol methods (`add_vio`, `add_pose_anchor`, `add_fc_imu`, `current_estimate`, `smoothed_history`, `health_snapshot`) intentionally still raise `NotImplementedError` — their bodies are owned by AZ-383 (factor adds) and AZ-384 (marginals + outputs). The `_isam2_handle.py` four-method surface (`add_factor`, `update`, `compute_marginals`, `last_anchor_age_ms`), however, is fully implemented here, including defensive success/failure logging on every mutation (R05 mitigation). | + +## Files added / modified + +### Added (prod) + +- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` — `GtsamIsam2StateEstimator` (`StateEstimator`) class + module-level `create(...)` factory + `register()` convenience. Constructor builds `_isam2 = gtsam.ISAM2(gtsam.ISAM2Params())`, `_smoother = gtsam_unstable.IncrementalFixedLagSmoother(K * _FRAME_PERIOD_S)`, `_graph = gtsam.NonlinearFactorGraph()`, `_values = gtsam.Values()`, `_key_for_frame: dict[UUID, int] = {}`, `_next_key_counter = 0`, `_last_anchor_ns = 0`. Emits one DEBUG log `kind="c5.state.isam2_initialised"`. Exposes `key_for_frame(frame_id) -> int` for AZ-383 key-management. The 6 Protocol methods raise `NotImplementedError` naming AZ-383 / AZ-384. + +### Modified (prod) + +- `src/gps_denied_onboard/components/c5_state/_isam2_handle.py` — replaces all four `NotImplementedError` bodies with real GTSAM calls: + - `add_factor(factor)` → `self._estimator._graph.add(factor)`; success → DEBUG log `c5.state.add_factor_ok` with `{factor_type, graph_size}`; failure → ERROR log `c5.state.add_factor_failed` + raise `EstimatorDegradedError`. + - `update(graph, values, timestamps=None)` → `self._estimator._isam2.update(graph, values)` then `self._estimator._smoother.update(graph, values, timestamps_map)`; `timestamps=None` substitutes an empty `gtsam_unstable.FixedLagSmootherKeyTimestampMap()`. Either failure → ERROR log + raise `EstimatorFatalError`. + - `compute_marginals()` → `gtsam.Marginals(self._estimator._isam2.getFactorsUnsafe(), self._estimator._isam2.calculateEstimate())`. + - `last_anchor_age_ms()` → `(time.monotonic_ns() - self._estimator._last_anchor_ns) // 1_000_000`. + +### Added (tests) + +- `tests/unit/c5_state/test_az382_isam2_smoother_wiring.py` — 27 tests covering all 10 ACs: substrate construction + DEBUG log; unique key assignment + idempotent re-lookup + `'x'` namespace; smoother window equals `K * frame_period_s`; window-bounds validation [10, 20] at config-load (`5` and `21` both rejected); `add_factor` success path + failure path with `EstimatorDegradedError` + structured failure log; `update` success path with iSAM2 estimate growing + smoother accepting empty timestamps map; `update` iSAM2 failure → `EstimatorFatalError`; `update` smoother failure → `EstimatorFatalError`; `compute_marginals` returns a `gtsam.Marginals`; `last_anchor_age_ms` returns very large initially and ~0 after anchor set; defensive logging on every mutation (success + failure paths); all six Protocol methods on the estimator raising `NotImplementedError` with `AZ-383` / `AZ-384` in the message; `register()` adds strategy to the factory registry; `create()` returns a handle bound to the returned estimator. + +### Modified (tests) + +- `tests/unit/c5_state/test_az381_state_protocol.py` — removed the now-obsolete `test_ac8_handle_methods_raise_named_task` (the bodies no longer raise `NotImplementedError`; they're real). Replaced with a comment pointing to the new AZ-382 AC test file. The Protocol-conformance assertion `test_ac8_handle_is_isam2_graph_handle` was retained. + +## Architectural notes + +- **`_FRAME_PERIOD_S = 1.0 / 3.0`** — D-C5-3 fixes the keyframe processing rate at 3 Hz (so a K=15 window equals 5 s of wall time). This is a module-level constant, not a config knob — only `K` (the keyframe count) is operator-tunable per the contract. +- **Empty `FixedLagSmootherKeyTimestampMap` shim** — GTSAM's `IncrementalFixedLagSmoother.update(...)` does not have a no-timestamps overload (unlike `ISAM2.update`). The handle's `update(...)` substitutes an empty map when the caller passes `timestamps=None` so the C4 → C5 seam can stay `timestamps: Any | None = None` per the Protocol; AZ-383 will populate the map with per-key arrival timestamps. +- **Defensive logging (R05)** — every mutation logs structured success or failure with `kind` keys: `c5.state.add_factor_ok`, `c5.state.add_factor_failed`, `c5.state.update_ok`, `c5.state.isam2_update_failed`, `c5.state.smoother_update_failed`. The C5 contract makes this mandatory because silent factor-add failures bit the prototype. +- **AC-4 enforcement** — `keyframe_window_size ∈ [10, 20]` is enforced at `C5StateConfig.__post_init__` (AZ-381 ground), so AZ-382 inherits the gate "for free"; the AZ-382 AC tests assert via `C5StateConfig(keyframe_window_size=5)` raising `ConfigError`. +- **`register()` is opt-in, not auto-import** — matches the C8 fc_factory pattern. Per-binary bootstrap modules and test fixtures call it explicitly; ADR-002 keeps the build-flag gate the single source of strategy availability. + +## Test counts + +| Suite | Before (B12) | After (B13) | Delta | +|-------|--------------|-------------|-------| +| Total passing | 521 | 547 | +26 | +| Skipped | 2 | 2 | 0 | +| AZ-382 (new) | 0 | 27 | +27 | +| AZ-381 (preserved) | 20 | 19 | −1 (obsolete handle-NIE test removed; behaviour now lives in AZ-382 tests) | + +Run command: `PYTHONPATH=src pytest tests/ -q` → `547 passed, 2 skipped in ~30s`. + +## Lint / type + +- `ruff check src/ tests/` — clean (1 fixable issue auto-corrected during the cycle). +- `ruff format src/ tests/` — clean (1 file reformatted). +- `mypy` on the new modules — 0 errors. Pre-existing errors in `logging/structured.py` + `c13_fdr/writer.py` left untouched per scope discipline. + +## Acceptance evidence + +| AC | Test(s) | Status | +|----|--------|--------| +| AC-1 Construction | `test_ac1_construction_initialises_substrate`, `test_ac1_construction_emits_debug_log` | PASS | +| AC-2 Key-management | `test_ac2_key_for_frame_assigns_unique_keys`, `test_ac2_key_for_frame_is_idempotent`, `test_ac2_keys_use_x_namespace` | PASS | +| AC-3 Window size | `test_ac3_smoother_window_is_k_times_frame_period` | PASS | +| AC-4 Window validation | `test_ac4_window_below_min_rejected_by_config`, `test_ac4_window_above_max_rejected_by_config` | PASS | +| AC-5 `add_factor` real body | `test_ac5_add_factor_appends_to_graph`, `test_ac5_add_factor_failure_raises_degraded_and_logs` | PASS | +| AC-6 `update` real body | `test_ac6_update_advances_isam2_and_smoother`, `test_ac6_update_with_none_timestamps_substitutes_empty_map`, `test_ac6_isam2_failure_raises_fatal`, `test_ac6_smoother_failure_raises_fatal` | PASS | +| AC-7 `compute_marginals` real body | `test_ac7_compute_marginals_returns_real_instance` | PASS | +| AC-8 `last_anchor_age_ms` | `test_ac8_last_anchor_age_ms_huge_when_no_anchor`, `test_ac8_last_anchor_age_ms_small_after_anchor_set` | PASS | +| AC-9 Defensive logging | `test_ac9_add_factor_emits_success_log`, `test_ac9_update_emits_success_log`, plus failure-path log assertions in AC-5/AC-6 tests | PASS | +| AC-10 `NotImplementedError` on estimator methods | `test_ac10_add_vio_raises_named_az383`, `test_ac10_add_pose_anchor_raises_named_az383`, `test_ac10_add_fc_imu_raises_named_az383`, `test_ac10_current_estimate_raises_named_az384`, `test_ac10_smoothed_history_raises_named_az384`, `test_ac10_health_snapshot_raises_named_az384` | PASS | + +## Caplog gotcha discovered + +`get_logger(name)` unconditionally sets `logger.setLevel(logging.NOTSET)`, which silently overrides any `caplog.at_level(logging.DEBUG, logger=)` set by a test BEFORE the logger is fetched. Symptom: DEBUG records vanish from `caplog.records`. Workaround in this batch: tests that need to capture construction-time DEBUG logs use `caplog.at_level(logging.DEBUG)` at the root level. Worth a project-wide rule once we hit the same in another component — but not in this batch's scope. + +## Known forward actions (not in scope this batch) + +- AZ-383 will populate `add_vio` / `add_pose_anchor` / `add_fc_imu` and start filling the `FixedLagSmootherKeyTimestampMap` with real per-key timestamps + recording `_last_anchor_ns` on every satellite-anchored pose. +- AZ-384 will populate `current_estimate` / `smoothed_history` / `health_snapshot` against the marginals computed by the handle. +- A per-binary bootstrap module (one per `BUILD_STATE_*` flag combination) will call `gtsam_isam2_estimator.register()` instead of relying on test fixtures. diff --git a/_docs/_autodev_state.md b/_docs/_autodev_state.md index ce0490c..a8af20a 100644 --- a/_docs/_autodev_state.md +++ b/_docs/_autodev_state.md @@ -8,7 +8,7 @@ status: in_progress sub_step: phase: 6 name: implement-tasks - detail: "batch 12 of N committed (AZ-381 c5 state protocol + DTOs + factory + ISam2GraphHandle skeleton + strict EstimatorOutput/EstimatorHealth reshape across C8)" + detail: "batch 13 of N committed (AZ-382 c5 GtsamIsam2StateEstimator skeleton + ISam2GraphHandleImpl real bodies + defensive logging)" retry_count: 0 cycle: 1 tracker: jira diff --git a/src/gps_denied_onboard/components/c5_state/_isam2_handle.py b/src/gps_denied_onboard/components/c5_state/_isam2_handle.py index f20a872..6b8f9b6 100644 --- a/src/gps_denied_onboard/components/c5_state/_isam2_handle.py +++ b/src/gps_denied_onboard/components/c5_state/_isam2_handle.py @@ -1,24 +1,35 @@ -"""Concrete ``ISam2GraphHandle`` skeleton — AZ-381. +"""Concrete ``ISam2GraphHandle`` — owned by AZ-382 / E-C5. C4 (``OpenCVGtsamPoseEstimator``) calls ``add_factor`` / ``update`` / ``compute_marginals`` against this handle, NOT against C5 directly — ADR-003 says C5 owns the graph; this handle is the typed seam C4 uses to drive it without importing C5 internals. -AZ-381 ships the skeleton: every method raises -``NotImplementedError("Body owned by AZ-382 iSAM2 wiring task")``. The -``NotImplementedError`` messages name AZ-382 so the next task's -implementer can grep for them. +AZ-381 shipped the Protocol surface and a NotImplementedError +skeleton; AZ-382 replaces the four method bodies with the real GTSAM +calls against the estimator's ``_isam2`` + ``_smoother`` instances. +The Protocol surface does not change between AZ-381 and AZ-382. -AZ-382 replaces the four method bodies with the real GTSAM calls -against the C5 estimator's ``_isam2`` + ``_smoother`` instances. The -Protocol surface is stable from AZ-381 onward. +Defensive logging (R05 mitigation): every mutation logs SUCCESS or +FAILURE with structured fields. Silent factor-add failures bit this +codebase during the prototype — the contract now mandates the +defensive trace. """ from __future__ import annotations +import time from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable +import gtsam +import gtsam_unstable + +from gps_denied_onboard.components.c5_state.errors import ( + EstimatorDegradedError, + EstimatorFatalError, +) +from gps_denied_onboard.logging import get_logger + if TYPE_CHECKING: from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import ( GtsamIsam2StateEstimator, @@ -48,37 +59,135 @@ class ISam2GraphHandle(Protocol): class ISam2GraphHandleImpl(ISam2GraphHandle): - """Skeleton — every method delegates to AZ-382 once that task lands. + """Real iSAM2 graph handle — drives the estimator's ``_isam2`` + ``_smoother``. - The skeleton exists so AZ-381 can ship a runnable composition - root that produces a concrete handle reference for C4 to inject - against (per ADR-009). AZ-382 replaces every body with the real - GTSAM calls; the Protocol surface does not change. + Every mutation is wrapped in success/failure logging per R05; + individual failures are translated into the C5 error hierarchy + (``EstimatorDegradedError`` for recoverable graph-add issues, + ``EstimatorFatalError`` for solver failures the calling thread + cannot recover from). """ def __init__(self, estimator: GtsamIsam2StateEstimator) -> None: self._estimator = estimator + self._log = get_logger("c5_state.isam2_handle") def add_factor(self, factor: Any) -> None: - raise NotImplementedError( - "Body owned by AZ-382 iSAM2 wiring task — " - "this skeleton is intentionally inert until iSAM2 wiring lands." + """Append ``factor`` to the pending ``NonlinearFactorGraph``. + + The estimator's ``_graph`` is a staging buffer that AZ-383 + flushes into ``_isam2`` + ``_smoother`` on every keyframe. + Per the C5 contract this method is the only sanctioned entry + point for C4 → C5 factor adds — direct mutation of + ``estimator._graph`` from outside this handle violates the + seam. + """ + try: + self._estimator._graph.add(factor) + except Exception as exc: + self._log.error( + "c5.state.add_factor_failed", + extra={ + "kind": "c5.state.add_factor_failed", + "kv": { + "factor_type": type(factor).__name__, + "error": str(exc), + }, + }, + ) + raise EstimatorDegradedError( + f"add_factor failed for {type(factor).__name__}: {exc}" + ) from exc + self._log.debug( + "c5.state.add_factor_ok", + extra={ + "kind": "c5.state.add_factor_ok", + "kv": { + "factor_type": type(factor).__name__, + "graph_size": self._estimator._graph.size(), + }, + }, ) def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None: - raise NotImplementedError( - "Body owned by AZ-382 iSAM2 wiring task — " - "this skeleton is intentionally inert until iSAM2 wiring lands." + """Apply ``(graph, values)`` to iSAM2 AND advance the smoother. + + The smoother requires a ``FixedLagSmootherKeyTimestampMap``; + when the caller passes ``None`` we substitute an empty map so + the call doesn't trip on the GTSAM C++ signature. AZ-383 will + populate the map with per-key arrival timestamps so the + sliding window can evict aged keyframes. + + ``EstimatorFatalError`` is raised on either iSAM2 or smoother + failure — both indicate solver state the calling thread + cannot recover from in-flight; the runtime root's AC-5.2 + fallback (AZ-388) catches it and drops C5 outputs entirely. + """ + timestamps_map = ( + timestamps + if timestamps is not None + else gtsam_unstable.FixedLagSmootherKeyTimestampMap() + ) + try: + self._estimator._isam2.update(graph, values) + except Exception as exc: + self._log.error( + "c5.state.isam2_update_failed", + extra={ + "kind": "c5.state.isam2_update_failed", + "kv": { + "graph_size": getattr(graph, "size", lambda: -1)(), + "error": str(exc), + }, + }, + ) + raise EstimatorFatalError(f"iSAM2.update failed: {exc}") from exc + + try: + self._estimator._smoother.update(graph, values, timestamps_map) + except Exception as exc: + self._log.error( + "c5.state.smoother_update_failed", + extra={ + "kind": "c5.state.smoother_update_failed", + "kv": { + "graph_size": getattr(graph, "size", lambda: -1)(), + "error": str(exc), + }, + }, + ) + raise EstimatorFatalError(f"IncrementalFixedLagSmoother.update failed: {exc}") from exc + + self._log.debug( + "c5.state.update_ok", + extra={ + "kind": "c5.state.update_ok", + "kv": { + "graph_size": getattr(graph, "size", lambda: -1)(), + "values_size": getattr(values, "size", lambda: -1)(), + }, + }, ) def compute_marginals(self) -> Any: - raise NotImplementedError( - "Body owned by AZ-382 iSAM2 wiring task — " - "this skeleton is intentionally inert until iSAM2 wiring lands." + """Return a ``gtsam.Marginals`` snapshot of the current iSAM2 state. + + Built on demand from ``getFactorsUnsafe()`` + ``calculateEstimate()`` + rather than cached — AZ-384 will decide on the cache layer + once the access pattern from C4 + the C13 smoothed-history + path is real. + """ + return gtsam.Marginals( + self._estimator._isam2.getFactorsUnsafe(), + self._estimator._isam2.calculateEstimate(), ) def last_anchor_age_ms(self) -> int: - raise NotImplementedError( - "Body owned by AZ-382 iSAM2 wiring task — " - "this skeleton is intentionally inert until iSAM2 wiring lands." - ) + """Milliseconds since the last satellite-anchored pose was added. + + Returns a very large number until AZ-383 records the first + anchor (``_last_anchor_ns`` is initialised to 0 in the + estimator constructor). This matches the C5 contract's + documented "no anchor yet" sentinel. + """ + return (time.monotonic_ns() - self._estimator._last_anchor_ns) // 1_000_000 diff --git a/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py b/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py new file mode 100644 index 0000000..aa9e23e --- /dev/null +++ b/src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py @@ -0,0 +1,230 @@ +"""C5 ``GtsamIsam2StateEstimator`` — skeleton wiring (AZ-382 / E-C5). + +Builds the GTSAM iSAM2 graph + ``IncrementalFixedLagSmoother`` substrate +on which AZ-383 / AZ-384 / AZ-385 will hang the actual factor adds, +marginals, and source-label logic. This task owns: + +* ``__init__`` — constructs ``_isam2`` (``gtsam.ISAM2``), ``_smoother`` + (``gtsam_unstable.IncrementalFixedLagSmoother`` with a + ``K * frame_period_s`` window), ``_graph`` + ``_values`` containers, + and the ``_key_for_frame`` mapping used by AZ-383 when it converts + ``UUID`` ``frame_id`` values into GTSAM ``Key`` integers via + ``gtsam.symbol('x', counter)``. +* :func:`create` — module-level factory the composition root registers + as ``register_state_estimator("gtsam_isam2", create)``. Returns the + ``(StateEstimator, ISam2GraphHandle)`` tuple per + :mod:`gps_denied_onboard.runtime_root.state_factory`. +* :func:`register` — convenience that calls ``register_state_estimator`` + with this strategy. Test fixtures + the per-binary bootstrap module + call this; AZ-381 deliberately did not auto-register at import. + +Every ``StateEstimator`` Protocol method intentionally raises +``NotImplementedError`` with the **next** task's tracker ID in the +message — the bodies are owned by: + +* ``add_vio`` / ``add_pose_anchor`` / ``add_fc_imu`` → AZ-383 +* ``current_estimate`` / ``smoothed_history`` / ``health_snapshot`` → AZ-384 + +The ``ISam2GraphHandleImpl`` bodies in +:mod:`gps_denied_onboard.components.c5_state._isam2_handle`, however, +ARE owned by this task and are populated against the estimator +constructed here. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any, Final +from uuid import UUID + +import gtsam +import gtsam_unstable + +from gps_denied_onboard.components.c5_state._isam2_handle import ( + ISam2GraphHandle, + ISam2GraphHandleImpl, +) +from gps_denied_onboard.components.c5_state.config import C5StateConfig +from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError +from gps_denied_onboard.components.c5_state.interface import StateEstimator +from gps_denied_onboard.logging import get_logger + +if TYPE_CHECKING: + from gps_denied_onboard._types.fc import ImuTelemetrySample + from gps_denied_onboard._types.pose import PoseEstimate + from gps_denied_onboard._types.state import EstimatorHealth, EstimatorOutput + from gps_denied_onboard._types.vio import VioOutput + from gps_denied_onboard.config import Config + +__all__ = [ + "GtsamIsam2StateEstimator", + "create", + "register", +] + + +# D-C5-3 keyframe processing rate (3 Hz → 1/3 s per keyframe). The +# ``IncrementalFixedLagSmoother`` window is expressed in seconds, so we +# multiply ``K`` keyframes by this constant to derive the temporal +# window passed to GTSAM. Kept as a module-level constant rather than a +# config knob — D-C5-3 fixes the keyframe rate; the window size ``K`` +# is the only operator-configurable lever. +_FRAME_PERIOD_S: Final[float] = 1.0 / 3.0 + +# Strategy slug used by the C5 composition factory and the +# ``register_state_estimator`` registry. +_STRATEGY: Final[str] = "gtsam_isam2" + + +class GtsamIsam2StateEstimator(StateEstimator): + """Production-default C5 estimator — iSAM2 + ``IncrementalFixedLagSmoother``. + + Holds the canonical GTSAM substrate per ADR-003. C4's + ``OpenCVGtsamPoseEstimator`` calls into the same graph indirectly + via the injected :class:`ISam2GraphHandle`; both components MUST + run on the same ingest thread (composition root enforces this via + :func:`bind_state_ingest_thread`). + """ + + def __init__( + self, + config: Config, + *, + imu_preintegrator: Any, + se3_utils: Any, + wgs_converter: Any, + fdr_client: Any, + ) -> None: + block = self._extract_block(config) + + self._config: Config = config + self._block: C5StateConfig = block + self._imu_preintegrator: Any = imu_preintegrator + self._se3_utils: Any = se3_utils + self._wgs_converter: Any = wgs_converter + self._fdr_client: Any = fdr_client + + self._isam2 = gtsam.ISAM2(gtsam.ISAM2Params()) + window_seconds: float = block.keyframe_window_size * _FRAME_PERIOD_S + self._smoother = gtsam_unstable.IncrementalFixedLagSmoother(window_seconds) + self._graph = gtsam.NonlinearFactorGraph() + self._values = gtsam.Values() + + self._key_for_frame: dict[UUID, int] = {} + self._next_key_counter: int = 0 + # Initialised to 0 ⇒ ``last_anchor_age_ms`` returns + # ``monotonic_ns() / 1e6`` (a very large number) until AZ-383 + # records the first satellite-anchored pose. The contract + # documents this as the "no anchor yet" sentinel. + self._last_anchor_ns: int = 0 + + get_logger("c5_state.gtsam_isam2").debug( + "c5.state.isam2_initialised", + extra={ + "kind": "c5.state.isam2_initialised", + "kv": { + "keyframe_window_size": block.keyframe_window_size, + "window_seconds": window_seconds, + "total_factors_initial": 0, + }, + }, + ) + + @staticmethod + def _extract_block(config: Config) -> C5StateConfig: + components = getattr(config, "components", None) or {} + block = components.get("c5_state") if isinstance(components, dict) else None + if block is None: + return C5StateConfig() + if isinstance(block, C5StateConfig): + return block + raise StateEstimatorConfigError( + f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}" + ) + + def key_for_frame(self, frame_id: UUID) -> int: + """Return the GTSAM ``Key`` for ``frame_id``, assigning on first use. + + AZ-383 factor adds call this to translate ``UUID`` frame + identifiers into the integer keys GTSAM uses for symbols. Per + the C5 contract §"Key management policy" we reserve the + ``'x'`` (pose) namespace; AZ-383 will additionally use + ``'b'`` (bias) and ``'v'`` (velocity). + """ + existing = self._key_for_frame.get(frame_id) + if existing is not None: + return existing + new_key: int = gtsam.symbol("x", self._next_key_counter) + self._key_for_frame[frame_id] = new_key + self._next_key_counter += 1 + return new_key + + def add_vio(self, vio: VioOutput) -> None: + raise NotImplementedError( + "Factor adds owned by AZ-383 — VIO/Pose/IMU factor wiring lands there." + ) + + def add_pose_anchor(self, pose: PoseEstimate) -> None: + raise NotImplementedError( + "Factor adds owned by AZ-383 — VIO/Pose/IMU factor wiring lands there." + ) + + def add_fc_imu(self, imu_window: ImuTelemetrySample) -> None: + raise NotImplementedError( + "Factor adds owned by AZ-383 — VIO/Pose/IMU factor wiring lands there." + ) + + def current_estimate(self) -> EstimatorOutput: + raise NotImplementedError( + "Marginals + outputs owned by AZ-384 — current_estimate body lands there." + ) + + def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]: + raise NotImplementedError( + "Marginals + outputs owned by AZ-384 — smoothed_history body lands there." + ) + + def health_snapshot(self) -> EstimatorHealth: + raise NotImplementedError( + "Marginals + outputs owned by AZ-384 — health_snapshot body lands there." + ) + + +def create( + *, + config: Config, + imu_preintegrator: Any, + se3_utils: Any, + wgs_converter: Any, + fdr_client: Any, +) -> tuple[StateEstimator, ISam2GraphHandle]: + """Composition-root factory — returns ``(estimator, handle)`` tuple. + + The handle holds a reference to the estimator so the four + ``ISam2GraphHandleImpl`` method bodies (defined in + :mod:`...components.c5_state._isam2_handle`) can drive the same + graph the estimator owns. + """ + estimator = GtsamIsam2StateEstimator( + config, + imu_preintegrator=imu_preintegrator, + se3_utils=se3_utils, + wgs_converter=wgs_converter, + fdr_client=fdr_client, + ) + handle = ISam2GraphHandleImpl(estimator) + return estimator, handle + + +def register() -> None: + """Register :func:`create` under the ``"gtsam_isam2"`` strategy slug. + + Called by per-binary bootstrap modules under the + ``BUILD_STATE_GTSAM_ISAM2`` flag, and by unit-test fixtures that + exercise the factory path. Deliberately NOT called at module + import — ADR-002 requires the bootstrap module to be the single + register-call site so the build-flag gate stays the only place + that decides which strategies are linked. + """ + from gps_denied_onboard.runtime_root.state_factory import register_state_estimator + + register_state_estimator(_STRATEGY, create) diff --git a/tests/unit/c5_state/test_az381_state_protocol.py b/tests/unit/c5_state/test_az381_state_protocol.py index 3ab484e..23dd9b6 100644 --- a/tests/unit/c5_state/test_az381_state_protocol.py +++ b/tests/unit/c5_state/test_az381_state_protocol.py @@ -293,17 +293,12 @@ def test_ac8_handle_is_isam2_graph_handle() -> None: assert isinstance(handle, ISam2GraphHandle) -def test_ac8_handle_methods_raise_named_task() -> None: - handle = ISam2GraphHandleImpl(estimator=mock.MagicMock()) - - with pytest.raises(NotImplementedError, match="AZ-382"): - handle.add_factor(mock.MagicMock()) - with pytest.raises(NotImplementedError, match="AZ-382"): - handle.update(mock.MagicMock(), mock.MagicMock()) - with pytest.raises(NotImplementedError, match="AZ-382"): - handle.compute_marginals() - with pytest.raises(NotImplementedError, match="AZ-382"): - handle.last_anchor_age_ms() +# Note: the AZ-381 skeleton's ``NotImplementedError`` bodies were +# replaced with real GTSAM calls by AZ-382. The "methods raise" test +# that lived here has moved to +# ``tests/unit/c5_state/test_az382_isam2_smoother_wiring.py``, which +# now asserts the real behaviour (``add_factor`` grows the graph, +# ``update`` advances iSAM2 + the smoother, etc.). # ---------------------------------------------------------------------- diff --git a/tests/unit/c5_state/test_az382_isam2_smoother_wiring.py b/tests/unit/c5_state/test_az382_isam2_smoother_wiring.py new file mode 100644 index 0000000..d1fe4d6 --- /dev/null +++ b/tests/unit/c5_state/test_az382_isam2_smoother_wiring.py @@ -0,0 +1,443 @@ +"""AZ-382 — GtsamIsam2StateEstimator + ISam2GraphHandleImpl real-body tests. + +Ten ACs from ``_docs/02_tasks/done/AZ-382_c5_isam2_smoother_wiring.md`` +(authored todo, archived here once the task transitions to Done): + +- AC-1 Construction succeeds; ``_isam2`` / ``_smoother`` / ``_graph`` + / ``_values`` / ``_key_for_frame`` initialised. +- AC-2 Key-management policy assigns unique GTSAM keys via + ``gtsam.symbol('x', counter)``; repeat lookups return the same + key. +- AC-3 ``IncrementalFixedLagSmoother`` instantiated with + ``K * frame_period_s``. +- AC-4 ``keyframe_window_size = 5`` rejected at config-load + (delegates to the AZ-381 ``C5StateConfig.__post_init__``). +- AC-5 ``ISam2GraphHandleImpl.add_factor`` real body — appends to + ``estimator._graph``; failure raises + :class:`EstimatorDegradedError` AND logs the failure record. +- AC-6 ``ISam2GraphHandleImpl.update`` real body — iSAM2 + + smoother advance; failure raises + :class:`EstimatorFatalError`. +- AC-7 ``ISam2GraphHandleImpl.compute_marginals`` returns a real + ``gtsam.Marginals`` instance. +- AC-8 ``ISam2GraphHandleImpl.last_anchor_age_ms`` returns a very + large number until an anchor lands. +- AC-9 Defensive logging emitted on every mutation (success + failure). +- AC-10 ``StateEstimator`` Protocol methods on the estimator still + raise ``NotImplementedError`` naming AZ-383 / AZ-384. +""" + +from __future__ import annotations + +import logging +from unittest import mock +from uuid import uuid4 + +import gtsam +import gtsam_unstable +import pytest + +from gps_denied_onboard.components.c5_state._isam2_handle import ( + ISam2GraphHandle, + ISam2GraphHandleImpl, +) +from gps_denied_onboard.components.c5_state.config import C5StateConfig +from gps_denied_onboard.components.c5_state.errors import ( + EstimatorDegradedError, + EstimatorFatalError, +) +from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import ( + _FRAME_PERIOD_S, + GtsamIsam2StateEstimator, + create, + register, +) +from gps_denied_onboard.components.c5_state.interface import StateEstimator +from gps_denied_onboard.config.schema import ConfigError +from gps_denied_onboard.runtime_root.state_factory import ( + build_state_estimator, + clear_state_registry, +) + + +@pytest.fixture(autouse=True) +def _registry_isolation(): + # Arrange + clear_state_registry() + yield + clear_state_registry() + + +def _build_config(*, strategy: str = "gtsam_isam2", keyframe_window_size: int = 15): + block = C5StateConfig(strategy=strategy, keyframe_window_size=keyframe_window_size) + cfg = mock.MagicMock() + cfg.components = {"c5_state": block} + return cfg + + +def _build_estimator(*, keyframe_window_size: int = 15) -> GtsamIsam2StateEstimator: + cfg = _build_config(keyframe_window_size=keyframe_window_size) + return GtsamIsam2StateEstimator( + cfg, + imu_preintegrator=mock.MagicMock(), + se3_utils=mock.MagicMock(), + wgs_converter=mock.MagicMock(), + fdr_client=mock.MagicMock(), + ) + + +# --------------------------------------------------------------------- +# AC-1: construction + + +def test_ac1_construction_initialises_substrate() -> None: + estimator = _build_estimator(keyframe_window_size=15) + + assert isinstance(estimator._isam2, gtsam.ISAM2) + assert isinstance(estimator._smoother, gtsam_unstable.IncrementalFixedLagSmoother) + assert isinstance(estimator._graph, gtsam.NonlinearFactorGraph) + assert isinstance(estimator._values, gtsam.Values) + assert estimator._key_for_frame == {} + assert estimator._next_key_counter == 0 + assert estimator._last_anchor_ns == 0 + assert estimator._graph.size() == 0 + assert isinstance(estimator, StateEstimator) + + +def test_ac1_construction_emits_debug_log(caplog: pytest.LogCaptureFixture) -> None: + # ``get_logger`` resets the named logger to NOTSET, which masks + # ``caplog.at_level(DEBUG, logger=...)``. Bump the root level + # instead so the DEBUG record propagates to caplog's handler. + with caplog.at_level(logging.DEBUG): + _build_estimator(keyframe_window_size=12) + + records = [r for r in caplog.records if r.kind == "c5.state.isam2_initialised"] + assert len(records) == 1 + assert records[0].kv["keyframe_window_size"] == 12 + assert records[0].kv["total_factors_initial"] == 0 + + +# --------------------------------------------------------------------- +# AC-2: key-management policy + + +def test_ac2_key_for_frame_assigns_unique_keys() -> None: + estimator = _build_estimator() + u1, u2, u3 = uuid4(), uuid4(), uuid4() + + k1 = estimator.key_for_frame(u1) + k2 = estimator.key_for_frame(u2) + k3 = estimator.key_for_frame(u3) + + assert k1 != k2 != k3 + assert estimator._next_key_counter == 3 + assert estimator._key_for_frame == {u1: k1, u2: k2, u3: k3} + + +def test_ac2_key_for_frame_is_idempotent() -> None: + estimator = _build_estimator() + u1 = uuid4() + + first = estimator.key_for_frame(u1) + second = estimator.key_for_frame(u1) + + assert first == second + assert estimator._next_key_counter == 1 + + +def test_ac2_keys_use_x_namespace() -> None: + estimator = _build_estimator() + u1 = uuid4() + + key = estimator.key_for_frame(u1) + expected_first_x_key = gtsam.symbol("x", 0) + + assert key == expected_first_x_key + + +# --------------------------------------------------------------------- +# AC-3: window size respected + + +def test_ac3_smoother_window_is_k_times_frame_period() -> None: + # Arrange / Act + estimator = _build_estimator(keyframe_window_size=15) + + expected_window_seconds = 15 * _FRAME_PERIOD_S + + # Hit the smoother through a controlled smoke path to be sure the + # window is honored — direct introspection is C++-private. We use + # the smoother's ``smootherLag()`` getter (FixedLagSmoother base). + assert estimator._smoother.smootherLag() == pytest.approx(expected_window_seconds, rel=1e-6) + + +# --------------------------------------------------------------------- +# AC-4: window size validation + + +def test_ac4_window_below_min_rejected_by_config() -> None: + with pytest.raises(ConfigError, match=r"\[10, 20\]"): + C5StateConfig(keyframe_window_size=5) + + +def test_ac4_window_above_max_rejected_by_config() -> None: + with pytest.raises(ConfigError, match=r"\[10, 20\]"): + C5StateConfig(keyframe_window_size=21) + + +# --------------------------------------------------------------------- +# AC-5: ISam2GraphHandleImpl.add_factor real body + + +def test_ac5_add_factor_appends_to_graph(caplog: pytest.LogCaptureFixture) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + key = estimator.key_for_frame(uuid4()) + factor = gtsam.PriorFactorPose3(key, gtsam.Pose3(), gtsam.noiseModel.Isotropic.Sigma(6, 0.1)) + + with caplog.at_level(logging.DEBUG, logger="c5_state.isam2_handle"): + handle.add_factor(factor) + + assert estimator._graph.size() == 1 + ok_records = [r for r in caplog.records if r.kind == "c5.state.add_factor_ok"] + assert len(ok_records) == 1 + assert ok_records[0].kv["factor_type"] == "PriorFactorPose3" + assert ok_records[0].kv["graph_size"] == 1 + + +def test_ac5_add_factor_failure_raises_degraded_and_logs( + caplog: pytest.LogCaptureFixture, +) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + # Replace the graph with a stub whose ``add`` raises so we can test + # the failure path without depending on a specific GTSAM exception. + failing_graph = mock.MagicMock() + failing_graph.add.side_effect = RuntimeError("synthetic graph failure") + estimator._graph = failing_graph + + with caplog.at_level(logging.ERROR, logger="c5_state.isam2_handle"): + with pytest.raises(EstimatorDegradedError, match="synthetic graph failure"): + handle.add_factor(mock.MagicMock()) + + err_records = [r for r in caplog.records if r.kind == "c5.state.add_factor_failed"] + assert len(err_records) == 1 + assert "synthetic graph failure" in err_records[0].kv["error"] + + +# --------------------------------------------------------------------- +# AC-6: ISam2GraphHandleImpl.update real body + + +def _build_unit_update_payload() -> tuple[ + gtsam.NonlinearFactorGraph, gtsam.Values, gtsam_unstable.FixedLagSmootherKeyTimestampMap +]: + key = gtsam.symbol("x", 0) + graph = gtsam.NonlinearFactorGraph() + graph.add(gtsam.PriorFactorPose3(key, gtsam.Pose3(), gtsam.noiseModel.Isotropic.Sigma(6, 0.1))) + values = gtsam.Values() + values.insert(key, gtsam.Pose3()) + timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + timestamps.insert((key, 0.0)) + return graph, values, timestamps + + +def test_ac6_update_advances_isam2_and_smoother(caplog: pytest.LogCaptureFixture) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + graph, values, timestamps = _build_unit_update_payload() + + with caplog.at_level(logging.DEBUG, logger="c5_state.isam2_handle"): + handle.update(graph, values, timestamps) + + # iSAM2 should now hold the prior factor; calculateEstimate() ⇒ non-empty Values. + assert estimator._isam2.calculateEstimate().size() == 1 + ok_records = [r for r in caplog.records if r.kind == "c5.state.update_ok"] + assert len(ok_records) == 1 + + +def test_ac6_update_with_none_timestamps_substitutes_empty_map() -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + graph, values, _ = _build_unit_update_payload() + + handle.update(graph, values, None) + + assert estimator._isam2.calculateEstimate().size() == 1 + + +def test_ac6_isam2_failure_raises_fatal(caplog: pytest.LogCaptureFixture) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + failing_isam2 = mock.MagicMock() + failing_isam2.update.side_effect = RuntimeError("synthetic isam2 failure") + estimator._isam2 = failing_isam2 + + with caplog.at_level(logging.ERROR, logger="c5_state.isam2_handle"): + with pytest.raises(EstimatorFatalError, match="synthetic isam2 failure"): + handle.update(mock.MagicMock(), mock.MagicMock()) + + err_records = [r for r in caplog.records if r.kind == "c5.state.isam2_update_failed"] + assert len(err_records) == 1 + + +def test_ac6_smoother_failure_raises_fatal(caplog: pytest.LogCaptureFixture) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + failing_smoother = mock.MagicMock() + failing_smoother.update.side_effect = RuntimeError("synthetic smoother failure") + estimator._smoother = failing_smoother + graph, values, timestamps = _build_unit_update_payload() + + with caplog.at_level(logging.ERROR, logger="c5_state.isam2_handle"): + with pytest.raises(EstimatorFatalError, match="synthetic smoother failure"): + handle.update(graph, values, timestamps) + + err_records = [r for r in caplog.records if r.kind == "c5.state.smoother_update_failed"] + assert len(err_records) == 1 + + +# --------------------------------------------------------------------- +# AC-7: compute_marginals real body + + +def test_ac7_compute_marginals_returns_real_instance() -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + graph, values, timestamps = _build_unit_update_payload() + handle.update(graph, values, timestamps) + + marginals = handle.compute_marginals() + + assert isinstance(marginals, gtsam.Marginals) + + +# --------------------------------------------------------------------- +# AC-8: last_anchor_age_ms real body + + +def test_ac8_last_anchor_age_ms_huge_when_no_anchor() -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + + age_ms = handle.last_anchor_age_ms() + + # _last_anchor_ns=0 ⇒ age = monotonic_ns()/1e6 (typically 1e9 ms+) + assert age_ms > 1_000_000 + + +def test_ac8_last_anchor_age_ms_small_after_anchor_set() -> None: + import time as _time + + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + estimator._last_anchor_ns = _time.monotonic_ns() + + age_ms = handle.last_anchor_age_ms() + + assert 0 <= age_ms < 100 + + +# --------------------------------------------------------------------- +# AC-9: defensive logging on every mutation (success and failure) + + +def test_ac9_add_factor_emits_success_log(caplog: pytest.LogCaptureFixture) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + key = estimator.key_for_frame(uuid4()) + factor = gtsam.PriorFactorPose3(key, gtsam.Pose3(), gtsam.noiseModel.Isotropic.Sigma(6, 0.1)) + + with caplog.at_level(logging.DEBUG, logger="c5_state.isam2_handle"): + handle.add_factor(factor) + + success_records = [r for r in caplog.records if r.kind == "c5.state.add_factor_ok"] + assert len(success_records) == 1 + + +def test_ac9_update_emits_success_log(caplog: pytest.LogCaptureFixture) -> None: + estimator = _build_estimator() + handle = ISam2GraphHandleImpl(estimator) + graph, values, timestamps = _build_unit_update_payload() + + with caplog.at_level(logging.DEBUG, logger="c5_state.isam2_handle"): + handle.update(graph, values, timestamps) + + success_records = [r for r in caplog.records if r.kind == "c5.state.update_ok"] + assert len(success_records) == 1 + + +# --------------------------------------------------------------------- +# AC-10: StateEstimator Protocol methods still raise NotImplementedError + + +def test_ac10_add_vio_raises_named_az383() -> None: + estimator = _build_estimator() + with pytest.raises(NotImplementedError, match="AZ-383"): + estimator.add_vio(mock.MagicMock()) + + +def test_ac10_add_pose_anchor_raises_named_az383() -> None: + estimator = _build_estimator() + with pytest.raises(NotImplementedError, match="AZ-383"): + estimator.add_pose_anchor(mock.MagicMock()) + + +def test_ac10_add_fc_imu_raises_named_az383() -> None: + estimator = _build_estimator() + with pytest.raises(NotImplementedError, match="AZ-383"): + estimator.add_fc_imu(mock.MagicMock()) + + +def test_ac10_current_estimate_raises_named_az384() -> None: + estimator = _build_estimator() + with pytest.raises(NotImplementedError, match="AZ-384"): + estimator.current_estimate() + + +def test_ac10_smoothed_history_raises_named_az384() -> None: + estimator = _build_estimator() + with pytest.raises(NotImplementedError, match="AZ-384"): + estimator.smoothed_history(n_keyframes=5) + + +def test_ac10_health_snapshot_raises_named_az384() -> None: + estimator = _build_estimator() + with pytest.raises(NotImplementedError, match="AZ-384"): + estimator.health_snapshot() + + +# --------------------------------------------------------------------- +# Factory integration: register() + build_state_estimator returns the tuple + + +def test_register_makes_strategy_available_to_factory() -> None: + register() + cfg = _build_config(strategy="gtsam_isam2") + + estimator, handle = build_state_estimator( + cfg, + imu_preintegrator=mock.MagicMock(), + se3_utils=mock.MagicMock(), + wgs_converter=mock.MagicMock(), + fdr_client=mock.MagicMock(), + ) + + assert isinstance(estimator, GtsamIsam2StateEstimator) + assert isinstance(handle, ISam2GraphHandle) + assert isinstance(handle, ISam2GraphHandleImpl) + + +def test_create_returns_handle_bound_to_returned_estimator() -> None: + cfg = _build_config(strategy="gtsam_isam2") + + estimator, handle = create( + config=cfg, + imu_preintegrator=mock.MagicMock(), + se3_utils=mock.MagicMock(), + wgs_converter=mock.MagicMock(), + fdr_client=mock.MagicMock(), + ) + + assert isinstance(handle, ISam2GraphHandleImpl) + assert handle._estimator is estimator