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