gps-denied-onboard/_docs/02_document/common-helpers/02_helper_se3_utils.md

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.