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

[AZ-383] C5 add_vio/add_pose_anchor/add_fc_imu factor adds

Browse Source 
Replaces AZ-382 NotImplementedError placeholders with real GTSAM factor
adds wired against the iSAM2 graph handle:

- add_vio  -> BetweenFactorPose3 between consecutive VIO pose keys
  (first call primes the chain; AZ-388 owns first-keyframe seeding).
- add_pose_anchor -> mode-dispatch per pose.covariance_mode:
  "marginals" -> PriorFactorPose3 + handle.update();
  "jacobian"  -> skip iSAM2 add per AZ-361 contract.
  Both paths bump _last_anchor_ns via time.monotonic_ns().
- add_fc_imu -> shared ImuPreintegrator.integrate_window +
  reset_for_new_keyframe; builds a CombinedImuFactor between the
  prev/curr (X, V, B) keyframe triple. Introduces new 'v' (velocity)
  and 'b' (bias) GTSAM key namespaces decoupled from the VIO/pose
  frame_id mapping.

Invariant 2 - non-decreasing timestamps - enforced per call with
EstimatorDegradedError + c5.state.out_of_order log. Every successful
add emits a structured DEBUG *_ok log; every failure emits a
structured ERROR *_failed log and raises through the C5 error
hierarchy (R05).

Contract-vs-reality fix-ups also landed:

- StateEstimator Protocol: add_fc_imu(ImuWindow) - was incorrectly
  annotated as ImuTelemetrySample by AZ-381.
- _last_anchor_ns semantics switched to monotonic_ns() to match
  last_anchor_age_ms.
- create() factory back-wires the ISam2GraphHandle to the estimator
  via the new attach_handle() method.

Tests: +21 in tests/unit/c5_state/test_az383_factor_adds.py covering
all 8 ACs with mock ISam2GraphHandle instances. Three obsolete
AZ-382 tests (test_ac10_add_*_raises_named_az383) removed. Full
suite: 565 passed, 2 skipped.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 06:07:45 +03:00
parent 8b394a98c6
commit fd848266d1
7 changed files with 955 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/unit/c5_state/test_az382_isam2_smoother_wiring.py
+8 -18
View File
@@ -369,24 +369,14 @@ def test_ac9_update_emits_success_log(caplog: pytest.LogCaptureFixture) -> None:
# ---------------------------------------------------------------------
# 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())
#
# Note: The three ``add_*`` methods USED to raise ``NotImplementedError``
# naming AZ-383 in AZ-382's scope. AZ-383 has since landed and replaced
# those bodies with real factor adds; the now-active behaviour is
# tested in ``tests/unit/c5_state/test_az383_factor_adds.py``. Only the
# three output methods (``current_estimate`` / ``smoothed_history`` /
# ``health_snapshot``) still raise NotImplementedError pointing at the
# next-task AZ-384.
def test_ac10_current_estimate_raises_named_az384() -> None: