azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 20: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
_docs/_autodev_state.md
+1 -1
View File
@@ -8,7 +8,7 @@ status: in_progress
sub_step:
phase: 6
name: implement-tasks
detail: "batch 13 of N committed (AZ-382 c5 GtsamIsam2StateEstimator skeleton + ISam2GraphHandleImpl real bodies + defensive logging)"
detail: "batch 14 of N committed (AZ-383 c5 factor adds: add_vio BetweenFactorPose3 + add_pose_anchor mode-dispatch + add_fc_imu CombinedImuFactor with v/b key namespaces)"
retry_count: 0
cycle: 1
tracker: jira