mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 21:41:12 +00:00
Oleksandr Bezdieniezhnykh
64542d32fc
Transitioned the autodev state to phase 21, reflecting the completion of Step 5 and the drafting of Step 6 epics. Revised the architecture documentation to clarify the roles of the Tile Manager and its components, ensuring accurate representation of the system's operational flow. Updated glossary entries for Flight State and Operator to incorporate recent changes and enhance clarity on component interactions and responsibilities.
1.2 KiB
1.2 KiB
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 (
solvePnPRansac4×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
manifpyandpylie; we use GTSAM's primitives directly to avoid pulling in a second math library. If a future strategy needs richer manifold ops, evaluatemanifpythen.