SE3 刚体变换工具 (SE3)

SE3 工具函数库 - 刚体变换操作

本库提供了完整的SE(3)刚体变换操作，包括： - 基础变换：构造、验证、相乘、求逆 - 表示转换：四元数、轴角、欧拉角等 - 李群李代数映射：log/Log, exp/Exp

李群李代数函数命名规范： - SE3_Log: SE(3) → 李代数向量 (6维向量 [ρ, θ]) - SE3_log: SE(3) → 李代数矩阵 (4×4 反对称形式) - SE3_Exp: 李代数向量 → SE(3) - SE3_exp: 李代数矩阵 → SE(3)

使用示例：

# 1. 基础变换操作 import numpy as np from pywayne.vio.SE3 import *

# 构造SE3变换矩阵 R = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) # 旋转矩阵 t = np.array([1, 2, 3]) # 平移向量 T = SE3_from_Rt(R, t)

# 验证SE3矩阵 is_valid = check_SE3(T)

# 矩阵相乘和求逆 T1 = SE3_from_Rt(np.eye(3), [1, 0, 0]) T2 = SE3_from_Rt(np.eye(3), [0, 1, 0]) T_combined = SE3_mul(T1, T2) T_inv = SE3_inv(T)

# 2. 李群李代数映射 # 向量形式 (常用) xi = np.array([0.1, 0.2, 0.3, 0.05, 0.1, 0.15]) # [ρ, θ] T_from_xi = SE3_Exp(xi) # 李代数向量 → SE(3) xi_recovered = SE3_Log(T_from_xi) # SE(3) → 李代数向量

# 矩阵形式 (理论计算) xi_hat = SE3_skew(xi) # 6维向量 → 4×4李代数矩阵 T_from_hat = SE3_exp(xi_hat) # 李代数矩阵 → SE(3) xi_hat_recovered = SE3_log(T_from_hat) # SE(3) → 李代数矩阵

# 3. 批量处理 n = 100 # 批量李代数向量 xi_batch = np.random.randn(n, 6) * 0.1 T_batch = SE3_Exp(xi_batch) # 批量指数映射 xi_batch_recovered = SE3_Log(T_batch) # 批量对数映射

# 4. 表示转换 q = np.array([[1, 0, 0, 0]]) # 四元数 (w,x,y,z) t = np.array([[1, 2, 3]]) # 平移 T_from_qt = SE3_from_quat_trans(q, t) q_recovered, t_recovered = SE3_to_quat_trans(T_from_qt)

# 5. 平均变换 T_list = [SE3_Exp(np.random.randn(6) * 0.1) for _ in range(10)] T_matrices = np.array(T_list) T_mean = SE3_mean(T_matrices)

性能参考 (1000个变换): - SE3_Exp: ~2.5 ms - SE3_Log: ~0.8 ms - SE3_exp: ~2.5 ms - SE3_log: ~0.9 ms

pywayne.vio.SE3.SE3_Exp(xi: ndarray) → ndarray

Convert SE(3) Lie algebra vector to transformation matrix. :param xi: Nx6 or 6 SE(3) Lie algebra vector [ρ, θ]

Returns:

Nx4x4 or 4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_Log(T: ndarray) → ndarray

Convert SE(3) transformation matrix to Lie algebra vector. :param T: Nx4x4 or 4x4 SE(3) transformation matrix

Returns:

Nx6 or 6 SE(3) Lie algebra vector [ρ, θ]

Return type:

xi

pywayne.vio.SE3.SE3_diff(T1: ndarray, T2: ndarray, from_1_to_2: bool = True) → ndarray

Compute the difference between two SE(3) transformation matrices. :param T1: 4x4 transformation matrix :param T2: 4x4 transformation matrix :param from_1_to_2: if True, compute T1^(-1) @ T2, otherwise compute T2^(-1) @ T1

Returns:

SE(3) difference matrix

pywayne.vio.SE3.SE3_exp(xi_hat: ndarray) → ndarray

Convert SE(3) Lie algebra matrix to transformation matrix. :param xi_hat: Nx4x4 or 4x4 SE(3) Lie algebra matrix

Returns:

Nx4x4 or 4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_from_Rt(R: ndarray, t: ndarray) → ndarray

Construct SE(3) matrix from rotation matrix and translation vector. :param R: Nx3x3 rotation matrices or 3x3 rotation matrix :param t: Nx3 translation vectors or 3 translation vector

