Sophus Python API 深度解析:ORB-SLAM3 内置李群库的符号计算接口
Sophus Python API 深度解析:ORB-SLAM3 内置李群库的符号计算接口
本篇技术指南围绕 ORB-SLAM3 内置第三方库 Sophus 的 Python API(即 pysophus)展开。pysophus 是 Sophus 的符号计算层:它以 SymPy 为后端实现 SO(2)/SO(3)/SE(2)/SE(3) 等李群,并用符号微分验证、生成 C++ 实现所需的导数公式。读完本文,你将掌握 pysophus 的模块划分、各类的数学约定与核心运算(指数映射、对数映射、hat/vee、伴随微分),理解其"符号求导 → CSE 公共子表达式消除 → 生成 C++ 代码"的完整流水线,并能在 ORB-SLAM3 工程中把 Python 推导结果与 C++ 侧 Sophus::SE3f、Sophus::Sim3f 的使用对应起来。
pysophus.rst:Sophus Python API 文档的入口
Sophus 的文档体系由 rst-dir 目录下的 Sphinx 工程组织。<a href="https://link.gitcode.com/i/3c55dc1ebcb82344385d9872c9bdb9cf" target="_blank">pysophus.rst</a> 是其中专门承载 Python API 的页面,全文仅由 7 个 automodule 指令组成:
.. automodule:: sophus.matrix
:members:
.. automodule:: sophus.complex
:members:
.. automodule:: sophus.quaternion
:members:
.. automodule:: sophus.so2
:members:
.. automodule:: sophus.so3
:members:
.. automodule:: sophus.se2
:members:
.. automodule:: sophus.se3
:members:
这 7 个 automodule 指令通过 sphinx.ext.autodoc 扩展自动提取对应 Python 模块中的类、方法与 docstring,将其渲染为 API 参考页面。因此,该文档所定义"Python API 的官方范围"就是这 7 个模块:matrix(矩阵工具)、complex(复数)、quaternion(四元数)、so2、so3(特殊正交群)、se2、se3(特殊欧氏群)。
配套的 conf.py 揭示了文档的构建方式:sys.path 被插入 ../py,使得 sophus.* 模块可被 autodoc 导入;同时加载 doxyrest 与 cpplexer 扩展,primary_domain = 'cpp'、highlight_language = 'cpp',主题为 sphinx_rtd_theme——这说明 Python API 文档与 C++ API 文档(由 Doxyrest 生成)被集成在同一个文档站中,pysophus 页面只是整个 Sophus 文档体系的一环。文档入口索引见 page_index.rst,其中通过 toctree 将 pysophus 编入目录。
Python API 的定位:面向符号计算的"影子实现"
从仓库结构可以清晰地推断出 pysophus 的设计意图。Sophus 本体是 C++ 实现的李群库,如 README.rst 所述,它提供用于 2D/3D 几何问题的 SO(2)、SO(3)(旋转)以及 SE(2)、SE(3)(刚体变换)等群。而 <a href="https://link.gitcode.com/i/74f6f122e63d7c52d15f4cab766dbf69" target="_blank">py/sophus</a> 下的 Python 模块并非为了"用 Python 重复实现一遍",而是:
- 符号验证:以 SymPy 符号变量为输入,实现与 C++ 相同数学定义的李群运算,并借助符号化简在测试中验证
exp∘log = identity等恒等式; - 符号求导:提供
calc_Dx_exp_x、calc_Dx_this_mul_exp_x_at_0等导数辅助方法,通过sympy.diff求解析导数,再用limit(x, 0)求原点处 Jacobian; - 代码生成:通过 cse_codegen.py 将符号表达式做公共子表达式消除(CSE)后输出为 C++ 代码,产物保存在 cpp_gencode 目录,供 C++ 实现参考。
这也是为什么 make_docs.sh 的流水线是 doxygen → doxyrest → sphinx-build:C++ 头文件与 Python 模块共用一套文档站,Python API 页面对应地作为"符号参考层"出现在 C++ 文档旁。
环境要求与运行测试
pysophus 依赖 SymPy 与 Python 3。在 matrix.py 第 4 行有 assert sys.version_info >= (3, 5),即要求 Python 3.5 及以上版本。install_docs_deps.sh 中通过 pip3 install sympy 安装符号计算依赖(同时安装 sphinx==2.0.1、sphinx_rtd_theme 等文档构建依赖)。
每个模块底部都自带 unittest 测试类,可以单独运行,例如:
python3 -m sophus.so3
python3 -m sophus.se3
批量运行则由 run_tests.sh 依次执行 7 个模块的测试(complex、quaternion、dual_quaternion、so2、se2、so3、se3),任一失败即记录非零退出码:
python3 -m sophus.complex || EXIT=$?
python3 -m sophus.quaternion || EXIT=$?
python3 -m sophus.dual_quaternion || EXIT=$?
python3 -m sophus.so2 || EXIT=$?
python3 -m sophus.se2 || EXIT=$?
python3 -m sophus.so3 || EXIT=$?
python3 -m sophus.se3 || EXIT=$?
注意:dual_quaternion(对偶四元数,见 dual_quaternion.py)参与测试,但未被 pysophus.rst 收录进 API 页面——从 __init__.py 看它同样未被导出,属于"内部/测试覆盖"模块。
模块总览与顶层导出接口
__init__.py 定义了顶层导出,即使用 import sophus 后可直接访问的符号:
| 类别 | 导出符号 |
|---|---|
| 代码生成 | cse_codegen |
| 矩阵工具 | dot、squared_norm、Vector2、Vector3、Vector6、ZeroVector2、ZeroVector3、ZeroVector6、proj、unproj |
| 基础代数 | Complex、Quaternion |
| 李群 | So2、So3、Se3 |
注意 Se2 不在顶层导出之列(需 from sophus.se2 import Se2),但它是 pysophus.rst 明确收录的 API 模块,功能完备。各模块中的测试代码大量使用 sophus.Vector3(...)、sophus.Quaternion(...) 等顶层符号,说明顶层导出就是为测试与用户代码准备的"快捷命名空间"。
基础工具模块:sophus.matrix
matrix.py 提供所有模块共用的矩阵与向量工具,全部基于 SymPy 的 sympy.Matrix:
dot(left, right):两个sympy.Matrix的逐元素内积(sum(left[r,c] * right[r,c])),要求两边都是矩阵,用于实现向量点积与范数;squared_norm(m):dot(m, m),即平方范数;Vector2(x, y)/Vector3(x, y, z)/Vector6(a, b, c, d, e, f):分别构造 2×1、3×1、6×1 符号列向量;ZeroVector2()/ZeroVector3()/ZeroVector6():对应维数的零向量;proj(v):把 n×1 向量做齐次归一化,取前m-1个分量除以末分量v[m-1],返回 (m-1)×1 向量——即投影运算;unproj(v):逆操作,在向量尾部拼接一个 1,得到齐次坐标。
proj/unproj 的组合在 SE(2)/SE(3) 测试中用于统一"直接乘法"与"齐次矩阵乘法"两种点变换视角。以 Se3.test_matrix 为例(se3.py 第 195-202 行):
p1_foo = T_foo_bar * point_bar
p2_foo = sophus.proj(Tmat_foo_bar * sophus.unproj(point_bar))
self.assertEqual(sympy.simplify(p1_foo - p2_foo), sophus.ZeroVector3())
它断言:用 Se3 对象直接作用 3×1 点向量,与先 unproj 成齐次坐标、经 4×4 齐次矩阵变换、再 proj 归一化的结果符号相等。
复数类:sophus.complex
complex.py 定义 Complex,内部以 (real, imag) 两个 SymPy 表达式存储,是 SO(2) 的内部表示(单位复数)。关键成员:
__mul__:复乘(a+bi)(c+di) = (ac−bd) + (ad+bc)i;__truediv__(scalar):按标量除法(注意:只支持标量除法,复数除法经conj()/squared_norm()组合实现);__getitem__:索引约定[real, imag](key == 0返回实部,否则返回虚部);squared_norm():real² + imag²;conj():共轭;inv():conj() / squared_norm();identity():(1, 0);zero():(0, 0);subs(x, y)与simplify():分别做符号代入与化简,用于测试中验证恒等式;- 导数矩阵:
Da_a_mul_b(a, b)返回复乘关于左乘因子 a 的 Jacobian[[b.real, -b.imag], [b.imag, b.real]];Db_a_mul_b(a, b)返回关于右乘因子 b 的 Jacobian。
测试类 TestComplex 用符号 x, y, u, v 构造两个复数,验证 a * a.inv() == identity(左逆、右逆均成立),并用 sympy.diff 直接求导结果比对 Da_a_mul_b/Db_a_mul_b 的解析公式——这是贯穿全部模块的验证模式:数值/符号求导结果必须与手写解析导数矩阵完全一致。
四元数类:sophus.quaternion
quaternion.py 定义 Quaternion(real, vec),由实部标量 real 与 3×1 虚部向量 vec 组成,构造时断言 vec 必须是形状为 (3, 1) 的 sympy.Matrix。它是 SO(3) 与 SE(3) 旋转部分的内部表示。关键成员:
__mul__:四元数乘法q·r = (q[3]·r[3] − vec_q·vec_r) + (q[3]·vec_r + r[3]·vec_q + vec_q × vec_r),即实部用点积、虚部用叉积的标准公式;__getitem__:索引约定<a href="https://link.gitcode.com/i/9e92b0fe1ad7581b2694aeaf844219f0" target="_blank">vec0, vec1, vec2, real]——与 Eigen 四元数coeffs()的(imag0, imag1, imag2, real)顺序一致(这点在 [HelloSO3.cpp 第 35-36 行的注释中亦有说明);squared_norm():‖vec‖² + real²;conj():(real, −vec);inv():conj() / squared_norm();identity():(1, [0,0,0]);zero():(0, [0,0,0]);- 导数矩阵
Da_a_mul_b/Db_a_mul_b:返回四元数乘法关于左/右因子的 4×4 Jacobian,形如
Da: | y v2 -v1 v0 |
| -v2 y v0 v1 |
| v1 -v0 y v2 |
| -v0 -v1 -v2 y |
测试 TestQuaternion 同样验证 a * a.inv() == identity() 及两个导数矩阵与 sympy.diff 结果逐元素相等。
SO(2):So2
so2.py 实现 2 维特殊正交群(行列式为 1 的正交矩阵),类 docstring 明确其"内部用单位复数 z 表示"。核心接口:
exp(theta):指数映射,返回Complex(cos(theta), sin(theta));log():对数映射,atan2(imag, real);hat(theta):李代数映射,[[0, -theta], [theta, 0]];matrix():旋转矩阵[[real, -imag], [imag, real]];__mul__:重载两种语义——若右操作数为 2×1sympy.Matrix,执行点变换R·p;若为So2,执行旋转复合z1·z2(左乘);__getitem__:委托给self.z,即[real, imag];- 导数辅助:
calc_Dx_exp_x(x):对So2.exp(x)[r]逐分量求∂/∂x,得到 2×1 Jacobian;Dx_exp_x_at_0():原点处解析值[0, 1];calc_Dx_exp_x_at_0(x):用.limit(x, 0)求极限,测试中与解析值比对;calc_Dx_this_mul_exp_x_at_0(x):∂(self · exp(x))/∂x在 0 处极限,即"左乘扰动"Jacobian;Dxi_x_matrix(x, i):旋转矩阵对第 i 个分量(i∈{0,1})的偏导:i=0为[[1,0],[0,1]],i=1为[[0,-1],[1,0]];Dx_exp_x_matrix_at_0():等于So2.hat(1)。
测试覆盖 exp∘log 还原(角度 0, 0.5, 0.1)、So2·p 与 matrix()·p 等价性,以及各类导数的解析值 vs 符号求导/极限比对。
SO(3):So3
so3.py 实现 3 维特殊正交群,类 docstring 说明"内部用单位四元数 q 表示"。核心接口:
exp(v):指数映射,theta = ‖v‖,返回四元数(cos(theta/2), sin(theta/2)/theta * v);log():对数映射,n = ‖vec‖,返回2·atan(n/real)/n · vec;inverse():取共轭四元数;hat(o):反对称矩阵[[0, -o[2], o[1]], [o[2], 0, -o[0]], [-o[1], o[0], 0]];vee(Omega):hat 的逆,把形如[[0,-c,b],[c,0,-a],[-b,a,0]]的矩阵映射回 3 向量(c? )(按源码实现取(Omega[2,1], Omega[0,2], Omega[1,0]));matrix():由四元数直接展开的 3×3 旋转矩阵;__mul__:3×1 点向量 →(q * Quaternion(0, p) * q.conj()).vec(共轭旋转);So3→ 四元数相乘复合;- 导数辅助:
calc_Dx_exp_x(4×3)、Dx_exp_x_at_0([[0.5,0,0],[0,0.5,0],[0,0,0.5],[0,0,0]],即 0.5·I 后补零行)、calc_Dx_exp_x_at_0(对前两分量subs(x[i], 0)后对x[2]取limit(x[2], 0))、calc_Dx_this_mul_exp_x_at_0、calc_Dx_exp_x_mul_this_at_0(右乘扰动)、Dxi_x_matrix(4 个 3×3 偏导矩阵)与Dxi_exp_x_matrix_at_0(i)(返回So3.hat(e_i))。
测试 test_exp_log 对三个旋转向量(如 (0.1, 0.1, 0.1))验证 exp∘log 逐分量还原;test_derivatives 将 calc_* 符号求导结果与手写解析矩阵逐元素比对为零矩阵;test_codegen 则把 So3.calc_Dx_exp_x 的符号结果经 cse_codegen 生成 C++ 代码流,并与 So3_Dx_exp_x.cpp 逐行比对(生成开关为源码中的 if False,默认校验模式)。
SE(2):Se2
se2.py 实现 2 维刚体变换群,内部为"单位复数 z + 平移 2 向量 t"。核心接口:
exp(v):输入(upsilon0, upsilon1, theta)。theta为旋转角,取a = imag/theta、b = (1 - real)/theta,平移为(a·v0 − b·v1, b·v0 + a·v1)——即经典的"平移由旋转矩阵 V 算子作用"的 2D 形式;log():先取theta = so2.log(),构造V_inv = [[a, half], [-half, a]](a = -half·imag/(real − 1)),反解upsilon = V_inv · t;hat(v):3 向量 → 3×3 矩阵[[So2.hat(theta) | upsilon], [0 0 0 | 0]](实际为 2×2 左上角 + 右上平移列 + 底行[0,0,0]);matrix():3×3 齐次矩阵[[R, t], [0 0 1]];__mul__:2×1 点向量 →so2·p + t;Se2→ 复合(so2₁·so2₂, t₁ + so2₁·t₂);__getitem__:索引约定[q0, q1, t0, t1](源码注释写作[q0,q1,q2,q3,t0,t1,t2]属于从 SE(3) 复制的笔误,实现为key < 2取so2[key],否则取t[key-2],共 4 个元素);- 导数辅助:
calc_Dx_exp_x(4×3)、Dx_exp_x_at_0([[0,0,0],[0,0,1],[1,0,0],[0,1,0]])、calc_Dx_this_mul_exp_x_at_0、Dxi_x_matrix(i<2复用So2的偏导并补零行/列,i≥2时在M[i-2, 2]置 1)、Dxi_exp_x_matrix_at_0(i)(Se2.hat(e_i))。
测试 test_matrix 用 proj/unproj 验证对象乘法与齐次矩阵乘法的等价性;test_derivatives 覆盖 i ∈ {0..3} 的矩阵偏导与 i ∈ {0..2} 的 exp 矩阵偏导。
SE(3):Se3
se3.py 实现 3 维刚体变换群,是 ORB-SLAM3 相机位姿与 IMU 位姿的最核心数学对象。内部为"单位四元数 q(旋转)+ 3 向量 t(平移)"。核心接口:
exp(v):输入 6 向量(upsilon, omega)。so3 = So3.exp(omega),并构造经典 V 算子V = I + (1−cos θ)/θ² · Ω + (θ − sin θ)/θ³ · Ω²(其中θ = ‖omega‖、Ω = So3.hat(omega)),平移取V · upsilon;log():反解omega = so3.log(),用V_inv = I − 0.5Ω + (1 − θ·cos(θ/2)/(2·sin(θ/2)))/θ² · Ω²求upsilon = V_inv · t,输出 6 向量(upsilon, omega);inverse():(so3.inverse(), so3.inverse()·(−t));hat(v):R⁶ → 4×4 矩阵[[hat(omega), upsilon], [0 0 0 0]];vee(Omega):4×4 → R⁶,取右上平移列与So3.vee的 3×3 旋转子矩阵,逆运算;matrix():4×4 齐次矩阵[[R, t], [0 0 0 1]];__mul__:3×1 点向量 →so3·p + t;Se3→ 标准复合;__getitem__:[q0, q1, q2, q3, t0, t1, t2]共 7 元素;- 导数辅助:
calc_Dx_exp_x(x):7×6 Jacobian(对 6 个输入分量逐个求偏导);Dx_exp_x_at_0():原点解析值(见 se3.py):
即旋转部分 0.5·I、平移部分单位阵,物理含义是"微小扰动下 exp 的一阶线性化";[[0, 0, 0, 0.5, 0, 0], [0, 0, 0, 0, 0.5, 0], [0, 0, 0, 0, 0, 0.5], [0, 0, 0, 0, 0, 0], [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]]calc_Dx_exp_x_at_0:对前 5 个分量subs(x[i], 0)、对x[5]取limit的符号极限;calc_Dx_this_mul_exp_x_at_0:左乘扰动∂(self · exp(x))/∂x在 0 处极限;Dxi_x_matrix(7 个 4×4 偏导)、Dxi_exp_x_matrix_at_0(i)(Se3.hat(e_i))。
测试 test_exp_log 对三个 6 向量(含平移与旋转分量)验证逐分量还原;test_derivatives 覆盖 7×6、4×4、4×4 三类矩阵的解析值与符号求导一致性。
符号导数与 C++ 代码生成:从 SymPy 到 .cpp
pysophus 最有价值的机制在 cse_codegen.py 中:cse_codegen(symbols) 先调用 sympy.cse(symbols, sympy.numbered_symbols("c")) 做公共子表达式消除,再用 sympy.printing.ccode 依次输出:
- 每条中间子表达式为一行
Scalar const cN = ...;; - 最终结果为
result[...] = ...;赋值语句。
以生成的 Se3_Dx_exp_x.cpp 为例,开头是 c0 = pow(omega[0], 2)、c4 = sqrt(c3)、c7 = sin(c6) 等一系列中间量,结尾 42 行 result[i] 完成 7×6 的 Jacobian 填充。这正是 exp 导数表达式中大量重复的 cos/sin/sqrt/pow 项被复用、避免冗余浮点运算的产物。
生成流程的"测试闭环"在各模块的 test_codegen 中:把 calc_Dx_exp_x(...) 或 calc_Dx_this_mul_exp_x_at_0(...) 送入 cse_codegen,然后与 cpp_gencode 下对应 .cpp 文件逐行比对(if False 分支为重新生成开关,默认走 else 分支做比对)。8 个生成文件覆盖了 So2/So3/Se2/Se3 的 Dx_exp_x 与 Dx_this_mul_exp_x_at_0 两类导数。
测试体系:恒等式、矩阵等价与导数三重验证
pysophus 每个模块内嵌 unittest.TestCase,验证策略高度统一:
- 恒等式验证(
test_muliplications/test_exp_log):复数与四元数断言a * a.inv() == identity左右逆;李群对若干随机(固定)李代数向量断言exp(v).log() == v; - 矩阵等价(
test_matrix):So2·p与Rmat·p、Se3·p与proj(Tmat·unproj(p))符号化简为零向量; - 导数一致性(
test_derivatives):calc_*(sympy.diff+limit)结果与手写解析矩阵(D*系列)逐元素相减为零矩阵; - 代码生成回归(
test_codegen):生成的 C++ 代码流与 cpp_gencode 预存文件逐行一致。
这意味着 C++ Sophus 中出现的导数 Jacobian(例如 SE(3) 优化时需要用到的扰动模型)在 Python 侧都有一份"符号公证",可由 run_tests.sh 一键回归。
Python API 与 C++ 实现、ORB-SLAM3 工程的对应关系
pysophus 的类设计直接对应 C++ 侧实现:C++ 头文件 se3.hpp 中 Sophus::SE3 由 SO3Type 与 TranslationType 组合而成,并提供 SE3d/SE3f 类型别名——与 Python Se3(so3, t) 的结构一一对应;HelloSO3.cpp 展示了 C++ 侧 SO3d::rotX/rotY/rotZ、群乘法与 unit_quaternion() 的用法,其四元数系数顺序注释与 Python Quaternion.__getitem__ 的 [imag0, imag1, imag2, real] 约定完全吻合。
在 ORB-SLAM3 主工程中,Sophus 的 C++ 类型被广泛使用:
- Converter.cc:
toSE3Quat(const Sophus::SE3f&)将 Sophus 位姿转为 g2o 的SE3Quat;toSophus(const cv::Mat&)把 4×4 变换矩阵封装为Sophus::SE3<float>;toSophus(const g2o::Sim3&)构造Sophus::Sim3f(含尺度),对应 Python 侧虽未直接实现Sim3,但其RxSO3/So3/Se3组件均与 pysophus 的代数对象同源; - Frame.cc:
SetPose(const Sophus::SE3<float>&)、GetImuPose()、GetRelativePoseTrl/Tlr()等围绕Sophus::SE3f完成相机/IMU 位姿的表示与变换,正是Se3.__mul__、Se3.inverse()语义的工程落地; - CameraModels/KannalaBrandt8.cpp:三角化与相对位姿函数直接以
Sophus::SE3f&为参数,并用Sophus::SO3f::hat(t12)构造反对称矩阵; - G2oTypes.cc:IMU 因子 Jacobian 中反复使用
Sophus::SO3d::hat(...)表达旋转向量叉乘矩阵,与 PythonSo3.hat的公式一致。
因此,当你在 ORB-SLAM3 中排查位姿求导、Sim3 尺度因子或 IMU 预积分 Jacobian 时,pysophus 的 Python 源码(尤其是 calc_Dx_* 系列与 cpp_gencode 的生成产物)就是最直接的"公式参考书"。
如何构建 pysophus 文档与复现验证
Sophus 的整套文档(含本文所述的 Python API 页面)由 make_docs.sh 构建,三步流水线:
doxygen doxyfile # 生成 C++ 头文件的 XML 描述
doxyrest -c doxyrest-config.lua # 将 XML 转为 Sphinx 可用的 RST
sphinx-build -b html rst-dir html-dir # 构建最终 HTML 文档
其中第二步依赖 doxyrest_b 工具链,install_docs_deps.sh 会安装 doxygen、liblua5.3-dev、sphinx==2.0.1、sphinx_rtd_theme、sympy,并克隆固定 commit 的 doxyrest_b 仓库后 cmake 编译。构建完成后,pysophus.rst 中的 7 个 automodule 将由 sphinx.ext.autodoc 展开为带成员列表的 API 页面,与 C++ 文档并排出现在 html-dir 下。
若只想快速验证符号推导的正确性,无需构建文档,直接运行:
cd Thirdparty/Sophus/py
python3 -m sophus.se3
输出 unittest 结果即完成对 Se3.exp/log、7×6 Jacobian 与代码生成回归的全部校验。
小结
<a href="https://link.gitcode.com/i/3c55dc1ebcb82344385d9872c9bdb9cf" target="_blank">pysophus.rst</a> 虽然只是一页 automodule 指令,但它划定了 Sophus Python API 的完整边界:matrix、complex、quaternion 是代数基础,so2/so3/se2/se3 是四个核心李群。pysophus 的价值不在于"Python 重写一遍 Sophus",而在于用 SymPy 为 C++ 实现提供可验证的符号镜像——指数/对数映射的解析公式、扰动模型的 Jacobian、乃至 CSE 优化后的 C++ 代码,都在这一层完成推导、测试与回归。理解这层接口,等于同时掌握了 Sophus 的数学定义与 ORB-SLAM3 位姿优化背后的导数来源。