Decompose Step 6 snapshot: 140 task specs + contract docs

Closes out greenfield Step 6 (Decompose) for all 14 components
(C1-C13 + cross-cutting helpers/replay). Covers tasks AZ-266..AZ-446
plus the _dependencies_table.md and component contract documents.

State file updated to greenfield Step 7 (Implement), not_started.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 00:39:48 +03:00
parent 8171fcb29e
commit 880eabcb3f
172 changed files with 22897 additions and 35 deletions
@@ -0,0 +1,138 @@
# ImuPreintegrator Helper Module
**Task**: AZ-276_imu_preintegrator
**Name**: ImuPreintegrator Helper
**Description**: Implement the shared `ImuPreintegrator` helper that wraps GTSAM's `PreintegrationCombinedParams` + `PreintegratedCombinedMeasurements` so C1 (VIO) and C5 (StateEstimator) consume one canonical preintegration of every FC IMU window. Single-threaded by design; one instance per writer thread, bound by the composition root. Bias drift remains the consumer's responsibility.
**Complexity**: 2 points
**Dependencies**: AZ-263_initial_structure
**Component**: shared.helpers.imu_preintegrator (cross-cutting; epic AZ-264 / E-CC-HELPERS)
**Tracker**: AZ-276
**Epic**: AZ-264 (E-CC-HELPERS)
### Document Dependencies
- `_docs/02_document/contracts/shared_helpers/imu_preintegrator.md` — frozen public interface this task produces.
- `_docs/02_document/common-helpers/01_helper_imu_preintegrator.md` — design rationale and consumer mapping.
## Problem
C1's VIO loop and C5's state estimator both consume the same FC IMU window every keyframe. Without a shared preintegrator:
- They drift into two slightly-different integrations of the same physical IMU stream (different sample-rejection rules, different bias-application order).
- The GTSAM `CombinedImuFactor` shape that goes into C5's iSAM2 graph diverges from the one C1 uses for its own pose update, breaking the "single source of IMU truth" invariant in `solution.md`.
- Per-deployment IMU noise covariances (which live in `CameraCalibration`) get parsed twice, with subtle unit differences.
## Outcome
- A single `ImuPreintegrator` is the only path through which any onboard process integrates IMU samples for a GTSAM `CombinedImuFactor`. C1 and C5 import it; nothing else does.
- The composition root binds ONE instance per writer thread; the helper's contract test confirms it does not acquire any locks (so no surprise serialisation under load).
- Sample monotonicity is enforced — non-monotonic samples raise `ImuPreintegrationError` before any state is mutated.
- Re-bias is explicit: `reset_with_bias` is called by consumers when their bias estimate changes; the helper never re-estimates bias internally.
## Scope
### Included
- `ImuPreintegrator` class + factory `make_imu_preintegrator(calibration: CameraCalibration) -> ImuPreintegrator`.
- Integration entrypoints: `integrate_sample(ImuSample)`, `integrate_window(ImuWindow)`.
- Factor accessors: `current_preintegration() -> CombinedImuFactor`, `reset_for_new_keyframe() -> CombinedImuFactor` (destructive).
- Bias control: `reset_with_bias(ImuBias) -> None`.
- `ImuPreintegrationError` exception type re-exported alongside the helper.
- Re-export of GTSAM's `CombinedImuFactor` (or a thin alias) so consumers do not import GTSAM directly.
- Public interface contract published at `_docs/02_document/contracts/shared_helpers/imu_preintegrator.md`.
### Excluded
- IMU sample acquisition / FC adapter integration — C8.
- Bias estimation / re-bias logic — C1, C5.
- Multi-threaded sample feeding — out of contract; helper is single-thread by design.
- Serialising preintegrated factors to FDR records — C13.
- The ImuSample / ImuWindow / ImuBias DTOs themselves — owned by `_types/nav.py` (AZ-263).
## Acceptance Criteria
**AC-1: Round-trip preintegration**
Given a synthetic IMU sequence of 100 samples with strictly-monotonic `ts_ns`
When the producer calls `integrate_sample` 100 times then `current_preintegration()`
Then a `CombinedImuFactor` is returned whose `deltaTij` equals the time span and whose `delta_pose` is non-zero
**AC-2: Strict monotonicity rejects non-monotonic samples**
Given a preintegrator with the last integrated sample at `ts_ns = T`
When `integrate_sample(sample)` is called with `sample.ts_ns <= T`
Then `ImuPreintegrationError` is raised AND the preintegrator's internal accumulators are unchanged (a subsequent valid sample integrates as if the bad one never came)
**AC-3: `reset_for_new_keyframe` is destructive**
Given a preintegrator with N integrated samples
When `reset_for_new_keyframe()` is called
Then the returned factor reflects all N samples AND a subsequent `current_preintegration()` (with no further samples) raises `ImuPreintegrationError`
**AC-4: Re-bias affects subsequent samples only**
Given a sequence: `reset_with_bias(bias_a)`, integrate 50 samples, `reset_with_bias(bias_b)`, integrate 50 more
When `current_preintegration()` is called
Then the resulting factor reflects bias_a applied to samples 150 and bias_b applied to samples 51100 (not bias_b retroactively)
**AC-5: Determinism**
Given two instances constructed from the same calibration and fed the same `(bias, samples)` sequence
When both call `current_preintegration()`
Then the outputs are deep-equal
**AC-6: Single-threaded, lock-free**
Given the helper's source code
When inspected by the contract test (static analysis OR runtime reflection)
Then no `threading.Lock`, `RLock`, `Semaphore`, or `mutex` is acquired anywhere in the integration path
**AC-7: No upward imports (Layer 1 invariant)**
Given the helper module
When a static-import check runs across `gps_denied_onboard.helpers.imu_preintegrator`
Then it imports ONLY from `_types`, GTSAM, numpy, and stdlib — no `gps_denied_onboard.components.*` imports anywhere
## Non-Functional Requirements
**Performance**
- `integrate_sample` p99 ≤ 200 µs on Tier-2 (Jetson Orin Nano Super) — overhead vs. inline GTSAM PIM ≤ 5 % (per E-CC-HELPERS hot-path NFR).
- `current_preintegration` p99 ≤ 100 µs on the same hardware.
**Reliability**
- Pure deterministic: same inputs → byte-equal `CombinedImuFactor` outputs.
- `ImuPreintegrationError` is the ONLY exception type the public surface raises on schema/timestamp violation; GTSAM's lower-level exceptions MUST be wrapped.
## Unit Tests
| AC Ref | What to Test | Required Outcome |
|--------|-------------|-----------------|
| AC-1 | 100 monotonic samples → `current_preintegration()` | factor `deltaTij` ≈ time span; non-zero `delta_pose` |
| AC-2 | non-monotonic sample injection | `ImuPreintegrationError`; internal state unchanged (next valid sample integrates correctly) |
| AC-3 | `reset_for_new_keyframe` then `current_preintegration` | second call raises `ImuPreintegrationError` (state cleared) |
| AC-4 | re-bias mid-window | resulting factor distinguishes bias_a vs bias_b epochs |
| AC-5 | two instances, same input | deep-equal factor outputs |
| AC-6 | static / runtime lock check | no lock acquisition on the integration path |
| AC-7 | importlinter / grep gate | no `gps_denied_onboard.components.*` imports |
| NFR-perf | microbench `integrate_sample` (10k iterations on Tier-2 fixture) | p99 ≤ 200 µs; overhead ≤ 5 % |
## Constraints
- Public surface frozen by `_docs/02_document/contracts/shared_helpers/imu_preintegrator.md` v1.0.0.
- Layer 1 Foundation only (per `module-layout.md` § Allowed Dependencies). NO upward imports.
- GTSAM is the single math backend — do not introduce a second IMU-preintegration library.
- No new dependency beyond what AZ-263 / E-BOOT pinned.
## Risks & Mitigation
**Risk 1: Concurrent calls from a misconfigured composition root silently corrupt the GTSAM PIM accumulator**
- *Risk*: Two threads call `integrate_sample` simultaneously; GTSAM's PIM is not thread-safe; numerical drift goes undiagnosed.
- *Mitigation*: Helper is single-threaded by contract; the composition root binds one instance per writer thread. The contract test (AC-6) asserts no internal locking — making it a hard error if a future change tries to "make it thread-safe" instead of fixing the composition.
**Risk 2: Sample-monotonicity-rejection silently masks an upstream FC clock skew**
- *Risk*: A real IMU stream produces a non-monotonic sample (clock jitter); the helper rejects it; the consumer never learns.
- *Mitigation*: `ImuPreintegrationError` carries the offending vs. previous timestamp in its message so the consumer's catch-and-log path can record it as an FDR `kind="imu.skew"` event.
## Runtime Completeness
- **Named capability**: GTSAM `CombinedImuFactor` preintegration via `PreintegrationCombinedParams` + `PreintegratedCombinedMeasurements` (architecture / E-CC-HELPERS / `01_helper_imu_preintegrator.md`).
- **Production code that must exist**: real GTSAM-backed integration; real noise-model parsing from `CameraCalibration`; real strict-monotonic guard.
- **Allowed external stubs**: none — GTSAM is the production runtime.
- **Unacceptable substitutes**: pure-numpy "approximate" preintegration that ignores GTSAM's covariance propagation; deterministic-fallback that returns a zero factor; "for now we just integrate position with Euler" placeholder. Each would silently break C5's iSAM2 covariance honesty (AC-NEW-4).
## Contract
This task produces the contract at `_docs/02_document/contracts/shared_helpers/imu_preintegrator.md`.
Consumers MUST read that file — not this task spec — to discover the interface.