Returns:

Nx4x4 or 4x4 SE(3) transformation matrices

Return type:

T

pywayne.vio.SE3.SE3_from_axis_angle_trans(axis: ndarray, angle: ndarray, t: ndarray) → ndarray

Convert axis-angle and translation to SE(3) transformation matrix. :param axis: Nx3 rotation axis :param angle: Nx1 rotation angle :param t: Nx3 translation vector

Returns:

Nx4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_from_euler_trans(euler_angles: ndarray, t: ndarray, axes: str = 'zyx', intrinsic: bool = True) → ndarray

Convert Euler angles and translation to SE(3) transformation matrix. :param euler_angles: Nx3 Euler angles, rad :param t: Nx3 translation vector :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:

Nx4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_from_quat_trans(q: ndarray, t: ndarray) → ndarray

Convert quaternion and translation to SE(3) transformation matrix. :param q: Nx4 quaternion, wxyz, Hamilton convention :param t: Nx3 translation vector

Returns:

Nx4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_inv(T: ndarray) → ndarray

Compute the inverse of SE(3) transformation matrix. :param T: Nx4x4 or 4x4 SE(3) transformation matrix

Returns:

Nx4x4 or 4x4 inverse transformation matrix

Return type:

T_inv

pywayne.vio.SE3.SE3_log(T: ndarray) → ndarray

Convert SE(3) transformation matrix to Lie algebra matrix. :param T: Nx4x4 or 4x4 SE(3) transformation matrix

Returns:

Nx4x4 or 4x4 SE(3) Lie algebra matrix

Return type:

xi_hat

pywayne.vio.SE3.SE3_mean(T: ndarray) → ndarray

Compute the mean of multiple SE(3) transformation matrices using scipy. :param T: Nx4x4 transformation matrices

Returns:

4x4 mean transformation matrix

Return type:

mean_T

pywayne.vio.SE3.SE3_mul(T1: ndarray, T2: ndarray) → ndarray

Multiply two SE(3) transformation matrices. :param T1: 4x4 transformation matrix :param T2: 4x4 transformation matrix

Returns:

T1 @ T2

pywayne.vio.SE3.SE3_skew(xi: ndarray) → ndarray

Convert a 6D vector to SE(3) Lie algebra (4x4 skew-symmetric form). :param xi: Nx6 vector [ρ, θ] where ρ is translation part, θ is rotation part

Returns:

Nx4x4 SE(3) Lie algebra matrix

Return type:

skew_mat

pywayne.vio.SE3.SE3_to_Rt(T: ndarray) → tuple

Extract rotation matrix and translation vector from SE(3) matrix. :param T: Nx4x4 or 4x4 SE(3) transformation matrices

Returns:

Nx3x3 or 3x3 rotation matrices t: Nx3 or 3 translation vectors

Return type:

R

pywayne.vio.SE3.SE3_to_axis_angle_trans(T: ndarray) → tuple

Convert SE(3) transformation matrix to axis-angle and translation. :param T: Nx4x4 SE(3) transformation matrix

Returns:

Nx3 rotation axis angle: Nx1 rotation angle t: Nx3 translation vector

Return type:

axis

pywayne.vio.SE3.SE3_to_euler_trans(T: ndarray, axes: str = 'zyx', intrinsic: bool = True) → tuple

Convert SE(3) transformation matrix to Euler angles and translation. :param T: Nx4x4 SE(3) transformation matrix :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:

Nx3 Euler angles, rad t: Nx3 translation vector

Return type:

euler_angles

pywayne.vio.SE3.SE3_to_quat_trans(T: ndarray) → tuple

Convert SE(3) transformation matrix to quaternion and translation. :param T: Nx4x4 SE(3) transformation matrix

Returns:

Nx4 quaternion, wxyz, Hamilton convention t: Nx3 translation vector

Return type:

q

pywayne.vio.SE3.SE3_unskew(xi_hat: ndarray) → ndarray

Convert SE(3) Lie algebra matrix to 6D vector. :param xi_hat: Nx4x4 or 4x4 SE(3) Lie algebra matrix

Returns:

Nx6 or 6 vector [ρ, θ] where ρ is translation part, θ is rotation part

Return type:

xi

pywayne.vio.SE3.check_SE3(T: ndarray) → bool

