SO3 旋转矩阵工具 (SO3)

pywayne.vio.SO3.SO3_Exp(rotvec: ndarray) → ndarray

Convert Lie algebra vector (rotation vector) to SO(3) rotation matrix. :param rotvec: Nx3 or 3 rotation vector (Lie algebra vector)

Returns:: Nx3x3 or 3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_Log(R: ndarray) → ndarray

Convert SO(3) rotation matrix to Lie algebra vector (rotation vector). :param R: Nx3x3 or 3x3 rotation matrix

Returns:: Nx3 or 3 rotation vector (Lie algebra vector)
Return type:: rotvec

pywayne.vio.SO3.SO3_diff(R1: ndarray, R2: ndarray, from_1_to_2: bool = True) → ndarray

Compute the difference between two SO(3) rotation matrices. :param R1: 3x3 rotation matrix :param R2: 3x3 rotation matrix :param from_1_to_2: if True, compute R2 @ R1.T, otherwise compute R1.T @ R2

Returns:: R1.T @ R2 if from_1_to_2 is True, otherwise R1 @ R2.T

pywayne.vio.SO3.SO3_exp(omega_hat: ndarray) → ndarray

Convert Lie algebra matrix (skew-symmetric matrix) to SO(3) rotation matrix. :param omega_hat: Nx3x3 or 3x3 skew-symmetric matrix (Lie algebra matrix)

Returns:: Nx3x3 or 3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_from_axis_angle(axis: ndarray, angle: float) → ndarray

Convert an axis-angle to a SO(3) rotation matrix. :param axis: Nx3 axis :param angle: Nx1 angle, rad

Returns:: Nx3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_from_euler(euler_angles: ndarray, axes: str = 'zyx', intrinsic: bool = True) → ndarray

Convert Euler angles to a SO(3) rotation matrix. :param euler_angles: Nx3 Euler angles, rad :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:: Nx3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_from_quat(q: ndarray) → ndarray

Convert a quaternion to a SO(3) rotation matrix. :param q: Nx4 quaternion, wxyz, Hamilton convention

Returns:: Nx3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_inv(R: ndarray) → ndarray

Compute the inverse of SO(3) rotation matrix. For SO(3) matrices, the inverse is simply the transpose. :param R: Nx3x3 or 3x3 rotation matrices

Returns:: Nx3x3 or 3x3 inverse rotation matrices
Return type:: R_inv

pywayne.vio.SO3.SO3_log(R: ndarray) → ndarray

Convert SO(3) rotation matrix to Lie algebra matrix (skew-symmetric matrix). :param R: Nx3x3 or 3x3 rotation matrix

Returns:: Nx3x3 or 3x3 skew-symmetric matrix (Lie algebra matrix)
Return type:: log_map

pywayne.vio.SO3.SO3_mean(R: ndarray) → ndarray

Compute the mean of multiple SO(3) rotation matrices using scipy. :param R: Nx3x3 rotation matrices

Returns:: 3x3 mean rotation matrix
Return type:: mean_R

pywayne.vio.SO3.SO3_mul(R1: ndarray, R2: ndarray) → ndarray

Multiply two SO(3) rotation matrices. :param R1: 3x3 rotation matrix :param R2: 3x3 rotation matrix

Returns:: R1 @ R2

pywayne.vio.SO3.SO3_skew(vec: ndarray) → ndarray

Convert a vector to a skew-symmetric matrix. :param vec: Nx3 vector or 3-element vector

Returns:: Nx3x3 skew-symmetric matrix
Return type:: skew_mat

pywayne.vio.SO3.SO3_to_axis_angle(R: ndarray) → ndarray

Convert a SO(3) rotation matrix to an axis-angle. :param R: Nx3x3 rotation matrix

Returns:: Nx3 axis angle: Nx1 angle
Return type:: axis

pywayne.vio.SO3.SO3_to_euler(R: ndarray, axes: str = 'zyx', intrinsic: bool = True) → ndarray

Convert a SO(3) rotation matrix to Euler angles. :param R: Nx3x3 rotation matrix :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:: Nx3 Euler angles, rad
Return type:: euler_angles

pywayne.vio.SO3.SO3_to_quat(R: ndarray) → ndarray

Convert a SO(3) rotation matrix to a quaternion. :param R: Nx3x3 rotation matrix

Returns:: Nx4 quaternion, wxyz, Hamilton convention
Return type:: q

pywayne.vio.SO3.SO3_unskew(skew: ndarray) → ndarray

Convert a skew-symmetric matrix to a vector. :param skew: Nx3x3 or 3x3 skew-symmetric matrix

Returns:: Nx3 or 3 vector
Return type:: vec

pywayne.vio.SO3.check_SO3(R: ndarray) → bool

Check if a matrix is a valid SO(3) rotation matrix. :param R: 3x3 rotation matrix

Returns:: True if R is a valid SO(3) rotation matrix, False otherwise

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

数学背景

SO(3)是特殊正交群，表示三维欧几里得空间中的旋转。SO(3)群的元素是3×3的正交矩阵，满足：

