mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 11:51:14 +00:00
[autodev] Step 13 partial: helpers 1-4 cycle-1 doc sync
Batch 5a of the cycle-1 doc sync. For each of the four foundation helpers (imu_preintegrator, se3_utils, lightglue_runtime, wgs_converter): - Append "Cycle-1 operational reality" section to the existing common-helpers/<NN>_*.md, documenting what the shipped implementation actually exposes vs. the design- intent sketch (interfaces, exception types, public constants, AZ-task lineage). Specific cycle-1 facts captured per helper: - imu_preintegrator (AZ-276): make_imu_preintegrator factory, BMI088-class noise defaults, single ImuPreintegrationError exception, actual return type is PreintegratedCombinedMeasurements (consumer builds the CombinedImuFactor), destructive reset_with_bias semantics, first-sample-not-integrated dt=0 handling. - se3_utils (AZ-277): SE3 = gtsam.Pose3 re-export, Se3InvalidMatrixError, strict caller-orthogonalisation invariant, _DEFAULT_ROT_ATOL=1e-6 and small-angle Taylor cutoff for exp_map, is_valid_rotation predicate, strict dtype=float64 everywhere. - lightglue_runtime (AZ-278 / R14 fix): EngineHandle Protocol-typed constructor, LightGlueRuntimeError + LightGlueConcurrentAccessError, non-blocking concurrent- access guard (raises rather than serialises), match_batch equal-length precondition, composition-root single-instance into C2.5 + C3. - wgs_converter (AZ-279 + AZ-490): WEB_MERCATOR_MAX_LAT_DEG and MAX_ZOOM constants, WgsConversionError, ECEF arrays are ndarray(3,) float64, new horizontal_distance_m method (AZ-490 takeoff-origin bounded-delta gate), slippy-map tile math hand-rolled to match satellite-provider on-disk layout. Two contract files (imu_preintegrator.md and wgs_converter.md) need follow-up minor revisions to match shipped surface; queued for the next contracts-folder sweep, noted inline in each helper's new section. Also refresh D-CROSS-CVE-1 opencv-pin leftover replay timestamp (8-min debounce — gtsam upstream state cannot change in that window). Bumps _docs/_autodev_state.md sub_step detail. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -28,3 +28,19 @@ def adjoint(pose: SE3) -> Matrix6
|
||||
## Caveats
|
||||
|
||||
- Library-grade Lie-algebra functions exist in `manifpy` and `pylie`; we use GTSAM's primitives directly to avoid pulling in a second math library. If a future strategy needs richer manifold ops, evaluate `manifpy` then.
|
||||
|
||||
## Cycle-1 operational reality
|
||||
|
||||
The shipped surface in `src/gps_denied_onboard/helpers/se3_utils.py` (AZ-277) extends the sketch above; this section is the authoritative inventory of what cycle-1 consumers actually see.
|
||||
|
||||
- **Type alias** — `SE3 = gtsam.Pose3` is re-exported by the helper. Consumers MUST import `SE3` from `helpers.se3_utils` and never `gtsam.Pose3` directly (keeps the Lie-algebra backend swappable without touching C1/C4/C5).
|
||||
- **`Se3InvalidMatrixError`** — single public exception type. Raised on (a) wrong array shape, (b) `dtype != float64`, (c) bottom row != `[0, 0, 0, 1]`, (d) rotation drift `‖R^TR − I‖_F > atol`, (e) negative-determinant rotation (mirror), (f) non-ndarray inputs. `matrix_to_se3` and `exp_map` raise this; `se3_to_matrix`, `log_map`, `adjoint` are no-throw on the typed input.
|
||||
- **Strict caller-orthogonalisation invariant** — the helper does NOT silently re-orthogonalise. AC-7 / `matrix_to_se3` always validates `‖R^TR − I‖_F ≤ atol` and rejects drift. Callers (C4 in particular, since `solvePnPRansac` output is not orthogonal to numerical precision) MUST run their own orthogonalisation (`cv2.Rodrigues` round-trip or `scipy.linalg.polar`) before calling `matrix_to_se3`. Default tolerance: `_DEFAULT_ROT_ATOL = 1e-6`; callers can pass a looser `atol` for relaxed contexts (none in cycle-1).
|
||||
- **`exp_map` near-identity fallback** — twist vectors with `‖xi‖ < _SMALL_ANGLE_THRESHOLD = 1e-10` return the identity `SE3()` instead of delegating to GTSAM's `Pose3.Expmap`. This guards against the `sin(theta)/theta` under-flow that surfaces when iSAM2's relinearisation produces a near-identity twist after a converged step.
|
||||
- **`is_valid_rotation(R_3x3, *, atol=1e-6)`** — predicate (no exception) for "is this matrix safe to feed to `matrix_to_se3`?". Returns False for non-ndarray, wrong shape, wrong dtype, orthogonality drift > atol, or negative determinant. Cycle-1 consumers: C4's `MarginalsAdapter` short-circuit (`opencv_gtsam_marginals.py` from AZ-358) and the contract test for AC-7.
|
||||
- **`dtype=float64` everywhere** — every public function enforces `float64`. `np.ndarray` returned from `se3_to_matrix`, `log_map`, `adjoint` is `np.ascontiguousarray(..., dtype=np.float64)` so callers can pass it through to GTSAM/Eigen without a copy.
|
||||
|
||||
### Cycle-1 task lineage
|
||||
|
||||
- AZ-277 — initial helper, contract producer.
|
||||
- No cycle-1 follow-up tasks touched this helper.
|
||||
|
||||
Reference in New Issue
Block a user