mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 03:31:13 +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:
@@ -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:
|
sub_step:
|
||||||
phase: 6
|
phase: 6
|
||||||
name: implement-tasks
|
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
|
retry_count: 0
|
||||||
cycle: 1
|
cycle: 1
|
||||||
tracker: jira
|
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`` /
|
C4 (``OpenCVGtsamPoseEstimator``) calls ``add_factor`` / ``update`` /
|
||||||
``compute_marginals`` against this handle, NOT against C5 directly —
|
``compute_marginals`` against this handle, NOT against C5 directly —
|
||||||
ADR-003 says C5 owns the graph; this handle is the typed seam C4 uses
|
ADR-003 says C5 owns the graph; this handle is the typed seam C4 uses
|
||||||
to drive it without importing C5 internals.
|
to drive it without importing C5 internals.
|
||||||
|
|
||||||
AZ-381 ships the skeleton: every method raises
|
AZ-381 shipped the Protocol surface and a NotImplementedError
|
||||||
``NotImplementedError("Body owned by AZ-382 iSAM2 wiring task")``. The
|
skeleton; AZ-382 replaces the four method bodies with the real GTSAM
|
||||||
``NotImplementedError`` messages name AZ-382 so the next task's
|
calls against the estimator's ``_isam2`` + ``_smoother`` instances.
|
||||||
implementer can grep for them.
|
The Protocol surface does not change between AZ-381 and AZ-382.
|
||||||
|
|
||||||
AZ-382 replaces the four method bodies with the real GTSAM calls
|
Defensive logging (R05 mitigation): every mutation logs SUCCESS or
|
||||||
against the C5 estimator's ``_isam2`` + ``_smoother`` instances. The
|
FAILURE with structured fields. Silent factor-add failures bit this
|
||||||
Protocol surface is stable from AZ-381 onward.
|
codebase during the prototype — the contract now mandates the
|
||||||
|
defensive trace.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import time
|
||||||
from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable
|
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:
|
if TYPE_CHECKING:
|
||||||
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
|
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
|
||||||
GtsamIsam2StateEstimator,
|
GtsamIsam2StateEstimator,
|
||||||
@@ -48,37 +59,135 @@ class ISam2GraphHandle(Protocol):
|
|||||||
|
|
||||||
|
|
||||||
class ISam2GraphHandleImpl(ISam2GraphHandle):
|
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
|
Every mutation is wrapped in success/failure logging per R05;
|
||||||
root that produces a concrete handle reference for C4 to inject
|
individual failures are translated into the C5 error hierarchy
|
||||||
against (per ADR-009). AZ-382 replaces every body with the real
|
(``EstimatorDegradedError`` for recoverable graph-add issues,
|
||||||
GTSAM calls; the Protocol surface does not change.
|
``EstimatorFatalError`` for solver failures the calling thread
|
||||||
|
cannot recover from).
|
||||||
"""
|
"""
|
||||||
|
|
||||||
def __init__(self, estimator: GtsamIsam2StateEstimator) -> None:
|
def __init__(self, estimator: GtsamIsam2StateEstimator) -> None:
|
||||||
self._estimator = estimator
|
self._estimator = estimator
|
||||||
|
self._log = get_logger("c5_state.isam2_handle")
|
||||||
|
|
||||||
def add_factor(self, factor: Any) -> None:
|
def add_factor(self, factor: Any) -> None:
|
||||||
raise NotImplementedError(
|
"""Append ``factor`` to the pending ``NonlinearFactorGraph``.
|
||||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
|
||||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
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:
|
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None:
|
||||||
raise NotImplementedError(
|
"""Apply ``(graph, values)`` to iSAM2 AND advance the smoother.
|
||||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
|
||||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
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:
|
def compute_marginals(self) -> Any:
|
||||||
raise NotImplementedError(
|
"""Return a ``gtsam.Marginals`` snapshot of the current iSAM2 state.
|
||||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
|
||||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
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:
|
def last_anchor_age_ms(self) -> int:
|
||||||
raise NotImplementedError(
|
"""Milliseconds since the last satellite-anchored pose was added.
|
||||||
"Body owned by AZ-382 iSAM2 wiring task — "
|
|
||||||
"this skeleton is intentionally inert until iSAM2 wiring lands."
|
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)
|
assert isinstance(handle, ISam2GraphHandle)
|
||||||
|
|
||||||
|
|
||||||
def test_ac8_handle_methods_raise_named_task() -> None:
|
# Note: the AZ-381 skeleton's ``NotImplementedError`` bodies were
|
||||||
handle = ISam2GraphHandleImpl(estimator=mock.MagicMock())
|
# replaced with real GTSAM calls by AZ-382. The "methods raise" test
|
||||||
|
# that lived here has moved to
|
||||||
with pytest.raises(NotImplementedError, match="AZ-382"):
|
# ``tests/unit/c5_state/test_az382_isam2_smoother_wiring.py``, which
|
||||||
handle.add_factor(mock.MagicMock())
|
# now asserts the real behaviour (``add_factor`` grows the graph,
|
||||||
with pytest.raises(NotImplementedError, match="AZ-382"):
|
# ``update`` advances iSAM2 + the smoother, etc.).
|
||||||
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()
|
|
||||||
|
|
||||||
|
|
||||||
# ----------------------------------------------------------------------
|
# ----------------------------------------------------------------------
|
||||||
|
|||||||
@@ -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