1. 视觉SLAM位姿估计的数学根基搞视觉SLAM的人迟早都会撞上李群和李代数这堵墙。我第一次接触是在做视觉里程计优化的时候明明推导看着都对代码跑起来位姿就是发散折腾了整整两天才发现是扰动模型的雅可比矩阵写错了。从那时起我就明白这部分内容绕不过去必须啃透。视觉SLAM的核心任务之一就是估计相机在三维空间中的位姿——位置和姿态。位置是三维平移向量姿态是三维旋转。平移好办直接加法就行但旋转不行旋转矩阵的加法没有意义两个旋转矩阵相加得到的不再是旋转矩阵。这就是问题的起点旋转空间不是一个向量空间它是一个流形。李群和李代数这套数学工具就是用来在流形上做优化计算的。这篇文章适合谁看如果你正在学视觉SLAM已经了解基本的特征匹配、PnP求解但对后端优化中的雅可比推导一知半解那这篇就是写给你的。如果你已经能跑通ORB-SLAM之类的框架但想搞清楚Sophus::SE3::exp()背后到底发生了什么同样可以往下看。我会从为什么需要李群讲起把左乘扰动模型的来龙去脉拆干净最后给出可以直接对照代码的实操细节。2. 为什么旋转不能用向量直接求导2.1 旋转矩阵的约束带来的麻烦一个三维旋转矩阵 ( R ) 属于特殊正交群 ( SO(3) )它满足两个约束( R^TR I ) 且 ( \det(R) 1 )。这意味着旋转矩阵只有3个自由度但用9个元素来表示。当你用数值优化方法去更新旋转时如果直接对9个元素做加法更新后的矩阵就不再是旋转矩阵了——正交性被破坏行列式也可能不再是1。那能不能只优化3个参数可以比如用欧拉角或者四元数。但欧拉角有万向锁问题四元数有单位模长约束而且它们和旋转矩阵之间的转换在小角度附近才比较线性。更关键的是在SLAM的Bundle Adjustment中我们需要对旋转矩阵求导而旋转矩阵对加法不封闭导致导数没有良好定义。这里的关键认知是旋转矩阵本身不能直接做加法但它的切空间——李代数——是一个向量空间可以在上面做加法。2.2 从李群到李代数的直观理解李群是群也是光滑流形( SO(3) ) 和 ( SE(3) ) 都是李群。李代数对应李群在单位元处的切空间( \mathfrak{so}(3) ) 就是 ( SO(3) ) 的李代数它是一个三维向量空间元素是反对称矩阵。给定一个三维向量 ( \phi [\phi_1, \phi_2, \phi_3]^T )可以构造反对称矩阵[ \phi^\wedge \begin{bmatrix} 0 -\phi_3 \phi_2 \ \phi_3 0 -\phi_1 \ -\phi_2 \phi_1 0 \end{bmatrix} ]这个反对称矩阵通过指数映射 ( \exp(\phi^\wedge) ) 就能得到对应的旋转矩阵。反过来对数映射 ( \log(R) ) 把旋转矩阵映射回李代数向量。这就是李群和李代数之间的桥梁。直观理解李代数向量 ( \phi ) 的方向表示旋转轴模长表示旋转角度。指数映射就是从“旋转向量”到“旋转矩阵”的转换。在SLAM优化中我们不在旋转矩阵上直接更新而是在李代数上做增量更新再通过指数映射映射回旋转矩阵。这样既保证了更新后的矩阵仍然是旋转矩阵又让求导变得可行。3. 指数映射与对数映射的实操细节3.1 SO(3)上的指数映射指数映射的闭式解由罗德里格斯公式给出[ \exp(\phi^\wedge) I \frac{\sin\theta}{\theta}\phi^\wedge \frac{1-\cos\theta}{\theta^2}(\phi^\wedge)^2 ]其中 ( \theta |\phi| ) 是旋转角度。这个公式在 ( \theta \to 0 ) 时会出现 ( 0/0 ) 的问题实际代码中需要做泰勒展开近似。以Sophus库为例它在 ( \theta ) 很小时直接用近似公式// Sophus 中 SO3::exp 的近似处理伪代码示意 if (theta epsilon) { // 泰勒展开sin(theta)/theta ≈ 1 - theta^2/6 // (1-cos(theta))/theta^2 ≈ 1/2 - theta^2/24 result I (1 - theta*theta/6) * phi_hat (0.5 - theta*theta/24) * phi_hat * phi_hat; }这个细节很重要。我见过有人在自定义实现时没做小角度近似结果在优化初期旋转增量很小时数值不稳定迭代直接崩掉。3.2 SE(3)上的指数映射( SE(3) ) 的李代数 ( \mathfrak{se}(3) ) 是六维的元素为 ( \xi [\rho, \phi]^T )其中 ( \rho ) 是平移部分( \phi ) 是旋转部分。指数映射为[ \exp(\xi^\wedge) \begin{bmatrix} \exp(\phi^\wedge) J\rho \ 0 1 \end{bmatrix} ]其中 ( J ) 是左雅可比矩阵[ J \frac{\sin\theta}{\theta}I \left(1 - \frac{\sin\theta}{\theta}\right)\frac{\phi\phi^T}{\theta^2} \frac{1-\cos\theta}{\theta}\frac{\phi^\wedge}{\theta} ]这个 ( J ) 矩阵在后续扰动模型的推导中会反复出现务必记牢。它描述了旋转部分对平移部分的耦合影响——简单说旋转增量会“带动”平移产生额外位移。3.3 对数映射的注意事项对数映射 ( \log: SO(3) \to \mathfrak{so}(3) ) 在 ( \theta \pi ) 附近有奇异性因为旋转轴方向不唯一。实际SLAM中很少直接用到对数映射但在计算两个旋转之间的相对旋转时会用到。处理方法是当 ( \theta ) 接近 ( \pi ) 时通过旋转矩阵的对称部分来提取旋转轴避免直接除以 ( \sin\theta )。实操心得如果你在用Ceres或者g2o做优化这些库内部已经处理好了指数映射的数值稳定性。但如果你自己手写优化器小角度近似和 ( \pi ) 附近的奇异性处理是两个必须覆盖的边界情况。4. BCH公式与左乘扰动模型的推导4.1 BCH公式告诉我们什么Baker-Campbell-Hausdorff公式描述了两个李代数元素相加后其指数映射的乘积如何计算[ \ln(\exp(\phi_1^\wedge)\exp(\phi_2^\wedge)) \phi_1 \phi_2 \frac{1}{2}[\phi_1, \phi_2] \cdots ]其中 ( [\cdot, \cdot] ) 是李括号。当 ( \phi_1 ) 或 ( \phi_2 ) 是小量时高阶项可以忽略近似为[ \ln(\exp(\phi_1^\wedge)\exp(\delta\phi^\wedge)) \approx \phi_1 J_l^{-1}\delta\phi ]或者[ \ln(\exp(\delta\phi^\wedge)\exp(\phi_1^\wedge)) \approx \phi_1 J_r^{-1}\delta\phi ]这里 ( J_l ) 和 ( J_r ) 分别是左雅可比和右雅可比。这两个近似公式是推导扰动模型的基础。4.2 左乘扰动模型 vs 右乘扰动模型在SLAM优化中我们需要对位姿 ( T ) 求导。做法是给 ( T ) 左乘或右乘一个小扰动 ( \Delta T \exp(\delta\xi^\wedge) )然后看目标函数如何变化。左乘扰动模型[ \frac{\partial (T \cdot p)}{\partial \delta\xi} \lim_{\delta\xi \to 0} \frac{\exp(\delta\xi^\wedge)Tp - Tp}{\delta\xi} ]推导结果是[ \frac{\partial (Tp)}{\partial \delta\xi} \begin{bmatrix} I -(Rpt)^\wedge \ 0 0 \end{bmatrix} ]不对更准确地说对于 ( SE(3) ) 上的左乘扰动关于 ( \delta\xi [\delta\rho, \delta\phi]^T ) 的导数是[ \frac{\partial (Tp)}{\partial \delta\xi} \begin{bmatrix} I -(Rpt)^\wedge \ 0^T 0^T \end{bmatrix} ]等等这里需要仔细。实际上对于 ( Tp ) 这个三维点关于六维扰动的导数是 ( 3 \times 6 ) 的矩阵[ \frac{\partial (Tp)}{\partial \delta\xi} [I_{3\times3}, \quad -(Rpt)^\wedge] ]右乘扰动模型[ \frac{\partial (Tp)}{\partial \delta\xi} [R, \quad -R p^\wedge] ]两者的区别在于扰动作用在哪个坐标系。左乘扰动作用在世界坐标系右乘扰动作用在局部坐标系。4.3 为什么SLAM中更常用左乘扰动这个问题我被问过很多次。核心原因在于左乘扰动模型的形式更简洁且与SLAM中位姿的定义方式更匹配。在视觉SLAM中我们通常定义相机位姿 ( T_{cw} ) 表示从世界坐标系到相机坐标系的变换。观测方程是 ( z \pi(T_{cw} \cdot P_w) )其中 ( P_w ) 是世界坐标系下的三维点。当我们对 ( T_{cw} ) 做左乘扰动时扰动直接作用在相机位姿上物理意义明确相机位姿发生了一个微小变化。而右乘扰动作用在局部坐标系对于 ( T_{cw} ) 来说右乘扰动相当于在相机坐标系下做变换物理意义不如左乘直观。此外左乘扰动下推导出的雅可比矩阵中平移部分的导数就是 ( I )旋转部分的导数是 ( -(Rpt)^\wedge )形式统一便于代码实现。注意左乘和右乘没有本质对错只是约定不同。关键是整个系统要保持一致。如果你在残差定义中用了左乘扰动雅可比就必须按左乘推导混用会导致优化不收敛。我踩过这个坑调试了一整天才发现是扰动方向搞反了。4.4 扰动模型在BA中的完整应用在Bundle Adjustment中重投影误差为[ e z - \pi(T_{cw} \cdot P_w) ]对位姿 ( T_{cw} ) 施加左乘扰动 ( \exp(\delta\xi^\wedge) )[ e(\delta\xi) z - \pi(\exp(\delta\xi^\wedge) \cdot T_{cw} \cdot P_w) ]令 ( P_c T_{cw} \cdot P_w [X, Y, Z]^T )则[ \frac{\partial e}{\partial \delta\xi} -\frac{\partial \pi}{\partial P_c} \cdot \frac{\partial P_c}{\partial \delta\xi} ]其中 ( \frac{\partial \pi}{\partial P_c} ) 是投影函数对相机坐标的导数( 2 \times 3 ) 矩阵( \frac{\partial P_c}{\partial \delta\xi} ) 就是前面推导的 ( [I, -P_c^\wedge] )( 3 \times 6 ) 矩阵。最终雅可比为 ( 2 \times 6 ) 矩阵可以直接用于高斯-牛顿或列文伯格-马夸尔特迭代。5. 代码实现与参数计算实录5.1 用Sophus实现左乘扰动更新以下是一个基于Sophus的位姿更新示例展示了左乘扰动的实际用法#include sophus/se3.hpp #include Eigen/Core // 假设当前位姿为 T_cw优化增量为 delta_xi (6维向量) Sophus::SE3d T_cw Sophus::SE3d::exp(initial_xi); // 左乘扰动更新 Eigen::Matrixdouble, 6, 1 delta_xi ...; // 优化器求解出的增量 T_cw Sophus::SE3d::exp(delta_xi) * T_cw; // 左乘 // 注意Sophus 的 SE3::exp 接受的六维向量顺序是 [平移, 旋转] // 即 delta_xi [delta_rho, delta_phi]这里有一个容易搞混的地方Sophus的SE3::exp接受的六维向量前三维是平移部分 ( \rho )后三维是旋转部分 ( \phi )。但有些库比如g2o的顺序可能不同。如果你在多个库之间切换一定要确认顺序。5.2 雅可比矩阵的数值验证推导完雅可比之后强烈建议做数值验证。方法很简单对扰动分量施加一个小量 ( \epsilon )计算残差变化与解析雅可比对比// 数值验证雅可比 double eps 1e-6; Eigen::Matrixdouble, 2, 6 J_numeric; for (int i 0; i 6; i) { Eigen::Matrixdouble, 6, 1 delta Eigen::Matrixdouble, 6, 1::Zero(); delta(i) eps; Sophus::SE3d T_plus Sophus::SE3d::exp(delta) * T_cw; Eigen::Vector2d e_plus project(T_plus * P_w); Eigen::Vector2d e_orig project(T_cw * P_w); J_numeric.col(i) (e_plus - e_orig) / eps; } // 与解析雅可比对比误差应在 1e-5 量级这个验证步骤花不了十分钟但能帮你省下几小时的调试时间。我现在的习惯是每推导一个新雅可比先跑数值验证确认无误再接入优化器。5.3 参数选择与迭代策略在BA优化中扰动的初始值通常设为零向量优化器会迭代求解增量。几个关键参数参数典型值说明最大迭代次数10-50取决于问题规模增量阈值1e-6增量小于此值时认为收敛初始阻尼因子1e-4LM算法的初始lambda阻尼因子更新系数10每次迭代调整倍数对于左乘扰动模型初始增量设为零向量即可。每次迭代后用T_cw SE3::exp(delta_xi) * T_cw更新位姿然后重新计算残差和雅可比。实操心得在优化初期旋转增量可能比较大此时BCH公式的高阶项不能完全忽略会导致雅可比不准确。解决办法是限制每次迭代的最大增量或者在优化初期使用更保守的阻尼因子。我在处理大基线场景时会把初始阻尼因子调大一个量级收敛更稳。6. 常见问题与排查技巧实录6.1 优化不收敛的典型原因现象可能原因排查方法残差震荡不下降雅可比符号错误数值验证雅可比迭代几次后发散扰动方向搞反检查左乘/右乘是否一致收敛但结果偏差大指数映射小角度近似缺失检查theta接近0时的处理平移部分不更新雅可比平移块为零检查( \partial P_c / \partial \delta\rho )是否为I旋转更新异常大李代数向量顺序搞混确认[rho, phi]还是[phi, rho]6.2 左乘与右乘混用的排查这是最容易犯的错误。症状是优化过程中残差先下降后上升或者直接发散。排查方法是打印每次迭代的增量向量观察旋转部分的模长是否合理。如果旋转增量经常超过1弧度很可能是扰动方向搞反了。另一个检查点如果你在残差定义中用了左乘扰动但在雅可比推导中不小心用了右乘的公式两者不匹配优化器会朝着错误方向更新。我的建议是在代码注释中明确标注“左乘扰动”或“右乘扰动”并且整个项目统一。6.3 数值精度问题的处理李代数在零附近有奇异性指数映射和对数映射都需要特殊处理。Sophus库内部已经做了处理但如果你自己实现需要注意当 ( \theta 10^{-10} ) 时直接返回单位矩阵或零向量当 ( \theta ) 接近 ( \pi ) 时对数映射需要特殊处理旋转轴使用double而非float李代数计算对精度敏感6.4 与优化库的配合要点Ceres和g2o都支持自定义李代数流形。在Ceres中你需要定义一个Manifold旧版本叫LocalParameterization实现Plus和ComputeJacobian方法。在g2o中需要继承BaseVertex并重写oplusImpl方法。关键点优化库通常默认使用右乘扰动在局部坐标系中更新如果你推导的是左乘扰动需要在Plus方法中明确实现左乘逻辑。Ceres的Manifold::Plus方法中x是当前值delta是增量返回更新后的值。对于左乘扰动bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { // x 是 SE3 的存储四元数平移delta 是六维增量 Sophus::SE3d T fromCeres(x); Eigen::Matrixdouble, 6, 1 dxi Eigen::Mapconst Eigen::Matrixdouble, 6, 1(delta); Sophus::SE3d T_new Sophus::SE3d::exp(dxi) * T; // 左乘 toCeres(T_new, x_plus_delta); return true; }这个实现细节在Ceres的文档中不会特别强调但如果你搞错了优化结果会完全错误。7. 从理论到工程的几个关键认知7.1 李代数不是“另一种旋转表示”很多人把李代数当成和欧拉角、四元数并列的旋转表示方法这个理解有偏差。李代数不是用来“表示”旋转的它是旋转流形的切空间是用来做微分和优化的工具。你最终存储旋转还是用旋转矩阵或四元数但在优化过程中增量是在李代数上计算的。这个认知转变很重要。一旦理解了这一点你就明白为什么SLAM后端优化中到处都是exp和log——它们是在流形和切空间之间来回切换的桥梁。7.2 左乘扰动的物理意义左乘扰动 ( \exp(\delta\xi^\wedge) \cdot T ) 的物理意义是在世界坐标系下对相机位姿施加一个微小变换。这个变换的平移部分 ( \delta\rho ) 是世界坐标系下的位移旋转部分 ( \delta\phi ) 是世界坐标系下的旋转轴角。右乘扰动 ( T \cdot \exp(\delta\xi^\wedge) ) 的物理意义是在相机坐标系下对相机位姿施加微小变换。平移部分 ( \delta\rho ) 是相机坐标系下的位移旋转部分是相机坐标系下的旋转。在视觉SLAM中观测方程通常写在世界坐标系下所以左乘扰动更自然。但在某些局部窗口优化中右乘扰动也有优势。选择哪种取决于你的残差定义和坐标系约定。7.3 雅可比矩阵的稀疏性在BA中每个观测只涉及一个相机位姿和一个路标点所以雅可比矩阵天然稀疏。左乘扰动模型下位姿雅可比是 ( 2 \times 6 ) 的稠密块路标雅可比是 ( 2 \times 3 ) 的稠密块。利用稀疏性可以大幅加速求解这也是为什么SLAM后端能用舒尔补高效求解的原因。理解这一点后你在实现时就应该注意不要把雅可比矩阵当成稠密矩阵存储而是按块存储利用稀疏结构。Ceres和g2o都内置了稀疏求解器你只需要提供正确的雅可比块即可。7.4 与IMU融合时的注意事项当视觉SLAM与IMU融合时状态量中除了位姿还有速度、零偏等。左乘扰动模型同样适用但需要注意IMU预积分中的旋转扰动定义。通常IMU预积分使用右乘扰动在局部坐标系中而视觉部分使用左乘扰动。融合时需要在两者之间做转换否则雅可比不一致会导致优化失败。这个坑我在做视觉惯性里程计时踩过表现是优化后轨迹在旋转方向上漂移。后来统一了扰动方向问题就消失了。8. 一个完整的左乘扰动BA示例8.1 问题设定假设有 ( N ) 个相机位姿和 ( M ) 个路标点观测方程为 ( z_{ij} \pi(T_i \cdot P_j) n_{ij} )。目标是最小化重投影误差[ \min_{{T_i}, {P_j}} \sum_{i,j} | z_{ij} - \pi(T_i \cdot P_j) |^2 ]使用左乘扰动模型对 ( T_i ) 施加扰动 ( \exp(\delta\xi_i^\wedge) )对 ( P_j ) 施加扰动 ( \delta p_j )三维向量直接加法。8.2 雅可比推导对于观测 ( (i, j) )残差对位姿扰动的雅可比[ J_{T} -\frac{\partial \pi}{\partial P_c} \cdot [I, -P_c^\wedge] ]残差对路标点的雅可比[ J_{P} -\frac{\partial \pi}{\partial P_c} \cdot R_i ]其中 ( P_c T_i \cdot P_j )( R_i ) 是 ( T_i ) 的旋转部分。8.3 代码框架// 构建雅可比矩阵 for (const auto obs : observations) { int i obs.camera_id; int j obs.point_id; Sophus::SE3d T_i poses[i]; Eigen::Vector3d P_j points[j]; Eigen::Vector3d P_c T_i * P_j; // 投影函数对相机坐标的导数 (2x3) Eigen::Matrixdouble, 2, 3 J_proj computeProjectionJacobian(P_c); // 对位姿的雅可比 (2x6) Eigen::Matrixdouble, 2, 6 J_T; J_T.block2,3(0,0) J_proj; J_T.block2,3(0,3) -J_proj * Sophus::SO3d::hat(P_c); // 对路标点的雅可比 (2x3) Eigen::Matrixdouble, 2, 3 J_P -J_proj * T_i.rotationMatrix(); // 填入全局雅可比矩阵 // ... }8.4 迭代更新// 求解增量后更新 for (int i 0; i num_poses; i) { Eigen::Matrixdouble, 6, 1 delta solution.segment6(i*6); poses[i] Sophus::SE3d::exp(delta) * poses[i]; // 左乘更新 } for (int j 0; j num_points; j) { points[j] solution.segment3(num_poses*6 j*3); // 直接加法 }这个框架可以直接套用到大多数BA问题中。关键点在于雅可比矩阵的推导要正确以及更新时左乘和右乘要一致。9. 我踩过的坑与经验总结第一个坑是李代数向量顺序。Sophus的SE3::exp接受的是[平移, 旋转]但有些教程和代码用的是[旋转, 平移]。我在第一次对接两个不同库的时候因为顺序搞反优化结果完全错误排查了大半天。现在的习惯是在每个项目开始时先写一个简单的测试用例验证exp和log的往返一致性。第二个坑是小角度近似。自己实现指数映射时如果没处理 ( \theta \to 0 ) 的情况数值会爆炸。这个坑在优化初期特别容易触发因为初始增量往往很小。解决办法很简单加一个阈值判断小于阈值时用泰勒展开。第三个坑是左乘右乘混用。这个坑最隐蔽因为代码看起来都对但优化就是不收敛。我的经验是在代码中显式标注每个雅可比的扰动方向并且在优化器的Plus方法中明确实现对应的更新逻辑。如果团队协作最好在代码规范中统一约定。第四个坑是数值验证缺失。推导完雅可比后一定要做数值验证。我现在的流程是推导→数值验证→接入优化器→小规模测试→大规模测试。每一步都确认无误再往下走比出了bug再回头查要高效得多。李群和李代数这部分内容初学的时候确实抽象但一旦理解了“流形上不能直接加法要在切空间做增量”这个核心思想剩下的就是推导和代码实现的功夫。希望这篇内容能帮你少走一些弯路。