以下是一份針對 Sophus 庫的 Python 使用文檔，涵蓋基礎概念、安裝方法、核心功能及代碼示例。內容圍繞 SO3（3D旋轉群）和 SE3（3D剛體變換群）展開，適合機器人學、SLAM、三維幾何等領域。

---

Sophus (Python) 使用文檔

目錄

1. Sophus 簡介
2. 安裝方法
3. 核心對象與操作
   • SO3：3D旋轉群
   • SE3：3D剛體變換群
4. 常用功能與示例
   • 創建旋轉/變換對象
   • 轉換不同表示形式
   • 位姿插值（Slerp/Lerp）
   • 李代數與指數映射
   • 逆變換與組合變換
5. 注意事項
6. 完整代碼示例

---

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. 注意事項

1. 四元數順序：Sophus 使用 [w, x, y, z] 順序，與其他庫（如PyTorch3D的xyzw）可能不同。
2. 版本兼容性：Python 版 Sophus 功能可能比 C++ 版少，建議參考官方文檔。
3. 性能問題：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())

---

通過本文檔，您可快速掌握 Sophus 庫的核心功能，實現三維空間中的旋轉、變換及插值操作。建議結合 Sophus 官方 GitHub 深入探索高級功能。