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

[AZ-382] C5 GtsamIsam2StateEstimator skeleton + real iSAM2 handle bodies

Browse Source 
- 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
2026-05-11 05:51:23 +03:00
parent beed43724f
commit 8b394a98c6
7 changed files with 899 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/components/c5_state/_isam2_handle.py
+135 -26
View File
@@ -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
src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py
+230
View File
@@ -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)