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