Check if a matrix is a valid SE(3) transformation matrix. :param T: 4x4 transformation matrix

Returns:

True if T is a valid SE(3) transformation matrix, False otherwise

该模块提供了SE(3)刚体变换矩阵的完整操作工具集，包括刚体变换的生成、转换、组合、对数/指数映射以及平均等功能。SE(3)群表示三维空间中的刚体运动（旋转+平移），是机器人学、SLAM和计算机视觉领域的核心数学工具。

数学背景

SE(3)是特殊欧几里得群，表示三维空间中的刚体变换。SE(3)群的元素是4×4的齐次变换矩阵：

\[\begin{split}T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4}\end{split}\]

其中： - \(R \in SO(3)\) 是3×3旋转矩阵 - \(t \in \mathbb{R}^3\) 是3×1平移向量

相应的李代数se(3)由4×4矩阵组成：

\[\begin{split}\xi^\wedge = \begin{bmatrix} \omega^\wedge & \rho \\ 0^T & 0 \end{bmatrix}\end{split}\]

其中： - \(\omega^\wedge\) 是3×3反对称矩阵 - \(\rho \in \mathbb{R}^3\) 是平移部分

主要功能分类

基础操作函数

pywayne.vio.SE3.check_SE3(T: ndarray) → bool

Check if a matrix is a valid SE(3) transformation matrix. :param T: 4x4 transformation matrix

Returns:

True if T is a valid SE(3) transformation matrix, False otherwise

验证矩阵是否为有效的SE(3)变换矩阵。

pywayne.vio.SE3.SE3_mul(T1: ndarray, T2: ndarray) → ndarray

Multiply two SE(3) transformation matrices. :param T1: 4x4 transformation matrix :param T2: 4x4 transformation matrix

Returns:

T1 @ T2

计算两个变换矩阵的乘积，实现变换组合。

pywayne.vio.SE3.SE3_diff(T1: ndarray, T2: ndarray, from_1_to_2: bool = True) → ndarray

Compute the difference between two SE(3) transformation matrices. :param T1: 4x4 transformation matrix :param T2: 4x4 transformation matrix :param from_1_to_2: if True, compute T1^(-1) @ T2, otherwise compute T2^(-1) @ T1

Returns:

SE(3) difference matrix

计算两个变换矩阵之间的相对变换。

pywayne.vio.SE3.SE3_inv(T: ndarray) → ndarray

Compute the inverse of SE(3) transformation matrix. :param T: Nx4x4 or 4x4 SE(3) transformation matrix

Returns:

Nx4x4 or 4x4 inverse transformation matrix

Return type:

T_inv

计算变换矩阵的逆。

反对称矩阵操作

pywayne.vio.SE3.SE3_skew(xi: ndarray) → ndarray

Convert a 6D vector to SE(3) Lie algebra (4x4 skew-symmetric form). :param xi: Nx6 vector [ρ, θ] where ρ is translation part, θ is rotation part

Returns:

Nx4x4 SE(3) Lie algebra matrix

Return type:

skew_mat

将6D向量转换为4×4 SE(3)李代数矩阵。

pywayne.vio.SE3.SE3_unskew(xi_hat: ndarray) → ndarray

Convert SE(3) Lie algebra matrix to 6D vector. :param xi_hat: Nx4x4 or 4x4 SE(3) Lie algebra matrix

Returns:

Nx6 or 6 vector [ρ, θ] where ρ is translation part, θ is rotation part

Return type:

xi

从4×4 SE(3)李代数矩阵提取对应的6D向量。

旋转与平移组合

pywayne.vio.SE3.SE3_from_Rt(R: ndarray, t: ndarray) → ndarray

Construct SE(3) matrix from rotation matrix and translation vector. :param R: Nx3x3 rotation matrices or 3x3 rotation matrix :param t: Nx3 translation vectors or 3 translation vector

Returns:

Nx4x4 or 4x4 SE(3) transformation matrices

Return type:

T

pywayne.vio.SE3.SE3_to_Rt(T: ndarray) → tuple

Extract rotation matrix and translation vector from SE(3) matrix. :param T: Nx4x4 or 4x4 SE(3) transformation matrices

Returns:

Nx3x3 or 3x3 rotation matrices t: Nx3 or 3 translation vectors

Return type:

R

