[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:
Oleksandr Bezdieniezhnykh
2026-05-19 17:33:59 +03:00
parent 12aba8139f
commit 4fdf1968af
6 changed files with 71 additions and 5 deletions
@@ -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.