gps-denied-onboard/_docs/02_tasks/done/AZ-277_se3_utils.md

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.