mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 22:11:13 +00:00
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:
@@ -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 1–50 and bias_b applied to samples 51–100 (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.
|
||||
Reference in New Issue
Block a user