正交性: \(R^T R = I\)
行列式为1: \(\det(R) = 1\)

相应的李代数so(3)由3×3反对称矩阵组成，通过指数映射连接到李群SO(3)。

主要功能分类

基础操作函数

pywayne.vio.SO3.check_SO3(R: ndarray) → bool

Check if a matrix is a valid SO(3) rotation matrix. :param R: 3x3 rotation matrix

Returns:: True if R is a valid SO(3) rotation matrix, False otherwise

验证矩阵是否为有效的SO(3)旋转矩阵。

pywayne.vio.SO3.SO3_mul(R1: ndarray, R2: ndarray) → ndarray

Multiply two SO(3) rotation matrices. :param R1: 3x3 rotation matrix :param R2: 3x3 rotation matrix

Returns:: R1 @ R2

计算两个旋转矩阵的乘积，实现旋转组合。

pywayne.vio.SO3.SO3_diff(R1: ndarray, R2: ndarray, from_1_to_2: bool = True) → ndarray

Compute the difference between two SO(3) rotation matrices. :param R1: 3x3 rotation matrix :param R2: 3x3 rotation matrix :param from_1_to_2: if True, compute R2 @ R1.T, otherwise compute R1.T @ R2

Returns:: R1.T @ R2 if from_1_to_2 is True, otherwise R1 @ R2.T

计算两个旋转矩阵之间的相对旋转。

pywayne.vio.SO3.SO3_inv(R: ndarray) → ndarray

Compute the inverse of SO(3) rotation matrix. For SO(3) matrices, the inverse is simply the transpose. :param R: Nx3x3 or 3x3 rotation matrices

Returns:: Nx3x3 or 3x3 inverse rotation matrices
Return type:: R_inv

计算旋转矩阵的逆（转置）。

反对称矩阵操作

pywayne.vio.SO3.SO3_skew(vec: ndarray) → ndarray

Convert a vector to a skew-symmetric matrix. :param vec: Nx3 vector or 3-element vector

Returns:: Nx3x3 skew-symmetric matrix
Return type:: skew_mat

将3D向量转换为反对称矩阵（skew-symmetric matrix）。

pywayne.vio.SO3.SO3_unskew(skew: ndarray) → ndarray

Convert a skew-symmetric matrix to a vector. :param skew: Nx3x3 or 3x3 skew-symmetric matrix

Returns:: Nx3 or 3 vector
Return type:: vec

从反对称矩阵提取对应的3D向量。

旋转表示转换

pywayne.vio.SO3.SO3_from_quat(q: ndarray) → ndarray

Convert a quaternion to a SO(3) rotation matrix. :param q: Nx4 quaternion, wxyz, Hamilton convention

Returns:: Nx3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_to_quat(R: ndarray) → ndarray

Convert a SO(3) rotation matrix to a quaternion. :param R: Nx3x3 rotation matrix

Returns:: Nx4 quaternion, wxyz, Hamilton convention
Return type:: q

四元数与旋转矩阵的相互转换，使用Hamilton约定(wxyz)。

pywayne.vio.SO3.SO3_from_axis_angle(axis: ndarray, angle: float) → ndarray

Convert an axis-angle to a SO(3) rotation matrix. :param axis: Nx3 axis :param angle: Nx1 angle, rad

Returns:: Nx3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_to_axis_angle(R: ndarray) → ndarray

Convert a SO(3) rotation matrix to an axis-angle. :param R: Nx3x3 rotation matrix

Returns:: Nx3 axis angle: Nx1 angle
Return type:: axis

轴角表示与旋转矩阵的相互转换。

pywayne.vio.SO3.SO3_from_euler(euler_angles: ndarray, axes: str = 'zyx', intrinsic: bool = True) → ndarray

Convert Euler angles to a SO(3) rotation matrix. :param euler_angles: Nx3 Euler angles, rad :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:: Nx3x3 rotation matrix
Return type:: R

pywayne.vio.SO3.SO3_to_euler(R: ndarray, axes: str = 'zyx', intrinsic: bool = True) → ndarray

Convert a SO(3) rotation matrix to Euler angles. :param R: Nx3x3 rotation matrix :param axes: Euler angles sequence :param intrinsic: if True, use intrinsic rotation, otherwise use extrinsic rotation

Returns:: Nx3 Euler angles, rad
Return type:: euler_angles

欧拉角与旋转矩阵的相互转换，支持多种旋转序列。

李群/李代数映射

pywayne.vio.SO3.SO3_Log(R: ndarray) → ndarray

Convert SO(3) rotation matrix to Lie algebra vector (rotation vector). :param R: Nx3x3 or 3x3 rotation matrix

Returns:: Nx3 or 3 rotation vector (Lie algebra vector)
Return type:: rotvec

SO(3) → so(3)的对数映射，输出旋转向量(3D)。

pywayne.vio.SO3.SO3_log(R: ndarray) → ndarray

Convert SO(3) rotation matrix to Lie algebra matrix (skew-symmetric matrix). :param R: Nx3x3 or 3x3 rotation matrix

Returns:: Nx3x3 or 3x3 skew-symmetric matrix (Lie algebra matrix)
Return type:: log_map

