mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 07:01:14 +00:00
[AZ-382] C5 GtsamIsam2StateEstimator skeleton + real iSAM2 handle bodies
- Add GtsamIsam2StateEstimator owning the GTSAM substrate:
gtsam.ISAM2(ISAM2Params()) + gtsam_unstable.IncrementalFixedLagSmoother
(K * 1/3 s window per D-C5-3) + NonlinearFactorGraph + Values.
- Module-level create(...) factory + register() helper for
register_state_estimator("gtsam_isam2", create). Opt-in registration
per ADR-002 — no auto-import.
- Key-management policy: key_for_frame(UUID) -> int via
gtsam.symbol('x', counter); idempotent re-lookup.
- Replace all four NotImplementedError bodies in _isam2_handle.py with
real GTSAM calls:
* add_factor → estimator._graph.add(factor); R05 defensive logging
on success/failure; EstimatorDegradedError on failure.
* update → _isam2.update + _smoother.update; empty
FixedLagSmootherKeyTimestampMap substituted for timestamps=None;
EstimatorFatalError on either failure.
* compute_marginals → gtsam.Marginals(getFactorsUnsafe(),
calculateEstimate()).
* last_anchor_age_ms → (monotonic_ns - _last_anchor_ns) // 1e6.
- StateEstimator Protocol methods on the estimator still raise
NotImplementedError naming AZ-383 (factor adds) / AZ-384
(marginals + outputs).
- AZ-382 AC tests: 27 cases covering 10/10 ACs + factory integration.
- AZ-381 test_ac8_handle_methods_raise_named_task removed (obsolete:
bodies are real now); test_ac8_handle_is_isam2_graph_handle retained.
- Full suite: 547 passed (+26 vs B12), 2 skipped.
- Impl report: _docs/03_implementation/batch_13_cycle1_report.md.
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -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=<name>)` 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.
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -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.).
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user