从旋转矩阵和平移向量构造SE(3)矩阵，以及反向分解。

变换表示转换

pywayne.vio.SE3.SE3_from_quat_trans(q: ndarray, t: ndarray) → ndarray

Convert quaternion and translation to SE(3) transformation matrix. :param q: Nx4 quaternion, wxyz, Hamilton convention :param t: Nx3 translation vector

Returns:

Nx4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_to_quat_trans(T: ndarray) → tuple

Convert SE(3) transformation matrix to quaternion and translation. :param T: Nx4x4 SE(3) transformation matrix

Returns:

Nx4 quaternion, wxyz, Hamilton convention t: Nx3 translation vector

Return type:

q

四元数+平移与SE(3)矩阵的相互转换。

pywayne.vio.SE3.SE3_from_axis_angle_trans(axis: ndarray, angle: ndarray, t: ndarray) → ndarray

Convert axis-angle and translation to SE(3) transformation matrix. :param axis: Nx3 rotation axis :param angle: Nx1 rotation angle :param t: Nx3 translation vector

Returns:

Nx4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_to_axis_angle_trans(T: ndarray) → tuple

Convert SE(3) transformation matrix to axis-angle and translation. :param T: Nx4x4 SE(3) transformation matrix

Returns:

Nx3 rotation axis angle: Nx1 rotation angle t: Nx3 translation vector

Return type:

axis

轴角+平移与SE(3)矩阵的相互转换。

pywayne.vio.SE3.SE3_from_euler_trans(euler_angles: ndarray, t: ndarray, axes: str = 'zyx', intrinsic: bool = True) → ndarray

Convert Euler angles and translation to SE(3) transformation matrix. :param euler_angles: Nx3 Euler angles, rad :param t: Nx3 translation vector :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:

Nx4x4 SE(3) transformation matrix

Return type:

T

pywayne.vio.SE3.SE3_to_euler_trans(T: ndarray, axes: str = 'zyx', intrinsic: bool = True) → tuple

Convert SE(3) transformation matrix to Euler angles and translation. :param T: Nx4x4 SE(3) transformation matrix :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:

Nx3 Euler angles, rad t: Nx3 translation vector

Return type:

euler_angles

欧拉角+平移与SE(3)矩阵的相互转换。

李群/李代数映射

pywayne.vio.SE3.SE3_Log(T: ndarray) → ndarray

Convert SE(3) transformation matrix to Lie algebra vector. :param T: Nx4x4 or 4x4 SE(3) transformation matrix

Returns:

Nx6 or 6 SE(3) Lie algebra vector [ρ, θ]

Return type:

xi

SE(3) → se(3)的对数映射，输出6D向量[ρ, θ]。

pywayne.vio.SE3.SE3_log(T: ndarray) → ndarray

Convert SE(3) transformation matrix to Lie algebra matrix. :param T: Nx4x4 or 4x4 SE(3) transformation matrix

Returns:

Nx4x4 or 4x4 SE(3) Lie algebra matrix

Return type:

xi_hat

SE(3) → se(3)的对数映射，输出4×4李代数矩阵。

pywayne.vio.SE3.SE3_Exp(xi: ndarray) → ndarray

Convert SE(3) Lie algebra vector to transformation matrix. :param xi: Nx6 or 6 SE(3) Lie algebra vector [ρ, θ]

Returns:

Nx4x4 or 4x4 SE(3) transformation matrix

Return type:

T

se(3) → SE(3)的指数映射，从6D向量生成变换矩阵。

pywayne.vio.SE3.SE3_exp(xi_hat: ndarray) → ndarray

Convert SE(3) Lie algebra matrix to transformation matrix. :param xi_hat: Nx4x4 or 4x4 SE(3) Lie algebra matrix

Returns:

Nx4x4 or 4x4 SE(3) transformation matrix

Return type:

T

se(3) → SE(3)的指数映射，从4×4矩阵生成变换矩阵。

平均与统计

pywayne.vio.SE3.SE3_mean(T: ndarray) → ndarray

Compute the mean of multiple SE(3) transformation matrices using scipy. :param T: Nx4x4 transformation matrices

Returns:

4x4 mean transformation matrix

Return type:

mean_T

计算多个变换矩阵的平均值，使用基于李代数的数值稳定方法。

数学关系

该模块中的函数满足以下重要数学关系：

往返转换:

