mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 21:41: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,152 @@
|
||||
# SE3Utils Helper Module
|
||||
|
||||
**Task**: AZ-277_se3_utils
|
||||
**Name**: SE3Utils Helper
|
||||
**Description**: Implement the shared `SE3Utils` helper for SE(3) ↔ 4×4-matrix conversion and Lie-algebra exp/log/adjoint, backed by GTSAM `Pose3` primitives. Used wherever a consumer needs a 6-vector twist, a Jacobian over an SE(3) operation, or a deterministic conversion between matrix and pose forms — i.e., C1, C2.5, C3, C3.5, C4, C5, C8. Stateless; pure functions; strict caller-orthogonalisation contract.
|
||||
**Complexity**: 2 points
|
||||
**Dependencies**: AZ-263_initial_structure
|
||||
**Component**: shared.helpers.se3_utils (cross-cutting; epic AZ-264 / E-CC-HELPERS)
|
||||
**Tracker**: AZ-277
|
||||
**Epic**: AZ-264 (E-CC-HELPERS)
|
||||
|
||||
### Document Dependencies
|
||||
|
||||
- `_docs/02_document/contracts/shared_helpers/se3_utils.md` — frozen public interface this task produces.
|
||||
- `_docs/02_document/common-helpers/02_helper_se3_utils.md` — design rationale and consumer mapping.
|
||||
|
||||
## Problem
|
||||
|
||||
Seven components (C1, C2.5, C3, C3.5, C4, C5, C8) need to cross the matrix-vs-pose boundary:
|
||||
- C4's `solvePnPRansac` returns a 4×4 matrix; C5's iSAM2 graph wants a GTSAM `Pose3`.
|
||||
- C1's relative-pose update needs `log_map` for covariance recovery.
|
||||
- C8 encodes pose as a 6-vector for FC adapter emission.
|
||||
|
||||
Without a shared helper:
|
||||
- Each component re-implements the conversion, drifting on rotation conventions, sign conventions, or near-identity edge cases.
|
||||
- Subtle differences in `det(R)` validation (some silently re-orthogonalise, others reject) break the "same pose in, same pose out" invariant across components.
|
||||
- Any future change (e.g., switching from GTSAM `Pose3` to `manifpy`) becomes a 7-place coordinated edit.
|
||||
|
||||
## Outcome
|
||||
|
||||
- A single `helpers.se3_utils` module is the only place that constructs a `Pose3` from a matrix or vice-versa across the codebase. Component imports go through the helper.
|
||||
- All conversions are pure functions: same input → byte-equal numpy / GTSAM output.
|
||||
- Strict orthogonal-rotation contract: `matrix_to_se3` rejects non-orthogonal or negative-determinant rotations with `Se3InvalidMatrixError` instead of silently fixing them. Callers are responsible for orthogonalisation; the rejection forces the bug back to the source.
|
||||
- Near-identity Lie-algebra inputs (twist norm < 1e-10) are stable — `exp_map` falls back to the small-angle Taylor expansion documented in GTSAM rather than NaN-ing on `sin(θ)/θ`.
|
||||
|
||||
## Scope
|
||||
|
||||
### Included
|
||||
|
||||
- `matrix_to_se3(T_4x4) -> SE3`, `se3_to_matrix(SE3) -> np.ndarray`.
|
||||
- `exp_map(xi) -> SE3`, `log_map(SE3) -> np.ndarray`, `adjoint(SE3) -> np.ndarray`.
|
||||
- `is_valid_rotation(R_3x3, *, atol)` predicate for callers to check before calling `matrix_to_se3`.
|
||||
- `Se3InvalidMatrixError` exception type.
|
||||
- Re-export of GTSAM `Pose3` as `SE3` so consumers do not import GTSAM directly.
|
||||
- Public interface contract published at `_docs/02_document/contracts/shared_helpers/se3_utils.md`.
|
||||
|
||||
### Excluded
|
||||
|
||||
- Quaternion conversions — consumers convert via numpy / GTSAM directly.
|
||||
- SE(2) helpers — out of scope.
|
||||
- Pose interpolation / Slerp — out of scope.
|
||||
- Higher-order manifold ops (parallel transport, composition Jacobians) — out of scope.
|
||||
|
||||
## Acceptance Criteria
|
||||
|
||||
**AC-1: 4×4 ↔ SE3 round-trip**
|
||||
Given a randomly-sampled valid `T_4x4` (orthogonal rotation, positive determinant, identity bottom row)
|
||||
When `matrix_to_se3` then `se3_to_matrix` runs
|
||||
Then the recovered matrix matches the input via `np.allclose(..., atol=1e-9)`
|
||||
|
||||
**AC-2: Lie-algebra round-trip**
|
||||
Given a random twist `xi` of shape `(6,)` and norm ≈ 1.0
|
||||
When `exp_map(xi)` then `log_map(...)` runs
|
||||
Then the recovered twist matches `xi` via `np.allclose(..., atol=1e-9)`
|
||||
|
||||
**AC-3: Near-identity Lie stability**
|
||||
Given `xi = [1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12]`
|
||||
When `exp_map(xi)` runs
|
||||
Then the result is the identity pose within `atol=1e-9`; no exception, no NaN
|
||||
|
||||
**AC-4: Strict orthogonality rejection**
|
||||
Given `T_4x4` whose `R` has `||R^T R - I||_F = 1e-3`
|
||||
When `matrix_to_se3(T)` runs
|
||||
Then `Se3InvalidMatrixError` is raised AND the helper does NOT silently re-orthogonalise (the message names the deviation magnitude)
|
||||
|
||||
**AC-5: Mirror rejection**
|
||||
Given `T_4x4` with `det(R) ≈ -1`
|
||||
When `matrix_to_se3(T)` runs
|
||||
Then `Se3InvalidMatrixError` is raised mentioning the negative determinant
|
||||
|
||||
**AC-6: Block-layout guard**
|
||||
Given `T_4x4` with bottom row `[0, 0, 0, 2]` (or any deviation from `[0, 0, 0, 1]`)
|
||||
When `matrix_to_se3(T)` runs
|
||||
Then `Se3InvalidMatrixError` is raised mentioning the bottom row
|
||||
|
||||
**AC-7: dtype contract**
|
||||
Given `T_4x4` with `dtype=float32`
|
||||
When `matrix_to_se3(T)` runs
|
||||
Then `Se3InvalidMatrixError` is raised mentioning dtype (helpers operate strictly on `float64`)
|
||||
|
||||
**AC-8: Determinism**
|
||||
Given the same `T_4x4` (or `xi`)
|
||||
When converted twice through any helper function
|
||||
Then both outputs are byte-equal
|
||||
|
||||
**AC-9: No upward imports (Layer 1 invariant)**
|
||||
Given the helper module
|
||||
When a static-import check runs
|
||||
Then it imports ONLY from `_types`, GTSAM, numpy, and stdlib — no `gps_denied_onboard.components.*` imports anywhere
|
||||
|
||||
## Non-Functional Requirements
|
||||
|
||||
**Performance**
|
||||
- Each helper function p99 ≤ 50 µs on Tier-2 — overhead vs. inline GTSAM ≤ 5 % (per E-CC-HELPERS hot-path NFR).
|
||||
|
||||
**Reliability**
|
||||
- Pure deterministic; same input → byte-equal output.
|
||||
- `Se3InvalidMatrixError` is the ONLY exception type the public surface raises on shape / orthogonality / dtype violations.
|
||||
|
||||
## Unit Tests
|
||||
|
||||
| AC Ref | What to Test | Required Outcome |
|
||||
|--------|-------------|-----------------|
|
||||
| AC-1 | `np.allclose(se3_to_matrix(matrix_to_se3(T)), T)` for 100 random valid `T` | all pass within `atol=1e-9` |
|
||||
| AC-2 | `np.allclose(log_map(exp_map(xi)), xi)` for 100 random `xi` (norm ≈ 1.0) | all pass within `atol=1e-9` |
|
||||
| AC-3 | `exp_map([1e-12]*6)` | identity pose within `atol=1e-9`; no NaN |
|
||||
| AC-4 | non-orthogonal `T` | `Se3InvalidMatrixError`; message names deviation |
|
||||
| AC-5 | `det(R) = -1` `T` | `Se3InvalidMatrixError`; mentions determinant |
|
||||
| AC-6 | bottom row `[0, 0, 0, 2]` | `Se3InvalidMatrixError`; mentions bottom row |
|
||||
| AC-7 | `float32` dtype | `Se3InvalidMatrixError`; mentions dtype |
|
||||
| AC-8 | call any helper twice with same input | byte-equal outputs |
|
||||
| AC-9 | static import scan | only `_types`, GTSAM, numpy, stdlib |
|
||||
| NFR-perf | microbench each helper (10k iterations on Tier-2 fixture) | p99 ≤ 50 µs each |
|
||||
|
||||
## Constraints
|
||||
|
||||
- Public surface frozen by `_docs/02_document/contracts/shared_helpers/se3_utils.md` v1.0.0.
|
||||
- Layer 1 Foundation only.
|
||||
- GTSAM is the single math backend; numpy fallback only when GTSAM does not expose the primitive.
|
||||
- No new dependency beyond what AZ-263 / E-BOOT pinned.
|
||||
|
||||
## Risks & Mitigation
|
||||
|
||||
**Risk 1: Silent re-orthogonalisation hides upstream rotation drift**
|
||||
- *Risk*: A future change "softens" `matrix_to_se3` to silently re-orthogonalise inputs; consumers no longer learn that their rotation source is producing non-orthogonal matrices.
|
||||
- *Mitigation*: AC-4 makes strict rejection part of the contract. The contract test enforces that `Se3InvalidMatrixError` is raised, not absorbed.
|
||||
|
||||
**Risk 2: GTSAM API drift between minor versions**
|
||||
- *Risk*: `Pose3.expmap` signature changes; this helper breaks on a GTSAM upgrade.
|
||||
- *Mitigation*: GTSAM is pinned in `pyproject.toml` at AZ-263 / E-BOOT; this helper's tests are the canary that detects drift before consumers do.
|
||||
|
||||
## Runtime Completeness
|
||||
|
||||
- **Named capability**: SE(3) ↔ matrix conversion + Lie-algebra exp/log/adjoint via GTSAM `Pose3` primitives (architecture / E-CC-HELPERS / `02_helper_se3_utils.md`).
|
||||
- **Production code that must exist**: real GTSAM-backed conversions; real strict-orthogonality guard; real small-angle Taylor fallback for near-identity exp.
|
||||
- **Allowed external stubs**: numpy fallback only where GTSAM does not expose the primitive (e.g., adjoint matrix construction).
|
||||
- **Unacceptable substitutes**: silent re-orthogonalisation; "for now we just call `np.linalg.logm`" (numerically inferior, no Jacobian); skipping near-identity small-angle handling (NaN risk).
|
||||
|
||||
## Contract
|
||||
|
||||
This task produces the contract at `_docs/02_document/contracts/shared_helpers/se3_utils.md`.
|
||||
Consumers MUST read that file — not this task spec — to discover the interface.
|
||||
Reference in New Issue
Block a user