# Common Helper — `SE3Utils`

## Purpose

SE(3) ↔ pose-matrix conversion and Lie-algebra exponential/logarithm. Used wherever a 4×4 transformation matrix needs to be converted to/from a 6-vector, or where Jacobians of SE(3) operations are needed for covariance recovery.

## Used By

- C1 — Visual / Visual-Inertial Odometry (relative pose updates).
- C4 — Pose Estimation (`solvePnPRansac` 4×4 → SE(3) for the GTSAM factor).
- C5 — State Estimator (iSAM2 graph keys + smoothed history).

## Interface (sketch)

```
def matrix_to_se3(T_4x4: ndarray) -> SE3
def se3_to_matrix(pose: SE3) -> ndarray
def exp_map(xi: Vector6) -> SE3
def log_map(pose: SE3) -> Vector6
def adjoint(pose: SE3) -> Matrix6
```

## Implementation Notes

- Backed by GTSAM `Pose3` + Eigen Lie-algebra primitives where available; otherwise pure numpy.
- All-positive-determinant rotation guarantee — caller is responsible for orthogonalising input rotation matrices before calling `matrix_to_se3`.

## Caveats

- Library-grade Lie-algebra functions exist in `manifpy` and `pylie`; we use GTSAM's primitives directly to avoid pulling in a second math library. If a future strategy needs richer manifold ops, evaluate `manifpy` then.

## Cycle-1 operational reality

The shipped surface in `src/gps_denied_onboard/helpers/se3_utils.py` (AZ-277) extends the sketch above; this section is the authoritative inventory of what cycle-1 consumers actually see.

- **Type alias** — `SE3 = gtsam.Pose3` is re-exported by the helper. Consumers MUST import `SE3` from `helpers.se3_utils` and never `gtsam.Pose3` directly (keeps the Lie-algebra backend swappable without touching C1/C4/C5).
- **`Se3InvalidMatrixError`** — single public exception type. Raised on (a) wrong array shape, (b) `dtype != float64`, (c) bottom row != `[0, 0, 0, 1]`, (d) rotation drift `‖R^TR − I‖_F > atol`, (e) negative-determinant rotation (mirror), (f) non-ndarray inputs. `matrix_to_se3` and `exp_map` raise this; `se3_to_matrix`, `log_map`, `adjoint` are no-throw on the typed input.
- **Strict caller-orthogonalisation invariant** — the helper does NOT silently re-orthogonalise. AC-7 / `matrix_to_se3` always validates `‖R^TR − I‖_F ≤ atol` and rejects drift. Callers (C4 in particular, since `solvePnPRansac` output is not orthogonal to numerical precision) MUST run their own orthogonalisation (`cv2.Rodrigues` round-trip or `scipy.linalg.polar`) before calling `matrix_to_se3`. Default tolerance: `_DEFAULT_ROT_ATOL = 1e-6`; callers can pass a looser `atol` for relaxed contexts (none in cycle-1).
- **`exp_map` near-identity fallback** — twist vectors with `‖xi‖ < _SMALL_ANGLE_THRESHOLD = 1e-10` return the identity `SE3()` instead of delegating to GTSAM's `Pose3.Expmap`. This guards against the `sin(theta)/theta` under-flow that surfaces when iSAM2's relinearisation produces a near-identity twist after a converged step.
- **`is_valid_rotation(R_3x3, *, atol=1e-6)`** — predicate (no exception) for "is this matrix safe to feed to `matrix_to_se3`?". Returns False for non-ndarray, wrong shape, wrong dtype, orthogonality drift > atol, or negative determinant. Cycle-1 consumers: C4's `MarginalsAdapter` short-circuit (`opencv_gtsam_marginals.py` from AZ-358) and the contract test for AC-7.
- **`dtype=float64` everywhere** — every public function enforces `float64`. `np.ndarray` returned from `se3_to_matrix`, `log_map`, `adjoint` is `np.ascontiguousarray(..., dtype=np.float64)` so callers can pass it through to GTSAM/Eigen without a copy.

### Cycle-1 task lineage

- AZ-277 — initial helper, contract producer.
- No cycle-1 follow-up tasks touched this helper.