"""AZ-383 — GtsamIsam2StateEstimator add_vio / add_pose_anchor / add_fc_imu. Eight ACs from ``_docs/02_tasks/done/AZ-383_c5_factor_adds.md``: - AC-1 VIO factor add — ``BetweenFactorPose3`` with correct keys + noise model. - AC-2 Pose-anchor MARGINALS path — ``PriorFactorPose3`` + update, ``_last_anchor_ns`` bumped. - AC-3 Pose-anchor JACOBIAN path SKIPS iSAM2 add but bumps ``_last_anchor_ns`` and emits INFO log. - AC-4 IMU factor add via the shared ``ImuPreintegrator``. - AC-5 Timestamp ordering — out-of-order ``add_*`` raises :class:`EstimatorDegradedError`. - AC-6 Defensive logging on every factor add (R05). - AC-7 Each ``add_*`` triggers ``handle.update()`` exactly once. - AC-8 ``_last_anchor_ns`` accuracy via ``last_anchor_age_ms``. The handle is replaced with a stub on every test so the asserts fire on call counts + factor types rather than iSAM2 convergence (real iSAM2 first-keyframe seeding is AZ-388's responsibility). """ from __future__ import annotations import logging from datetime import datetime, timezone from unittest import mock from uuid import UUID import gtsam import numpy as np import pytest from gps_denied_onboard._types.geo import LatLonAlt from gps_denied_onboard._types.nav import ( FeatureQuality, ImuBias, ImuSample, ImuWindow, VioOutput, ) from gps_denied_onboard._types.pose import ( CovarianceMode, PoseEstimate, PoseSourceLabel, Quat, ) from gps_denied_onboard.components.c5_state.config import C5StateConfig from gps_denied_onboard.components.c5_state.errors import EstimatorDegradedError from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import ( GtsamIsam2StateEstimator, create, ) from gps_denied_onboard.runtime_root.state_factory import clear_state_registry @pytest.fixture(autouse=True) def _registry_isolation(): # Arrange clear_state_registry() yield clear_state_registry() def _build_estimator(*, with_stub_handle: bool = True) -> GtsamIsam2StateEstimator: block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15) cfg = mock.MagicMock() cfg.components = {"c5_state": block} estimator, _real_handle = create( config=cfg, imu_preintegrator=mock.MagicMock(), se3_utils=mock.MagicMock(), wgs_converter=mock.MagicMock(), fdr_client=mock.MagicMock(), ) if with_stub_handle: estimator._isam2_handle = mock.MagicMock() return estimator _ZERO_BIAS = ImuBias(accel_bias=(0.0, 0.0, 0.0), gyro_bias=(0.0, 0.0, 0.0)) _NEUTRAL_FQ = FeatureQuality( tracked=20, new=2, lost=1, mean_parallax=5.0, mre_px=1.0, ) def _make_vio(*, frame_id: int, t_seconds: float, pose: np.ndarray | None = None) -> VioOutput: matrix = pose if pose is not None else np.eye(4) return VioOutput( frame_id=str(frame_id), relative_pose_T=gtsam.Pose3(matrix), pose_covariance_6x6=np.eye(6) * 0.01, imu_bias=_ZERO_BIAS, feature_quality=_NEUTRAL_FQ, emitted_at_ns=int(t_seconds * 1_000_000_000), ) def _make_pose( *, frame_id: int, t_seconds: float, covariance_mode: CovarianceMode | str = CovarianceMode.MARGINALS, ) -> PoseEstimate: mode = ( covariance_mode if isinstance(covariance_mode, CovarianceMode) else CovarianceMode(covariance_mode.lower()) ) return PoseEstimate( frame_id=UUID(int=frame_id), position_wgs84=LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0), orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0), covariance_6x6=np.eye(6) * 0.01, covariance_mode=mode, source_label=PoseSourceLabel.SATELLITE_ANCHORED, last_satellite_anchor_age_ms=0, emitted_at=int(t_seconds * 1_000_000_000), ) def _make_imu_window(*, ts_start_ns: int, ts_end_ns: int) -> ImuWindow: samples = ( ImuSample(ts_ns=ts_start_ns, accel_xyz=(0.0, 0.0, 9.81), gyro_xyz=(0.0, 0.0, 0.0)), ImuSample(ts_ns=ts_end_ns, accel_xyz=(0.0, 0.0, 9.81), gyro_xyz=(0.0, 0.0, 0.0)), ) return ImuWindow(samples=samples, ts_start_ns=ts_start_ns, ts_end_ns=ts_end_ns) # --------------------------------------------------------------------- # AC-1: VIO factor add — BetweenFactorPose3 with correct keys + noise def test_ac1_vio_first_call_records_without_factor() -> None: estimator = _build_estimator() vio = _make_vio(frame_id=1, t_seconds=1000.0) estimator.add_vio(vio) # First call MUST NOT emit a factor or update — there's no prev frame. estimator._isam2_handle.add_factor.assert_not_called() estimator._isam2_handle.update.assert_not_called() assert estimator._prev_vio is vio def test_ac1_vio_second_call_adds_between_factor() -> None: estimator = _build_estimator() vio1 = _make_vio(frame_id=1, t_seconds=1000.0) vio2 = _make_vio(frame_id=2, t_seconds=1001.0) estimator.add_vio(vio1) estimator.add_vio(vio2) estimator._isam2_handle.add_factor.assert_called_once() factor = estimator._isam2_handle.add_factor.call_args[0][0] assert isinstance(factor, gtsam.BetweenFactorPose3) # Keys should be the consecutive x-symbols assert factor.keys()[0] == gtsam.symbol("x", 0) assert factor.keys()[1] == gtsam.symbol("x", 1) # --------------------------------------------------------------------- # AC-2: Pose-anchor MARGINALS path def test_ac2_pose_anchor_marginals_adds_prior_factor_and_updates() -> None: estimator = _build_estimator() pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="marginals") estimator.add_pose_anchor(pose) estimator._isam2_handle.add_factor.assert_called_once() factor = estimator._isam2_handle.add_factor.call_args[0][0] assert isinstance(factor, gtsam.PriorFactorPose3) estimator._isam2_handle.update.assert_called_once() def test_ac2_pose_anchor_marginals_bumps_last_anchor_ns() -> None: estimator = _build_estimator() pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="marginals") before_ns = estimator._last_anchor_ns estimator.add_pose_anchor(pose) after_ns = estimator._last_anchor_ns assert after_ns > before_ns assert after_ns > 0 # --------------------------------------------------------------------- # AC-3: Pose-anchor JACOBIAN path SKIPS factor add but bumps _last_anchor_ns def test_ac3_pose_anchor_jacobian_skips_factor_add(caplog: pytest.LogCaptureFixture) -> None: estimator = _build_estimator() pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian") with caplog.at_level(logging.INFO): estimator.add_pose_anchor(pose) estimator._isam2_handle.add_factor.assert_not_called() estimator._isam2_handle.update.assert_not_called() skip_records = [r for r in caplog.records if r.kind == "c5.state.skip_isam2_jacobian_path"] assert len(skip_records) == 1 def test_ac3_pose_anchor_jacobian_still_bumps_last_anchor_ns() -> None: estimator = _build_estimator() pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian") before_ns = estimator._last_anchor_ns estimator.add_pose_anchor(pose) assert estimator._last_anchor_ns > before_ns # --------------------------------------------------------------------- # AC-4: IMU factor add via shared ImuPreintegrator def test_ac4_imu_first_window_primes_chain_no_factor() -> None: estimator = _build_estimator() # Stub preintegrator returns a real PIM-like sentinel estimator._imu_preintegrator.reset_for_new_keyframe.return_value = mock.MagicMock() window = _make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000) estimator.add_fc_imu(window) estimator._imu_preintegrator.integrate_window.assert_called_once_with(window) estimator._imu_preintegrator.reset_for_new_keyframe.assert_called_once() # First window primes the chain — no factor yet estimator._isam2_handle.add_factor.assert_not_called() estimator._isam2_handle.update.assert_not_called() def test_ac4_imu_second_window_adds_combined_imu_factor() -> None: estimator = _build_estimator() estimator._imu_preintegrator.reset_for_new_keyframe.return_value = ( gtsam.PreintegratedCombinedMeasurements( gtsam.PreintegrationCombinedParams.MakeSharedU(9.81), gtsam.imuBias.ConstantBias(), ) ) win1 = _make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000) win2 = _make_imu_window(ts_start_ns=2_000_001, ts_end_ns=3_000_000) estimator.add_fc_imu(win1) estimator.add_fc_imu(win2) assert estimator._imu_preintegrator.integrate_window.call_count == 2 estimator._isam2_handle.add_factor.assert_called_once() factor = estimator._isam2_handle.add_factor.call_args[0][0] assert isinstance(factor, gtsam.CombinedImuFactor) # --------------------------------------------------------------------- # AC-5: Timestamp ordering def test_ac5_vio_out_of_order_raises_degraded() -> None: estimator = _build_estimator() later = _make_vio(frame_id=1, t_seconds=1001.0) earlier = _make_vio(frame_id=2, t_seconds=1000.0) estimator.add_vio(later) with pytest.raises(EstimatorDegradedError, match="out-of-order vio"): estimator.add_vio(earlier) def test_ac5_pose_anchor_out_of_order_raises_degraded() -> None: estimator = _build_estimator() later = _make_pose(frame_id=10, t_seconds=1001.0) earlier = _make_pose(frame_id=11, t_seconds=1000.0) estimator.add_pose_anchor(later) with pytest.raises(EstimatorDegradedError, match="out-of-order pose_anchor"): estimator.add_pose_anchor(earlier) def test_ac5_imu_out_of_order_raises_degraded() -> None: estimator = _build_estimator() estimator._imu_preintegrator.reset_for_new_keyframe.return_value = mock.MagicMock() later = _make_imu_window(ts_start_ns=2_000_000, ts_end_ns=3_000_000) earlier = _make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000) estimator.add_fc_imu(later) with pytest.raises(EstimatorDegradedError, match="out-of-order imu_window"): estimator.add_fc_imu(earlier) def test_ac5_emits_out_of_order_log(caplog: pytest.LogCaptureFixture) -> None: estimator = _build_estimator() later = _make_vio(frame_id=1, t_seconds=1001.0) earlier = _make_vio(frame_id=2, t_seconds=1000.0) estimator.add_vio(later) with caplog.at_level(logging.ERROR): with pytest.raises(EstimatorDegradedError): estimator.add_vio(earlier) err_records = [r for r in caplog.records if r.kind == "c5.state.out_of_order"] assert len(err_records) == 1 assert err_records[0].kv["source"] == "vio" # --------------------------------------------------------------------- # AC-6: Defensive logging on every factor add (R05) def test_ac6_vio_success_emits_debug_log(caplog: pytest.LogCaptureFixture) -> None: estimator = _build_estimator() vio1 = _make_vio(frame_id=1, t_seconds=1000.0) vio2 = _make_vio(frame_id=2, t_seconds=1001.0) with caplog.at_level(logging.DEBUG): estimator.add_vio(vio1) estimator.add_vio(vio2) ok_records = [r for r in caplog.records if r.kind == "c5.state.add_vio_ok"] assert len(ok_records) == 1 def test_ac6_pose_anchor_success_emits_debug_log(caplog: pytest.LogCaptureFixture) -> None: estimator = _build_estimator() pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="marginals") with caplog.at_level(logging.DEBUG): estimator.add_pose_anchor(pose) ok_records = [r for r in caplog.records if r.kind == "c5.state.add_pose_anchor_ok"] assert len(ok_records) == 1 def test_ac6_vio_failure_emits_error_log_and_raises(caplog: pytest.LogCaptureFixture) -> None: estimator = _build_estimator() # First call records the prev_vio without going through add_factor. estimator.add_vio(_make_vio(frame_id=1, t_seconds=1000.0)) # Make the handle fail. estimator._isam2_handle.add_factor.side_effect = RuntimeError("synthetic") with caplog.at_level(logging.ERROR): with pytest.raises(EstimatorDegradedError, match="synthetic"): estimator.add_vio(_make_vio(frame_id=2, t_seconds=1001.0)) err_records = [r for r in caplog.records if r.kind == "c5.state.add_vio_failed"] assert len(err_records) == 1 # --------------------------------------------------------------------- # AC-7: Each add_* triggers handle.update() exactly once def test_ac7_vio_triggers_update_once() -> None: estimator = _build_estimator() estimator.add_vio(_make_vio(frame_id=1, t_seconds=1000.0)) estimator.add_vio(_make_vio(frame_id=2, t_seconds=1001.0)) # Only the second call (which adds a factor) triggers update. assert estimator._isam2_handle.update.call_count == 1 def test_ac7_pose_anchor_marginals_triggers_update_once() -> None: estimator = _build_estimator() estimator.add_pose_anchor(_make_pose(frame_id=10, t_seconds=1000.0)) assert estimator._isam2_handle.update.call_count == 1 def test_ac7_pose_anchor_jacobian_triggers_no_update() -> None: estimator = _build_estimator() estimator.add_pose_anchor(_make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian")) assert estimator._isam2_handle.update.call_count == 0 def test_ac7_imu_second_window_triggers_update_once() -> None: estimator = _build_estimator() estimator._imu_preintegrator.reset_for_new_keyframe.return_value = ( gtsam.PreintegratedCombinedMeasurements( gtsam.PreintegrationCombinedParams.MakeSharedU(9.81), gtsam.imuBias.ConstantBias(), ) ) estimator.add_fc_imu(_make_imu_window(ts_start_ns=1_000_000, ts_end_ns=2_000_000)) estimator.add_fc_imu(_make_imu_window(ts_start_ns=2_000_001, ts_end_ns=3_000_000)) assert estimator._isam2_handle.update.call_count == 1 # --------------------------------------------------------------------- # AC-8: _last_anchor_ns accuracy via last_anchor_age_ms def test_ac8_last_anchor_age_ms_after_anchor_is_small() -> None: block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15) cfg = mock.MagicMock() cfg.components = {"c5_state": block} estimator, real_handle = create( config=cfg, imu_preintegrator=mock.MagicMock(), se3_utils=mock.MagicMock(), wgs_converter=mock.MagicMock(), fdr_client=mock.MagicMock(), ) # Use the JACOBIAN path so we exercise the _last_anchor_ns bump # without needing real iSAM2 seeding (AZ-388's territory). pose = _make_pose(frame_id=10, t_seconds=1000.0, covariance_mode="jacobian") estimator.add_pose_anchor(pose) age_ms = real_handle.last_anchor_age_ms() assert 0 <= age_ms < 1000 # --------------------------------------------------------------------- # Cross-cutting: estimator requires a handle def test_no_handle_raises_state_estimator_config_error() -> None: block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15) cfg = mock.MagicMock() cfg.components = {"c5_state": block} # Construct directly without attach_handle. estimator = GtsamIsam2StateEstimator( cfg, imu_preintegrator=mock.MagicMock(), se3_utils=mock.MagicMock(), wgs_converter=mock.MagicMock(), fdr_client=mock.MagicMock(), ) from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError with pytest.raises(StateEstimatorConfigError, match="not attached"): estimator.add_vio(_make_vio(frame_id=1, t_seconds=1000.0))