pywayne-vio-se3

SE3 Rigid Body Transformations

Quick Start

import numpy as np
from pywayne.vio.SE3 import *

# Create SE(3) transformation from rotation and translation
R = np.eye(3)
t = np.array([1, 2, 3])
T = SE3_from_Rt(R, t)

# Lie algebra operations
xi = np.array([0.1, 0.2, 0.3, 0.05, 0.1, 0.15])  # [rho, theta]
T_from_xi = SE3_Exp(xi)  # se(3) vector -> SE(3)
xi_recovered = SE3_Log(T_from_xi)  # SE(3) -> se(3) vector

Core Operations

Basic Matrix Operations

Create/Verify SE(3) matrices:

• check_SE3(T) - Validate 4x4 matrix is valid SE(3)
• SE3_from_Rt(R, t) - Construct from rotation matrix and translation
• SE3_to_Rt(T) - Extract rotation matrix and translation vector

Combine/invert transformations:

• SE3_mul(T1, T2) - Matrix multiplication (compose transforms)
• SE3_inv(T) - Vectorized matrix inverse for (4,4) or (N,4,4) input
• SE3_diff(T1, T2, from_1_to_2=True) - Compute relative transform

Lie Group/Lie Algebra Mappings

Vector form (preferred):

• SE3_Exp(xi) - se(3) 6D vector -> SE(3) matrix, xi = [rho, theta]
• SE3_Log(T) - SE(3) matrix -> se(3) 6D vector

Matrix form (theoretical):

• SE3_exp(xi_hat) - se(3) 4x4 matrix -> SE(3) matrix
• SE3_log(T) - SE(3) matrix -> se(3) 4x4 matrix
• SE3_skew(xi) - 6D vector -> 4x4 Lie algebra matrix
• SE3_unskew(xi_hat) - 4x4 matrix -> 6D vector

Naming convention: Uppercase = vector, lowercase = matrix

Representation Conversions

Quaternion + translation:

• SE3_from_quat_trans(q, t) - q is wxyz quaternion
• SE3_to_quat_trans(T) - Returns (quaternion, translation)

Axis-angle + translation:

• SE3_from_axis_angle_trans(axis, angle, t)
• SE3_to_axis_angle_trans(T) - Returns (axis, angle, translation)

Euler angles + translation:

• SE3_from_euler_trans(euler_angles, t, axes='zyx', intrinsic=True)
• SE3_to_euler_trans(T, axes='zyx', intrinsic=True)

Statistical Operations

• SE3_mean(T_batch) - Compute mean of multiple SE(3) matrices (Nx4x4 -> 4x4)