【学习笔记】Sophus (Python) 使用文档

以下是一份针对 Sophus 库的 Python 使用文档，涵盖基础概念、安装方法、核心功能及代码示例。内容围绕 SO3（3D旋转群）和 SE3（3D刚体变换群）展开，适合机器人学、SLAM、三维几何等领域。

Sophus (Python) 使用文档

Sophus 简介
安装方法
核心对象与操作
- SO3：3D旋转群
- SE3：3D刚体变换群
常用功能与示例
- 创建旋转/变换对象
- 转换不同表示形式
- 位姿插值（Slerp/Lerp）
- 李代数与指数映射
- 逆变换与组合变换
注意事项
完整代码示例

1. Sophus 简介

Sophus 是一个用于处理 李群（Lie Groups） 的C++/Python库，支持 SO2/SE2（2D旋转/变换）和 SO3/SE3（3D旋转/变换）。

核心功能：
- 旋转与变换的表示（四元数、旋转矩阵、轴角、李代数等）
- 李群与李代数的相互转换（指数/对数映射）
- 位姿插值（Slerp）、逆变换、组合变换
适用场景：机器人运动学、SLAM、三维重建、姿态估计等。

2. 安装方法

通过 pip 安装 Sophus 的 Python 绑定（需提前安装 Eigen 库）：

pip install sophus

3. 核心对象与操作

3.1 SO3 (3D旋转群)

表示三维空间中的旋转，支持四元数、旋转矩阵、轴角等表示形式。

创建 SO3对象

from sophus.so3 import SO3
import numpy as np# 从四元数创建 (w, x, y, z 顺序)
q = np.array([0.707, 0.0, 0.707, 0.0])  # 绕x轴旋转90度
so3_quat = SO3(q)# 从旋转矩阵创建
R = np.eye(3)
so3_rotmat = SO3.from_matrix(R)# 从轴角创建 (旋转向量，模长为旋转角度)
axis_angle = np.array([0, 0, np.pi/2])  # 绕z轴旋转90度
so3_axis_angle = SO3.exp(axis_angle)

转换表示形式

# 转为四元数 [w, x, y, z]
q = so3_axis_angle.quaternion()# 转为旋转矩阵 (3x3)
R = so3_axis_angle.matrix()# 转为轴角（李代数）
log = so3_axis_angle.log()  # 返回旋转向量

3.2 SE3 (3D刚体变换群)

表示三维空间中的刚体变换（旋转 + 平移），由 SO3 和平移向量组成。

创建 SE3 对象

from sophus.se3 import SE3# 从旋转矩阵 + 平移向量创建
R = np.eye(3)
t = np.array([1.0, 2.0, 3.0])
se3_rotmat = SE3(R, t)# 从四元数 + 平移向量创建
q = np.array([0.707, 0.0, 0.707, 0.0])  # 绕x轴旋转90度
se3_quat = SE3(SO3(q), t)# 从变换矩阵 (4x4) 创建
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
se3_matrix = SE3.from_matrix(T)

转换表示形式

# 获取旋转部分 (SO3)
so3_part = se3_matrix.so3()# 获取平移部分 (3x1)
t_part = se3_matrix.translation()# 转为变换矩阵 (4x4)
T = se3_matrix.matrix()

4. 常用功能与示例

4.1 位姿插值（Slerp + Lerp）

在 SE3 中插值旋转（Slerp）和平移（Lerp）：

def interpolate_se3(pose1, pose2, t):""" SE3 插值：旋转用 Slerp，平移用 Lerp """se3_1 = SE3.from_matrix(pose1)se3_2 = SE3.from_matrix(pose2)# 旋转插值 (SO3)so3_interp = SO3.slerp(se3_1.so3(), se3_2.so3(), t)# 平移插值 (Lerp)t_interp = (1 - t) * se3_1.translation() + t * se3_2.translation()return SE3(so3_interp, t_interp).matrix()# 示例：从 pose1 到 pose2 的中间位姿
pose_mid = interpolate_se3(pose1, pose2, 0.5)

4.2 李代数与指数映射

# 从李代数生成 SE3
se3 = SE3.exp(np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]))  # 前3维平移，后3维旋转# 从 SE3 提取李代数
lie_alg = se3.log()  # 6维向量 [t_x, t_y, t_z, ω_x, ω_y, ω_z]

4.3 逆变换与组合变换

# 逆变换
se3_inv = se3.inverse()# 组合变换：se3_new = se3_a * se3_b
se3_new = se3_a * se3_b

5. 注意事项

四元数顺序：Sophus 使用 [w, x, y, z] 顺序，与其他库（如PyTorch3D的xyzw）可能不同。
版本兼容性：Python 版 Sophus 功能可能比 C++ 版少，建议参考官方文档。
性能问题：Python 接口适合原型设计，性能关键场景建议使用 C++。

6. 完整代码示例

import numpy as np
from sophus.so3 import SO3
from sophus.se3 import SE3# 创建两个 SE3 位姿
pose1 = SE3(SO3.exp([0, 0, 0]), np.array([1, 2, 3]))
pose2 = SE3(SO3.exp([0, 0, np.pi/2]), np.array([4, 5, 6]))# 插值
pose_mid = interpolate_se3(pose1.matrix(), pose2.matrix(), 0.5)
print("插值后的变换矩阵:\n", pose_mid)# 计算相对变换
relative_pose = pose1.inverse() * pose2
print("相对变换的李代数:\n", relative_pose.log())