mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 08:31: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:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,78 @@
|
||||
# Contract: se3_utils
|
||||
|
||||
**Component**: shared_helpers / `helpers.se3_utils` (cross-cutting concern owned by E-CC-HELPERS / AZ-264)
|
||||
**Producer task**: AZ-277 — `_docs/02_tasks/todo/AZ-277_se3_utils.md`
|
||||
**Consumer tasks**: every C1 VIO task that produces relative poses, every C2.5 / C3 / C3.5 task that handles 4x4 → SE(3) conversion, every C4 task that converts `solvePnPRansac` output into a GTSAM factor, every C5 task that builds iSAM2 graph keys, every C8 task that encodes pose for FC emission
|
||||
**Version**: 1.0.0
|
||||
**Status**: draft
|
||||
**Last Updated**: 2026-05-10
|
||||
|
||||
## Purpose
|
||||
|
||||
Centralise SE(3) ↔ 4×4-matrix conversion and Lie-algebra exponential / logarithm / adjoint so every component that crosses the matrix-vs-pose boundary uses the same numerical convention. Per `_docs/02_document/common-helpers/02_helper_se3_utils.md`. Backed by GTSAM `Pose3` primitives where available; pure numpy fallback otherwise.
|
||||
|
||||
## Shape
|
||||
|
||||
### For function / method APIs
|
||||
|
||||
```python
|
||||
def matrix_to_se3(T_4x4: np.ndarray) -> SE3: ...
|
||||
def se3_to_matrix(pose: SE3) -> np.ndarray: ...
|
||||
def exp_map(xi: np.ndarray) -> SE3: ... # xi shape (6,)
|
||||
def log_map(pose: SE3) -> np.ndarray: ... # returns shape (6,)
|
||||
def adjoint(pose: SE3) -> np.ndarray: ... # returns shape (6, 6)
|
||||
def is_valid_rotation(R_3x3: np.ndarray, *, atol: float = 1e-6) -> bool: ...
|
||||
```
|
||||
|
||||
| Name | Signature | Throws / Errors | Blocking? |
|
||||
|------|-----------|-----------------|-----------|
|
||||
| `matrix_to_se3` | `(T_4x4) -> SE3` | `Se3InvalidMatrixError` if shape != (4,4), bottom row != [0,0,0,1], or rotation is not orthogonal within `atol` | sync, pure |
|
||||
| `se3_to_matrix` | `(SE3) -> np.ndarray (4,4)` | none | sync, pure |
|
||||
| `exp_map` | `(xi: (6,)) -> SE3` | `Se3InvalidMatrixError` if shape != (6,) | sync, pure |
|
||||
| `log_map` | `(SE3) -> np.ndarray (6,)` | none | sync, pure |
|
||||
| `adjoint` | `(SE3) -> np.ndarray (6,6)` | none | sync, pure |
|
||||
| `is_valid_rotation` | `(R_3x3) -> bool` | none (returns False for any invalid input) | sync, pure |
|
||||
|
||||
`SE3` is a type alias for the GTSAM `Pose3` (re-exported from `helpers.se3_utils` so consumers do not import GTSAM directly). All numpy arrays use `dtype=float64`; passing `float32` raises `Se3InvalidMatrixError`.
|
||||
|
||||
## Invariants
|
||||
|
||||
- **Stateless**: no module-level state; every function is pure. The same input always produces the same output (deep-equal).
|
||||
- **Right-handed convention**: rotation order is right-handed; `T_4x4` follows the standard `[[R, t], [0, 1]]` block layout.
|
||||
- **Orthogonal-rotation guarantee on the way in**: callers MUST orthogonalise their rotation matrices before `matrix_to_se3`. The helper rejects matrices whose `R^T R` deviates from `I` by more than `atol`. The helper does NOT silently re-orthogonalise.
|
||||
- **Positive-determinant rotation**: `det(R) ≈ +1`. Mirror matrices (`det(R) ≈ -1`) are rejected.
|
||||
- **Round-trip identity**: `se3_to_matrix(matrix_to_se3(T)) == T` for any valid `T` within numerical tolerance (`np.allclose(..., atol=1e-9)`).
|
||||
- **Lie-algebra round-trip**: `exp_map(log_map(p)) == p` for any non-degenerate `p` within `atol=1e-9`. Near-identity edge cases (twist norm < 1e-10) MUST not raise — the implementation falls back to the small-angle Taylor expansion documented in GTSAM.
|
||||
- **No upward imports** (Layer 1): the module imports ONLY from `_types`, GTSAM, numpy, and stdlib. No `gps_denied_onboard.components.*` imports.
|
||||
|
||||
## Non-Goals
|
||||
|
||||
- Quaternion utilities (`Rotation` / `Quaternion`) — out of scope; consumers that need a quaternion are expected to convert via numpy's `from_matrix` / `from_quat` paths inline.
|
||||
- SE(2) / planar pose helpers — out of scope.
|
||||
- Pose interpolation / Slerp — out of scope (consumers that need it implement it locally on top of `exp_map` / `log_map`).
|
||||
- Manifold operators richer than exp/log/adjoint (e.g., parallel transport, twist composition Jacobians) — out of scope; revisit when a consumer needs them.
|
||||
|
||||
## Versioning Rules
|
||||
|
||||
- **Breaking changes** (function renamed/removed, signature changed, error type changed, dtype contract relaxed) require a new major version + a deprecation pass through C1, C2.5, C3, C3.5, C4, C5, C8.
|
||||
- **Non-breaking additions** (new helper function, new optional kwarg with safe default) require a minor version bump.
|
||||
|
||||
## Test Cases
|
||||
|
||||
| Case | Input | Expected | Notes |
|
||||
|------|-------|----------|-------|
|
||||
| valid-roundtrip-4x4 | random valid `T_4x4` | `np.allclose(se3_to_matrix(matrix_to_se3(T)), T, atol=1e-9)` | Round-trip happy path |
|
||||
| valid-roundtrip-lie | random `xi` of norm ≈ 1.0 | `np.allclose(log_map(exp_map(xi)), xi, atol=1e-9)` | Lie-algebra round-trip |
|
||||
| valid-near-identity | `xi = [1e-12]*6` | `exp_map(xi)` returns identity within `atol=1e-9`; no exception | Small-angle stability |
|
||||
| invalid-non-orthogonal | `T_4x4` whose `R` has `R^T R - I` of norm 1e-3 | `Se3InvalidMatrixError` raised; helper does NOT silently re-orthogonalise | Strict caller-orthogonalisation rule |
|
||||
| invalid-mirror | `T_4x4` with `det(R) = -1` | `Se3InvalidMatrixError` raised | Positive-det invariant |
|
||||
| invalid-bottom-row | `T_4x4` with bottom row `[0,0,0,2]` | `Se3InvalidMatrixError` raised | Block-layout guard |
|
||||
| invalid-dtype | `T_4x4` with `dtype=float32` | `Se3InvalidMatrixError` raised mentioning dtype | dtype contract |
|
||||
| determinism | same `T_4x4` through `matrix_to_se3 → se3_to_matrix` twice | byte-equal numpy outputs | Pure-function determinism |
|
||||
| no-upward-imports | static import scan of `helpers.se3_utils` | only `_types`, GTSAM, numpy, stdlib | Layer 1 invariant |
|
||||
|
||||
## Change Log
|
||||
|
||||
| Version | Date | Change | Author |
|
||||
|---------|------|--------|--------|
|
||||
| 1.0.0 | 2026-05-10 | Initial contract derived from `_docs/02_document/common-helpers/02_helper_se3_utils.md` | autodev decompose Step 2 |
|
||||
Reference in New Issue
Block a user