SO(3) → so(3)的对数映射，输出反对称矩阵(3×3)。

pywayne.vio.SO3.SO3_Exp(rotvec: ndarray) → ndarray

Convert Lie algebra vector (rotation vector) to SO(3) rotation matrix. :param rotvec: Nx3 or 3 rotation vector (Lie algebra vector)

Returns:: Nx3x3 or 3x3 rotation matrix
Return type:: R

so(3) → SO(3)的指数映射，从旋转向量生成旋转矩阵。

pywayne.vio.SO3.SO3_exp(omega_hat: ndarray) → ndarray

Convert Lie algebra matrix (skew-symmetric matrix) to SO(3) rotation matrix. :param omega_hat: Nx3x3 or 3x3 skew-symmetric matrix (Lie algebra matrix)

Returns:: Nx3x3 or 3x3 rotation matrix
Return type:: R

so(3) → SO(3)的指数映射，从反对称矩阵生成旋转矩阵。

平均与统计

pywayne.vio.SO3.SO3_mean(R: ndarray) → ndarray

Compute the mean of multiple SO(3) rotation matrices using scipy. :param R: Nx3x3 rotation matrices

Returns:: 3x3 mean rotation matrix
Return type:: mean_R

计算多个旋转矩阵的平均值，使用基于四元数的数值稳定方法。

数学关系

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

往返转换:

\(R = \text{SO3\_Exp}(\text{SO3\_Log}(R))\)
\(\text{SO3\_log}(R) = \text{SO3\_skew}(\text{SO3\_Log}(R))\)

群运算:

\(\text{SO3\_mul}(R_1, R_2) = R_1 \cdot R_2\)
\(\text{SO3\_inv}(R) = R^T\)

代数操作:

\(\text{SO3\_exp}(\omega^\wedge) = \exp(\omega^\wedge)\)
\(\text{SO3\_Log}(R) = \log(R)^\vee\)

使用示例

基础操作示例

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

# 创建旋转矩阵（绕Z轴90度）
angle = np.pi/2
R = np.array([
    [np.cos(angle), -np.sin(angle), 0],
    [np.sin(angle),  np.cos(angle), 0],
    [0,              0,             1]
])

# 验证是否为有效的SO(3)矩阵
print(f"是否为有效SO3: {check_SO3(R)}")

# 计算逆矩阵
R_inv = SO3_inv(R)

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

反对称矩阵操作

# 创建向量并转换为反对称矩阵
vec = np.array([1, 2, 3])
skew_mat = SO3_skew(vec)
print(f"反对称矩阵:\n{skew_mat}")

# 从反对称矩阵恢复向量
recovered_vec = SO3_unskew(skew_mat)
print(f"恢复向量: {recovered_vec}")
print(f"往返误差: {np.linalg.norm(vec - recovered_vec):.2e}")

旋转表示转换

# 四元数转换
quat = SO3_to_quat(R.reshape(1, 3, 3))
R_from_quat = SO3_from_quat(quat)
print(f"四元数转换误差: {np.linalg.norm(R - R_from_quat[0]):.2e}")

# 轴角转换
axis, angle = SO3_to_axis_angle(R.reshape(1, 3, 3))
R_from_axis_angle = SO3_from_axis_angle(axis, angle)
print(f"轴角转换误差: {np.linalg.norm(R - R_from_axis_angle[0]):.2e}")

# 欧拉角转换
euler = SO3_to_euler(R.reshape(1, 3, 3))
R_from_euler = SO3_from_euler(euler)
print(f"欧拉角转换误差: {np.linalg.norm(R - R_from_euler[0]):.2e}")

李群/李代数映射

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

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

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

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

批量处理示例

# 创建多个随机旋转
n_rotations = 100
random_axis = np.random.randn(n_rotations, 3)
random_axis = random_axis / np.linalg.norm(random_axis, axis=1, keepdims=True)
random_angles = np.random.uniform(0, np.pi, n_rotations)

# 批量生成旋转矩阵
R_batch = SO3_from_axis_angle(random_axis, random_angles)

# 批量转换为四元数
quat_batch = SO3_to_quat(R_batch)

# 计算平均旋转
R_mean = SO3_mean(R_batch)
print(f"平均旋转矩阵是否有效: {check_SO3(R_mean)}")

性能特点

批量处理: 所有函数都支持批量操作，提高计算效率
数值稳定: 采用数值稳定的算法，避免奇异情况
高精度: 往返转换误差通常在机器精度水平(~1e-16)
向量化: 使用NumPy的einsum等高效操作

应用场景

机器人学: 关节旋转、工具坐标变换
计算机视觉: 相机外参、特征匹配
SLAM: 里程计估计、姿态优化
航空航天: 姿态控制、导航系统
动画制作: 3D模型旋转、插值

注意事项

输入格式: 单个矩阵输入为3×3，批量输入为N×3×3
约定: 四元数使用Hamilton约定(wxyz)
角度单位: 所有角度使用弧度制
数值精度: 建议使用float64以获得最佳精度
奇异情况: 180度旋转附近可能出现数值不稳定