Replaces the last three NotImplementedError placeholders on
GtsamIsam2StateEstimator with real Marginals + output methods:
- current_estimate(): recovers the 6x6 Marginals covariance for the
most-recently committed pose key, enforces the SPD invariant via
np.linalg.cholesky (Invariant 10), converts the local-ENU pose
translation to WGS84 via the shared WgsConverter, derives a
body->world quaternion, and emits a fresh EstimatorOutput
(smoothed=False, Invariant 4). On SPD failure transitions
isam2_state -> LOST and raises EstimatorFatalError (AC-5.2 path).
- smoothed_history(n): iterates the smoother's active POSE keys via
_smoother.calculateEstimate().keys() (filtered by GTSAM symbol
char) and the smoother timestamps via ts_map.at(key) - workaround
for the pinned gtsam_unstable build's non-iterable
FixedLagSmootherKeyTimestampMap. Bounded by K (Invariant 6); every
entry has smoothed=True (Invariant 7).
- health_snapshot(): cheap O(1) accumulator read; reports
IsamState lifecycle, pose-key count, AC-NEW-8
cov_norm_growing_for_s rolling 60s deque-backed counter, and
spoof_promotion_blocked via the AZ-385 state machine injection
point.
Adds two public injection points for AZ-385/composition root:
set_enu_origin(LatLonAlt) and attach_source_label_state_machine(machine).
Defaults: (0, 0, 0) ENU origin, VISUAL_PROPAGATED source label,
spoof_promotion_blocked=False.
Wires _record_committed_pose_key into the three add_* success paths
so current_estimate only reads keys that have real values in iSAM2.
The JACOBIAN path in add_pose_anchor deliberately skips this call -
Invariant 3 keeps the JACOBIAN pose out of the iSAM2 graph.
Tests: +27 in tests/unit/c5_state/test_az384_marginals_outputs.py
covering all 10 ACs. Three obsolete AZ-382 tests
(test_ac10_*_raises_named_az384) removed. Full suite: 589 passed,
2 skipped.
Co-authored-by: Cursor <cursoragent@cursor.com>
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>
Oleksandr Bezdieniezhnykh