1. 项目概述从旋转矩阵到李代数的思维跃迁刚接触视觉SLAM的朋友学到第三讲“李群和李代数”时十有八九会卡壳。前一讲还在用旋转矩阵、变换矩阵优雅地描述相机位姿怎么突然就冒出来一堆像SO(3)、SE(3)、so(3)、se(3)这样抽象的数学符号更让人头疼的是书上说旋转矩阵对加法不封闭对导数不好定义所以需要引入李群和李代数来“解决求导问题”。这话每个字都认识连起来却让人云里雾里为什么要求导李代数怎么就比矩阵好求导了我自己最初学到这里时也是反复琢磨了好几遍才打通任督二脉。其实这个章节是整个视觉SLAM数学基础的关键转折点。它不是为了炫技而是为了解决一个非常实际且核心的工程问题在非线性优化中如何高效、正确地求解相机位姿想象一下你的SLAM系统通过传感器数据估算相机的位置和朝向一个旋转加一个平移但这个估算有误差。为了得到最优的位姿你需要不断微调它让误差最小。这个“微调”本质上就是在求导寻找让误差函数下降最快的方向。如果位姿直接用旋转矩阵表示这个“微调”操作会变得异常棘手李代数正是解开这把锁的钥匙。简单来说这一讲的目的是让我们学会把位姿估计这个几何问题“翻译”成可以在计算机里进行高效数值优化比如高斯牛顿法、列文伯格-马夸尔特法的代数问题。理解李群和李代数就是掌握这门“翻译”手艺。接下来我会结合自己的理解拆解其中的核心概念、内在联系并聚焦于最关键的实践应用李代数求导。我们会看到理论上的繁琐最终会落地为一个简洁的雅可比矩阵直接用于我们的优化程序中。2. 核心概念拆解群、李群与李代数要理解李代数必须先弄清楚什么是“群”以及“李群”特殊在哪里。很多教程一上来就扔公式我们换个方式从几何直观和运算性质入手。2.1 群与李群描述“对称性”的集合群是一个数学概念它指的是一个集合加上一种运算这个组合需要满足四个条件“封结幺逆”封闭性集合内任意两个元素做运算结果还在这个集合里。结合律运算的顺序不影响结果先算哪两个都一样。幺元存在一个单位元素和任何元素运算都等于该元素本身。逆元每个元素都有一个对应的逆元素两者运算得到单位元。为什么SLAM关心这个因为三维空间中的旋转天然就构成一个群称为特殊正交群 SO(3)。这里的集合是所有合法的三维旋转矩阵3x3正交且行列式为1运算是矩阵乘法。你可以验证两个旋转矩阵相乘结果还是旋转矩阵封闭性旋转矩阵乘法满足结合律单位矩阵就是幺元旋转矩阵的逆就是它的转置逆元。平移变换本身也构成一个群加法群而旋转加平移合起来就构成了描述刚体运动的特殊欧氏群 SE(3)其元素是4x4的变换矩阵。那么李群又是什么简单说就是“连续”的群。SO(3)和SE(3)中的元素矩阵不是孤立的你可以从一个旋转平滑地、连续地变化到另一个旋转。比如相机从朝北慢慢转到朝东这个过程中有无数个中间的旋转状态这些状态都在SO(3)这个集合里。这种“连续性”和“光滑性”是我们可以讨论“求导”的前提。因为求导本质上是研究微小变化。注意这里容易产生一个误解认为引入李代数是因为旋转矩阵“不能求导”。更准确的说法是旋转矩阵作为优化变量时其约束正交且行列式为1会让优化问题变得复杂带约束优化。李代数提供了一种在无约束空间中进行优化的途径。2.2 李代数李群的“局部坐标系”李群是连续的我们想研究它在某个点比如单位元附近的变化。李代数就是描述这种“局部变化”的数学工具。你可以把李群想象成一个弯曲的空间比如一个球面而李代数就是这个弯曲空间在单位元处切平之后得到的平坦空间切空间。对于SO(3)群其对应的李代数记作so(3)。so(3)的元素是一个三维向量φ或者其对应的反对称矩阵φ^。这个φ的几何意义非常明确它的方向代表旋转轴它的模长代表旋转角度。这就是著名的旋转向量或轴角表示。李代数 so(3) 就是由所有可能的旋转向量构成的空间它是一个普通的三维向量空间没有旋转矩阵那些复杂的约束。它们之间的桥梁是指数映射和对数映射指数映射 exp(φ^) 把一个李代数 so(3) 中的元素旋转向量 φ映射回李群 SO(3) 中的一个元素旋转矩阵 R。物理意义是绕着轴 φ/|φ| 旋转 |φ| 角度。对数映射 log(R) 反过来从旋转矩阵 R 求出对应的旋转向量 φ。这解决了如何从矩阵得到轴角的问题。关键点来了李代数 so(3) 作为一个向量空间对加法是封闭的而且它是“平坦”的。这意味着我们在 so(3) 空间里做加法、求导就跟在普通的三维欧式空间里一样自然、没有约束。这正好解决了我们优化相机旋转时的核心痛点。2.3 SE(3)与se(3)把平移也纳入体系对于完整的刚体运动旋转平移即 SE(3)其李代数记作se(3)。se(3) 的元素不再是一个简单的三维向量而是一个六维向量ξ [ρ, φ]^T或者其对应的4x4矩阵形式ξ^。这里φ 同样代表旋转对应 so(3)。ρ 与平移有关但并非直接的平移向量。在指数映射下它经过一个与旋转相关的线性变换后才得到实际的平移量 t。se(3) 的指数映射将六维向量 ξ 映射为4x4变换矩阵 T。同样se(3) 空间也是一个无约束的六维空间为我们同时优化旋转和平移提供了数学基础。实操心得在实际编程中例如使用Sophus库我们很少直接操作指数对数的具体公式。但必须理解当我们谈论“在李代数上求导”时我们实际上是把位姿变换矩阵 T 对应的李代数 ξ 作为一个六维优化变量在R^6这个平坦空间里进行无约束优化。这是理解后续所有推导和代码的基石。3. 核心动机为什么必须使用李代数求导这是本章最需要厘清的问题。我们用一个具体的SLAM例子来说明。假设有一个空间点P世界坐标它被相机观测到投影到像素坐标为z。根据相机模型这个投影过程是z h(T, P)。其中T是相机位姿属于 SE(3)h 是包含了内参和投影的非线性函数。在SLAM中我们有很多这样的观测。由于噪声存在实际观测值z_meas和理论投影值h(T, P)之间有误差e z_meas - h(T, P)。优化目标就是找到一个最优的T让所有观测误差的平方和最小。我们使用高斯牛顿法等迭代优化算法。每一步我们需要计算当前位姿T处误差函数e关于T的导数雅可比矩阵J然后根据J来更新T使误差下降。问题就出在这个“更新”上。3.1 直接对变换矩阵T求导的困境假设我们有一个更新量ΔT我们想计算T ΔT处的误差。但T是变换矩阵ΔT应该是什么如果ΔT也是一个变换矩阵T ΔT很可能不再是一个合法的变换矩阵不满足行列式为1等约束。这破坏了优化变量必须在流形上的要求。即使我们强行定义一种更新由于 SE(3) 对加法不封闭T ΔT这个操作本身在几何上没有良好的定义。所以我们不能直接在矩阵空间做“加法更新”。3.2 李代数提供的解决方案扰动模型李代数提供了完美的解决方案。思路如下优化变量我们不直接优化变换矩阵T而是优化其对应的李代数ξ。ξ是一个六维向量生活在平坦的向量空间R^6中可以自由地做加法。更新方式在迭代中我们计算出一个李代数空间的更新量δξ也是一个六维向量。然后通过指数映射将这个更新“作用”到当前位姿上T_new exp(δξ^) * T_old这里exp(δξ^)是一个由微小李代数 δξ 通过指数映射生成的一个微小变换矩阵。关键是两个变换矩阵相乘结果永远是一个变换矩阵。这完美地满足了“更新后变量仍在群内”的要求。求导对象因此我们的求导目标从“误差e对矩阵T”变成了“误差e对李代数扰动δξ”。我们想知道当在李代数上施加一个微小扰动 δξ 时误差 e 会如何变化。这被称为扰动模型。生活类比想象你在驾驶一艘船位姿T。你不能直接“跳”到一个新位置TΔT因为海洋不是平坦的坐标系。但你可以用舵和油门给出一个微小的控制指令李代数扰动δξ——左转一点加速一点。这个指令作用于当前状态通过动力学指数映射自然地将船带到下一个合法状态T_new。李代数就是这个“控制指令”的空间而求导就是计算哪个方向的指令能最快减小定位误差。4. 核心公式推导SO(3)与SE(3)的雅可比矩阵理论铺垫完成现在进入最硬核也最实用的部分推导雅可比矩阵J。这是连接优化算法如g2o, Ceres的桥梁。我们关注两种导数空间点坐标关于李代数的导数。重投影误差关于李代数的导数。4.1 SO(3)上的李代数求导以左扰动模型为例假设有一个空间点p三维向量经过旋转R后得到Rp。现在考虑在R上施加一个左扰动这个扰动对应的李代数为φ注意这里φ是扰动是一个微小旋转向量。扰动后的旋转为exp(φ^)扰动后的点为p exp(φ^) Rp我们想求p关于扰动φ的导数。由于 φ 是微小量根据指数映射的泰勒展开和一阶近似有exp(φ^) ≈ I φ^代入上式p ≈ (I φ^) R p R p φ^ (R p)记Rp为p_rotated。那么变化量Δp p - R p ≈ φ^ p_rotated。根据反对称矩阵的性质a^ b -b^ a有φ^ p_rotated - (p_rotated)^ φ。 因此Δp ≈ - (p_rotated)^ φ。所以导数雅可比矩阵为∂(Rp) / ∂φ - (R p)^这个雅可比矩阵是一个3x3的矩阵。它的意义是当旋转有一个微小的左扰动 φ 时旋转后的点Rp的变化量近似等于- (Rp)^乘以这个扰动向量 φ。注意事项这里使用的是“左扰动”模型即扰动乘在左边。还有“右扰动”模型导数形式略有不同。在视觉SLAM中左扰动模型更为常用。在推导和使用时必须保持一致否则会导致错误的优化结果。4.2 SE(3)上的李代数求导重投影误差雅可比在视觉SLAM的BABundle Adjustment中我们更常直接求重投影误差关于李代数的导数。考虑一个世界点P_w齐次坐标 [X, Y, Z, 1]^T相机位姿为T变换矩阵相机内参矩阵为K。投影过程为将世界点变换到相机坐标系P_c T P_w [X, Y, Z, 1]^T。取前三维得到相机坐标系下的三维点p_c [X, Y, Z]^T。进行投影u K p_c得到归一化像素坐标去除了Z’。更具体地常写作u (fx * X/Z cx, fy * Y/Z cy)^T其中 fx, fy, cx, cy 为内参。重投影误差e u_meas - u是一个二维向量。现在我们对李代数ξ施加左扰动δξ。扰动后的位姿为exp(δξ^) T。扰动后的投影点为u(δξ)。我们要求雅可比矩阵J ∂e / ∂δξ。根据链式法则J - (∂u / ∂δξ)因为 e u_meas - uu_meas是常数而∂u / ∂δξ (∂u / ∂p_c) * (∂p_c / ∂δξ)。第一项 ∂u / ∂p_c 这是投影方程关于相机坐标系下点的导数是一个2x3的矩阵。 设p_c [x, y, z]^T则u [fx * x/z cx, fy * y/z cy]^T。 求导可得∂u/∂p_c [ [fx/z, 0, -fx*x/(z^2) ], [ 0, fy/z, -fy*y/(z^2) ] ]第二项 ∂p_c / ∂δξ 这是相机坐标系下点关于李代数扰动的导数。p_c是变换后的点p_c(δξ) exp(δξ^) T P_w。我们求它在δξ0处的导数。这类似于之前SO(3)的推导但现在是SE(3)。经过推导过程略涉及SE(3)指数映射的一阶近似可以得到一个3x6的矩阵∂p_c / ∂δξ [ I, -p_c^ ]这里I是3x3单位矩阵p_c^是p_c的反对称矩阵。注意这个矩阵是关于扰动 δξ 的导数而 δξ 是一个六维向量 [ρ, φ]^T。这个结果可以直观理解扰动的平移部分前三维ρ直接作用于点所以是单位阵 I扰动的旋转部分后三维φ产生的影响是-p_c^。将两项相乘得到最终的2x6的雅可比矩阵JJ - (∂u/∂p_c) * [ I, -p_c^ ] [ [ -fx/z, 0, fx*x/(z^2), fx*x*y/(z^2), -fx - fx*x^2/(z^2), fx*y/z ], [ 0, -fy/z, fy*y/(z^2), fy fy*y^2/(z^2), -fy*x*y/(z^2), -fy*x/z ] ]这个矩阵的前三列对应扰动平移量 ρ 的导数后三列对应扰动旋转量 φ 的导数。实操心得这个雅可比矩阵J是视觉SLAM后端优化的核心。在实现Bundle Adjustment时你需要为每一个三维点对每一个相机的观测计算这个2x6的矩阵。它告诉优化器当前位姿应该沿着这六个自由度的哪个方向如何结合平移和旋转进行微小调整才能最有效地降低当前的重投影误差。很多开源的优化库如g2o已经内置了这些求导规则但理解其由来对于调试和自定义新的误差模型至关重要。5. 实践应用与代码实现要点理论最终要服务于代码。在《十四讲》的代码中这一部分主要体现在 Sophus 库的使用和自定义的雅可比计算上。5.1 使用Sophus库处理李代数Sophus是一个常用的李代数库它提供了SO(3)、SE(3)、so(3)、se(3)的良好封装。#include sophus/so3.hpp #include sophus/se3.hpp // 1. 从旋转矩阵或变换矩阵构造李群 Eigen::Matrix3d R ...; // 旋转矩阵 Sophus::SO3d SO3_R(R); // 构造SO(3)对象 Eigen::Matrix4d T ...; // 变换矩阵 Sophus::SE3d SE3_Rt(T); // 构造SE(3)对象内部包含R和t // 2. 李群与李代数的相互转换对数/指数映射 Sophus::Vector3d so3 SO3_R.log(); // 从SO(3)得到对应的李代数 so3 (三维向量) Sophus::SO3d SO3_R2 Sophus::SO3d::exp(so3); // 从李代数 so3 恢复 SO(3) Sophus::Vector6d se3 SE3_Rt.log(); // 从SE(3)得到对应的李代数 se3 (六维向量) Sophus::SE3d SE3_Rt2 Sophus::SE3d::exp(se3); // 从李代数 se3 恢复 SE(3) // 3. 扰动模型更新 Sophus::Vector6d update_se3; // 优化器计算出的李代数更新量通常很小 update_se3.setZero(); update_se3 ... // 来自优化器的增量 // 使用指数映射将更新量转换为变换矩阵再左乘或右乘当前位姿 Sophus::SE3d T_updated Sophus::SE3d::exp(update_se3) * SE3_Rt;5.2 在非线性优化中集成李代数以高斯牛顿法优化相机位姿为例伪代码流程如下// 初始化位姿 T (Sophus::SE3d) Sophus::SE3d T_esti; // 定义重投影误差函数输入为位姿李代数 se3 和三维点 P vectorVector3d points; // 已知的三维点 vectorVector2d measurements; // 对应的像素观测 for (int iter 0; iter max_iteration; iter) { Matrix6d H Matrix6d::Zero(); // 海塞矩阵近似6x6 (因为李代数 se3 是6维) Vector6d b Vector6d::Zero(); // 梯度项6x1 double total_cost 0; for (size_t i 0; i points.size(); i) { // 1. 计算当前位姿下的投影和误差 Vector3d P_c T_esti * points[i]; // 变换到相机系 Vector2d proj project(P_c); // 投影函数包含内参 Vector2d e measurements[i] - proj; // 2. 计算雅可比矩阵 J (2x6) Matrixdouble, 2, 6 J computeJacobian(T_esti, points[i]); // 调用前面推导的公式 // 3. 构建增量方程 H * dx b H J.transpose() * J; // 高斯牛顿法的近似海塞阵 b -J.transpose() * e; total_cost e.squaredNorm(); } // 4. 求解线性方程得到李代数空间的更新量 dx Vector6d dx H.ldlt().solve(b); // 这里 dx 就是 δξ // 5. 判断是否收敛 if (isnan(dx[0])) { cout result is nan! endl; break; } if (dx.norm() epsilon) { cout converged! endl; break; } // 6. 使用李代数更新位姿 T_esti Sophus::SE3d::exp(dx) * T_esti; // 左扰动更新 }关键点解析computeJacobian函数需要实现前面推导的2x6雅可比矩阵公式。增量dx直接是李代数空间se(3)的向量。更新步骤T_esti Sophus::SE3d::exp(dx) * T_esti是扰动模型的核心体现。exp(dx)将微小的李代数增量转换为一个微小的变换矩阵然后左乘到当前估计值上得到更新后的位姿这个新位姿仍然是一个合法的 SE(3) 元素。5.3 常见陷阱与调试技巧左扰动与右扰动混淆这是最常见的错误。公式推导、雅可比计算、更新步骤必须使用同一种扰动模型。书中和大部分代码默认使用左扰动。如果混用优化将无法收敛甚至发散。在阅读他人代码或论文时首先要确认其使用的扰动模型。李代数的维度so(3)是3维se(3)是6维。在构建海塞矩阵H时优化位姿对应的H是6x6优化三维点坐标对应的部分是3x3。如果维度弄错矩阵运算会报错。雅可比矩阵的符号误差定义为e 观测 - 预测还是e 预测 - 观测会影响雅可比矩阵的符号。必须与优化库如g2o、Ceres中定义的误差接口保持一致。通常e 观测 - 预测更为常见此时雅可比为J - (∂u/∂δξ)。初始值的重要性非线性优化严重依赖初始值。如果初始位姿离真实值太远比如旋转误差超过30度一阶近似可能失效导致优化陷入局部极小或发散。在SLAM中通常需要运动估计如视觉里程计提供一个较好的初始位姿。数值稳定性当点深度Z很小时靠近相机雅可比矩阵中1/z和1/z^2项会变得非常大导致H矩阵条件数变差。在求解线性方程H dx b时需要使用稳定的求解器如LDLT、QR分解避免直接求逆。在计算exp(δξ)时如果δξ的模长很小可以使用一阶近似I δξ^来提高速度。Sophus库内部已经做了稳健的实现。排查技巧当优化不收敛时可以按以下步骤检查打印增量观察每次迭代的更新量dx.norm()是否在稳定下降。检查雅可比随机选取一个点用数值差分法验证解析雅可比是否正确。即给李代数一个微小扰动如1e-6计算误差的变化与解析雅可比计算的结果对比。可视化中间结果将每次迭代优化后的位姿和三维点画出来观察其变化趋势判断是震荡、发散还是缓慢收敛。简化问题用仿真数据无噪声已知真实值测试确保算法流程和雅可比计算本身无误再应用到真实数据上。理解李群和李代数并掌握其求导方法是打开视觉SLAM后端优化大门的钥匙。它把看似复杂的几何约束转化成了我们熟悉的数值优化问题。虽然公式推导需要花些功夫但一旦掌握你对SLAM系统的理解会深入到另一个层次。在实际项目中你可能不需要每次都手推雅可比但当你需要自定义新的传感器模型或误差项时这套方法论将是不可或缺的工具。