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