gps-denied-onboard/_docs/02_document/contracts/shared_helpers/se3_utils.md

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

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