• \(T = \text{SE3\_Exp}(\text{SE3\_Log}(T))\)

• \(\text{SE3\_log}(T) = \text{SE3\_skew}(\text{SE3\_Log}(T))\)

群运算:

• \(\text{SE3\_mul}(T_1, T_2) = T_1 \cdot T_2\)

• \(\text{SE3\_inv}(T) = T^{-1}\)

代数操作:

• \(\text{SE3\_exp}(\xi^\wedge) = \exp(\xi^\wedge)\)

• \(\text{SE3\_Log}(T) = \log(T)^\vee\)

指数映射公式:

对于6D向量 \(\xi = [\rho, \theta]^T\)：

\[\begin{split}\exp(\xi^\wedge) = \begin{bmatrix} \exp(\theta^\wedge) & V\rho \\ 0^T & 1 \end{bmatrix}\end{split}\]

其中V矩阵定义为：

\[V = I + \frac{1-\cos\|\theta\|}{\|\theta\|^2}\theta^\wedge + \frac{\|\theta\|-\sin\|\theta\|}{\|\theta\|^3}(\theta^\wedge)^2\]

使用示例

基础操作示例

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

# 创建SE(3)变换矩阵
R = np.eye(3)  # 无旋转
t = np.array([1, 2, 3])  # 平移向量
T = SE3_from_Rt(R, t)

print(f"SE3矩阵:\n{T}")
print(f"是否为有效SE3: {check_SE3(T)}")

# 计算逆变换
T_inv = SE3_inv(T)

# 验证 T @ T^(-1) = I
identity_check = SE3_mul(T, T_inv)
print(f"与单位矩阵的误差: {np.linalg.norm(identity_check - np.eye(4)):.2e}")

反对称矩阵操作

# 创建6D向量并转换为SE(3)李代数矩阵
xi = np.array([0.1, 0.2, 0.3, 0.1, 0.2, 0.3])  # [ρ, θ]
xi_hat = SE3_skew(xi)
print(f"SE3李代数矩阵:\n{xi_hat}")

# 从李代数矩阵恢复6D向量
recovered_xi = SE3_unskew(xi_hat)
print(f"恢复向量: {recovered_xi}")
print(f"往返误差: {np.linalg.norm(xi - recovered_xi):.2e}")

变换表示转换

# 从旋转矩阵和平移向量构造
angle = np.pi/4
R = np.array([
    [np.cos(angle), -np.sin(angle), 0],
    [np.sin(angle),  np.cos(angle), 0],
    [0,              0,             1]
])
t = np.array([1, 2, 3])
T = SE3_from_Rt(R, t)

# 分解为旋转和平移
R_recovered, t_recovered = SE3_to_Rt(T)
print(f"旋转矩阵误差: {np.linalg.norm(R - R_recovered):.2e}")
print(f"平移向量误差: {np.linalg.norm(t - t_recovered):.2e}")

李群/李代数映射

# 对数映射 - 向量形式
log_vec = SE3_Log(T)
print(f"对数映射向量: {log_vec}")

# 对数映射 - 矩阵形式
log_mat = SE3_log(T)
print(f"对数映射矩阵:\n{log_mat}")

# 指数映射 - 从向量
T_from_exp = SE3_Exp(log_vec)
print(f"指数映射(向量)误差: {np.linalg.norm(T - T_from_exp):.2e}")

# 指数映射 - 从矩阵
T_from_exp_mat = SE3_exp(log_mat)
print(f"指数映射(矩阵)误差: {np.linalg.norm(T - T_from_exp_mat):.2e}")

批量处理示例

# 创建多个随机变换
n_transforms = 50

# 随机旋转
random_axis = np.random.randn(n_transforms, 3)
random_axis = random_axis / np.linalg.norm(random_axis, axis=1, keepdims=True)
random_angles = np.random.uniform(0, np.pi, n_transforms)

# 随机平移
random_trans = np.random.randn(n_transforms, 3) * 10

# 批量生成SE(3)矩阵
T_batch = SE3_from_axis_angle_trans(random_axis, random_angles, random_trans)

# 批量转换为四元数+平移
quat_batch, trans_batch = SE3_to_quat_trans(T_batch)

# 计算平均变换
T_mean = SE3_mean(T_batch)
print(f"平均变换矩阵是否有效: {check_SE3(T_mean)}")

轨迹处理示例

