Sophus Python API 深度解析:ORB-SLAM3 内置李群库的符号计算接口

原创2026-10-13 19:39:381,951 阅读
文章标签:计算机视觉机器人科研

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 重复实现一遍",而是:

  1. 符号验证:以 SymPy 符号变量为输入,实现与 C++ 相同数学定义的李群运算,并借助符号化简在测试中验证 exp∘log = identity 等恒等式;
  2. 符号求导:提供 calc_Dx_exp_x、calc_Dx_this_mul_exp_x_at_0 等导数辅助方法,通过 sympy.diff 求解析导数,再用 limit(x, 0) 求原点处 Jacobian;
  3. 代码生成:通过 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×1 sympy.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, 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]]
      
      即旋转部分 0.5·I、平移部分单位阵,物理含义是"微小扰动下 exp 的一阶线性化";
    • 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 依次输出:

  1. 每条中间子表达式为一行 Scalar const cN = ...;;
  2. 最终结果为 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(...) 表达旋转向量叉乘矩阵,与 Python So3.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 位姿优化背后的导数来源。

登录后查看全文
ORB_SLAM3