# 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.