# 模拟机器人轨迹
n_poses = 100
time_steps = np.linspace(0, 2*np.pi, n_poses)

# 圆形轨迹
radius = 5.0
positions = np.column_stack([
    radius * np.cos(time_steps),
    radius * np.sin(time_steps),
    np.ones(n_poses) * 2.0  # 固定高度
])

# 计算朝向（切线方向）
orientations = []
for i, t in enumerate(time_steps):
    # 指向轨迹切线方向
    yaw = t + np.pi/2
    R = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw),  np.cos(yaw), 0],
        [0,            0,           1]
    ])
    orientations.append(R)

# 构造轨迹的SE(3)表示
trajectory = []
for R, t in zip(orientations, positions):
    T = SE3_from_Rt(R, t)
    trajectory.append(T)
trajectory = np.array(trajectory)

# 计算相邻帧之间的相对变换
relative_transforms = []
for i in range(len(trajectory) - 1):
    rel_T = SE3_diff(trajectory[i], trajectory[i+1])
    relative_transforms.append(rel_T)

print(f"轨迹包含 {len(trajectory)} 个姿态")
print(f"计算了 {len(relative_transforms)} 个相对变换")

实际应用示例

# 相机姿态估计示例
def camera_pose_estimation():
    # 模拟相机在世界坐标系中的位置和朝向
    camera_position = np.array([2, 3, 5])

    # 相机朝向原点，up方向为z轴
    forward = -camera_position / np.linalg.norm(camera_position)
    up = np.array([0, 0, 1])
    right = np.cross(forward, up)
    right = right / np.linalg.norm(right)
    up = np.cross(right, forward)

    # 构造旋转矩阵（相机坐标系到世界坐标系）
    R_cam_to_world = np.column_stack([right, up, forward])

    # 构造SE(3)变换
    T_cam_to_world = SE3_from_Rt(R_cam_to_world, camera_position)

    # 计算世界坐标系到相机坐标系的变换
    T_world_to_cam = SE3_inv(T_cam_to_world)

    return T_cam_to_world, T_world_to_cam

T_c2w, T_w2c = camera_pose_estimation()
print(f"相机到世界变换:\n{T_c2w}")
print(f"世界到相机变换:\n{T_w2c}")

性能特点

• 批量处理: 所有函数都支持批量操作，适合轨迹处理

• 数值稳定: 采用鲁棒的算法处理大角度旋转和大位移

• 高精度: 往返转换误差通常在机器精度水平(~1e-16)

• 向量化: 使用高效的NumPy操作，性能优异

• 内存优化: 避免不必要的内存分配和复制

性能基准

基于1000个变换矩阵的性能测试结果：

• SE3_Exp: ~2.5ms (6D向量→SE(3))

• SE3_exp: ~2.5ms (4×4矩阵→SE(3))

• SE3_Log: ~0.8ms (SE(3)→6D向量)

• SE3_log: ~0.9ms (SE(3)→4×4矩阵)

• SE3_mean: ~15ms (多个SE(3)→平均SE(3))

应用场景

1. 机器人学: - 运动规划和控制 - 机械臂正逆运动学 - 移动机器人定位

2. SLAM系统: - 相机姿态估计 - 关键帧优化 - 回环检测

3. 计算机视觉: - 多视图几何 - 姿态估计 - 3D重建

4. 增强现实: - 设备追踪 - 虚实融合 - 标定校准

5. 自动驾驶: - 车辆定位 - 传感器融合 - 轨迹规划

注意事项

1. 输入格式: 单个矩阵输入为4×4，批量输入为N×4×4

2. 向量格式: 6D向量格式为[ρ₁, ρ₂, ρ₃, θ₁, θ₂, θ₃]，前3维为平移，后3维为旋转

3. 约定: 变换矩阵采用右乘约定，即 P’ = TP

4. 角度单位: 所有角度使用弧度制

5. 数值精度: 建议使用float64以获得最佳精度

6. 大角度处理: 算法对大角度旋转具有数值稳定性

常见错误与解决方案

1. 维度错误: 确保输入数组的形状正确，使用reshape调整维度

2. 非刚体变换: 检查输入矩阵是否满足SE(3)的约束条件

3. 奇异情况: 180度旋转附近注意数值稳定性

4. 内存问题: 大批量处理时注意内存使用，考虑分批处理