1. 项目概述为什么SLAM绕不开李群与李代数如果你正在学习视觉SLAM尤其是《视觉SLAM十四讲》这本书那么“第三讲 李群和李代数”绝对是一个让你又爱又恨的坎。爱的是一旦跨过去你对机器人姿态、旋转、优化的理解会瞬间提升一个维度恨的是它可能是你遇到的第一个真正意义上的数学难关满篇的符号和抽象概念很容易让人望而却步。我自己当年啃这部分的时候也是反复琢磨了好几遍才打通任督二脉。今天我就以一个过来人的身份结合大量实际编程和调试的经验帮你把这块硬骨头嚼碎了、消化掉。简单来说这一讲的核心目标是解决一个在SLAM中至关重要的问题如何对机器人的旋转和平移合称“位姿”进行“微小”的调整和优化我们直观上知道机器人的位姿可以用一个旋转矩阵R和一个平移向量t来表示。但是当我们在后端优化中需要根据观测数据去微调这个位姿时问题就来了旋转矩阵R本身有9个数但它只代表3个自由度的旋转这9个数之间还存在复杂的约束正交且行列式为1。直接对这9个数求导、做加法进行优化会破坏这些约束导致结果不再是一个合法的旋转。这就好比你想微调一个已经校准好的精密仪器的一个螺丝结果直接操作却把整个结构弄散架了。李群和李代数就是为解决这个“带约束的优化”问题而引入的一套优雅的数学工具。它提供了一种方法让我们能把复杂的、带约束的旋转矩阵李群SO(3)映射到一个简单的、无约束的向量空间李代数so(3)里。在这个向量空间里我们可以放心大胆地做加法、求导数进行优化计算。算完之后再通过一个指数映射把结果变回合法的旋转矩阵。这个过程是理解后续非线性优化如高斯牛顿法、列文伯格-马夸尔特法如何应用于位姿估计的基石。没有它SLAM的后端优化就无法进行。所以无论你感觉多抽象请务必坚持学下去。接下来我会抛开复杂的数学推导外壳用最直白的语言和大量编程视角的解读带你掌握李群李代数的核心思想、关键公式以及它们在SLAM中的实际用法。2. 核心概念拆解群、李群、李代数到底在说什么很多教程一上来就扔出一堆定义让人云里雾里。我们换个顺序先从实际问题出发看看这些概念是如何被“逼”出来的。2.1 从旋转矩阵的麻烦说起为什么需要李代数假设我们有一个旋转矩阵R它满足R^T R I且det(R) 1。现在相机在运动R随时间t连续变化即R(t)是一个关于时间的函数并且始终是一个旋转矩阵。我们对R(t)求导看看会发生什么。对约束R(t)^T R(t) I两边关于时间求导\dot{R}(t)^T R(t) R(t)^T \dot{R}(t) 0令\phi(t) R(t)^T \dot{R}(t)代入上式得\phi(t)^T \phi(t) 0这说明\phi(t)是一个反对称矩阵对于三维空间任何一个反对称矩阵都对应一个三维向量我们记这个向量为\phi实际上书里常用\omega或\xi的上三角部分。于是有R(t)^T \dot{R}(t) \phi(t)^{\wedge}等式两边右乘R(t)得到\dot{R}(t) R(t) \phi(t)^{\wedge}看这个微分方程它告诉我们旋转矩阵R的导数等于R本身右乘一个由向量\phi生成的反对称矩阵。这个向量\phi就蕴含了旋转的瞬时方向和速度你可以把它想象成瞬时角速度。而所有可能的\phi构成的这个向量空间加上一个特定的运算李括号就是李代数 so(3)。所以李代数 so(3) 的物理意义非常直观它就是旋转的“瞬时变化率”所在的空间是一个三维向量空间没有讨厌的约束。我们可以在这个空间里自由地做加法、数乘来表征旋转的微小变化。2.2 李群 SO(3) 与李代数 so(3) 的“桥梁”指数与对数映射既然李代数 so(3) 是“变化率”那么如何从“变化率”积分得到最终的“旋转”呢这就要用到指数映射。对于上面的微分方程\dot{R}(t) R(t) \phi^{\wedge}给定初始条件R(0)I它的解是R(t) \exp(\phi^{\wedge} t)当t1时R \exp(\phi^{\wedge})。这意味着一个李代数向量\phi通过指数运算exp可以对应到李群旋转矩阵SO(3)中的一个元素R。反过来给定一个旋转矩阵R我们也可以通过对数映射log找到对应的李代数向量\phi。这两个映射构成了李群和李代数之间的核心对应关系指数映射 Exp:\phi \in \mathbb{R}^3 \rightarrow R \in SO(3)对数映射 Log:R \in SO(3) \rightarrow \phi \in \mathbb{R}^3注意这里的指数exp不是简单的标量指数而是矩阵指数。对于 so(3)它有闭式解这就是著名的罗德里格斯公式。这是需要你亲手推导和编程实现的关键公式。一个极其重要的类比你可以把 SO(3) 想象成一个三维的球面虽然实际是三维流形球面上的每一个点代表一个旋转姿态。而 so(3) 就是这个球面在“原点”单位矩阵I处的切空间。这个切空间是一个平坦的二维平面对于球面或三维空间对于SO(3)。指数映射就是把切空间so(3)里的一小段向量\phi沿着测地线“缠绕”到球面SO(3)上去。对数映射则是反过来。优化就是在切空间平坦、无约束里调整向量\phi然后映射回球面得到新的合法旋转。2.3 李代数求导解决位姿优化的核心武器终于到了最实战的部分。假设我们有一个空间点P经过旋转R和平移t后得到观测点P RP t。现在我们有一个观测误差e u - \pi(P)其中u是像素坐标\pi是投影函数。我们的目标是调整R和t使得误差e最小。关键问题误差e关于旋转R的导数怎么求我们不能直接对R的9个元素求导因为会破坏约束。李代数提供了完美的解决方案我们不直接优化R而是优化其对应的李代数\phi。当R有一个微小的扰动\Delta R时这个扰动可以对应到李代数空间的一个微小增量\delta \phi。即\Delta R \exp(\delta \phi^{\wedge})。那么扰动后的函数值为f(R \oplus \delta \phi) f(\exp(\delta \phi^{\wedge}) R)。这里的\oplus表示在李群上的左乘扰动。我们对\delta \phi求导就是李代数上的导数。书中引入了两个重要的扰动模型左乘扰动模型对R左乘一个微小扰动\Delta R \exp(\delta \phi^{\wedge})。右乘扰动模型对R右乘一个微小扰动。以左乘扰动为例经过推导这是必须掌握的推导过程误差关于李代数扰动的雅可比矩阵为\frac{\partial e}{\partial \delta \phi} -\frac{\partial \pi}{\partial P} \frac{\partial P}{\partial \delta \phi} -\frac{\partial \pi}{\partial P} [I, -(RPt)^{\wedge}]这个雅可比矩阵的维度是2x6对于重投影误差像素误差是2维扰动量\delta \phi和\delta p合起来是6维。有了这个雅可比我们就可以把它代入高斯牛顿或LM等优化算法中迭代求解最优的\delta \phi和\delta p平移扰动。每次迭代得到增量后用这个增量去更新李代数\phi \leftarrow \phi \delta \phi然后再通过指数映射更新旋转矩阵R \exp(\phi^{\wedge})。实操心得在代码中我们几乎从不直接存储和优化旋转矩阵R。对于位姿T属于SE(3)我们通常用一个Eigen::Matrix4d表示但在优化变量中我们优化的是一个6维向量\xi前3维是旋转对应的李代数\phi后3维是平移\rho。Sophus库中的SE3类就完美封装了T和\xi之间的转换。3. 从理论到代码手撕SO(3)和SE(3)的运算理论懂了不写代码等于没学。我们结合《十四讲》中的代码深入几个关键实现。3.1 实现SO(3)的指数映射罗德里格斯公式指数映射的闭式解就是罗德里格斯公式。给定李代数\phi \theta \mathbf{a}其中\theta是旋转角\mathbf{a}是单位旋转轴向量。Eigen::Matrix3d Exp(const Eigen::Vector3d phi) { double theta phi.norm(); // 旋转角 Eigen::Vector3d a phi.normalized(); // 旋转轴注意phi为零向量时的处理 Eigen::Matrix3d a_hat Sophus::SO3d::hat(a); // 反对称矩阵 Eigen::Matrix3d R Eigen::Matrix3d::Identity() sin(theta) * a_hat (1 - cos(theta)) * a_hat * a_hat; return R; }关键点需要处理phi为零向量的特殊情况此时theta0应直接返回单位阵。a_hat * a_hat这个运算等于a*a^T - I这个恒等式可以用于化简和验证能加深你对公式的理解。这个公式非常直观I是基础sin(theta)*a_hat是旋转的主体线性部分(1-cos(theta))*a_hat*a_hat是补偿项。3.2 实现SO(3)的对数映射对数映射是指数映射的逆过程。给定旋转矩阵R求对应的李代数\phi。Eigen::Vector3d Log(const Eigen::Matrix3d R) { double theta acos((R.trace() - 1) / 2); // 通过迹求旋转角 // 处理theta为0或π的边界情况 if (theta 1e-10) { return Eigen::Vector3d::Zero(); } if (fabs(theta - M_PI) 1e-6) { // 当旋转角为π时轴的方向求解需要特殊处理可能不唯一 // 通常通过 (R - R^T) 的非零元素提取轴这里简化处理 Eigen::Matrix3d lnR (theta / (2 * sin(theta))) * (R - R.transpose()); return Sophus::SO3d::vee(lnR); } Eigen::Matrix3d lnR (theta / (2 * sin(theta))) * (R - R.transpose()); return Sophus::SO3d::vee(lnR); }注意事项奇异点处理当theta 0时李代数为零向量。当theta接近π时公式中的sin(theta)接近零会带来数值不稳定且旋转轴方向可能不唯一。这是李群的一个性质类似于三维旋转中的“万向节死锁”但含义不同。在实际SLAM中由于旋转是连续的、微小的很少会遇到θπ的情况但代码中必须有边界判断。vee算子是hat的逆运算将反对称矩阵变回向量。3.3 使用Sophus库进行李代数扰动在实际的SLAM优化中我们直接使用成熟的Sophus库。下面是一个在非线性优化中更新位姿的典型代码片段#include sophus/se3.hpp #include sophus/so3.hpp // 假设我们有一个初始位姿 T_init Sophus::SE3d T(T_init); // 优化变量是李代数 se3一个6维向量 Eigen::Matrixdouble, 6, 1 xi T.log(); // 对数映射得到李代数 // 在优化迭代中... for (int iter 0; iter iterations; iter) { Eigen::Matrixdouble, 6, 1 delta_xi; // 优化算法计算出的增量例如 -H^{-1} * b // ... 计算雅可比构建增量方程 H * delta_xi b ... // 更新位姿李代数左乘扰动更新 // T_new exp(delta_xi^{\wedge}) * T_old T Sophus::SE3d::exp(delta_xi) * T; // 或者更常见的是用“盒式加法”更新李代数变量本身 // xi_new xi_old \boxplus delta_xi // 对于李代数在切空间上的“加法”就是普通向量加法然后再指数映射回李群 xi delta_xi; // 在切空间李代数做加法 T Sophus::SE3d::exp(xi); // 映射回李群 }核心技巧Sophus::SE3d::exp(delta_xi) * T对应的是左乘扰动模型。优化时我们既可以直接更新李群T也可以更新李代数xi。后者在概念上更清晰优化变量始终是李代数xi每次迭代在xi所在的向量空间里做加法然后再用指数映射“拉回”到李群上得到用于计算的位姿T。Sophus库的manifold模块就是为了这种“流形优化”设计的。4. 实战中的深坑与精要那些书里没细说的掌握了基本公式和代码后要真正用好还得踩过一些坑。下面是我总结的几个关键点。4.1 左扰动 vs 右扰动到底用哪个这是初学者最容易混淆的地方。书中给出了两种扰动模型下的雅可比矩阵形式。左扰动扰动量\delta \phi乘在旋转矩阵的左边R \exp(\delta \phi^{\wedge}) R。右扰动扰动量乘在右边R R \exp(\delta \phi^{\wedge})。如何选择一致性在同一个项目中务必统一使用一种扰动模型。混用会导致雅可比矩阵错误优化不收敛。惯例在视觉SLAM领域左扰动模型是更常见的选择。很多经典文献和库如g2o, ceres的某些位姿参数化默认或推荐使用左扰动。推导来源左扰动模型通常来源于对旋转矩阵R的扰动而右扰动可能来源于对点坐标的扰动即RP中的R被扰动。两者最终得到的雅可比矩阵形式不同但本质上是等价的只是导数定义的位置不同。对于SO(3)左扰动雅可比和右扰动雅可比之间只差一个负号和一个伴随矩阵。简单原则如果你是跟着《十四讲》学习书中主要推导和示例代码如Ceres, g2o的例子都基于左扰动模型。所以无脑先用左扰动直到你需要在某个特定库如某些版本的g2o中配置右扰动时再去深入研究区别。4.2 李代数参数化中的“万向锁”问题注意这里说的不是欧拉角的万向锁。对于李代数\phi \theta \mathbf{a}当旋转角\theta接近2k\pi时特别是\theta0会出现问题。θ0此时旋转轴\mathbf{a}是未定义的零向量无法归一化。对应的指数映射exp(0)I但对数映射log(I)可以返回任意方向的轴因为没旋转。Sophus库通常定义log(I)返回零向量。这在优化中没问题因为零增量不会改变位姿。θ接近2π物理上旋转θ和旋转θ2π是一样的。但在李代数表示中\phi和\phi 2\pi\mathbf{a}对应同一个旋转矩阵。这称为周期性或覆盖性。在优化迭代中如果增量使θ跳变到另一个周期虽然位姿没变但李代数变量发生了巨大变化可能导致优化算法误判为发散。应对策略在正常的视觉SLAM中帧间运动是微小的θ通常远小于π所以基本不会遇到这个问题。但在初始化或回环检测时如果估计的旋转很大需要注意。一些稳健的实现会在更新李代数后将其“折叠”到主值区间如[-π, π)的θ范围内但这需要小心处理旋转轴。4.3 雅可比矩阵的数值求导验证推导出来的解析雅可比矩阵左扰动长这样J \frac{\partial e}{\partial \delta \xi} [\frac{\partial e}{\partial \delta \phi}, \frac{\partial e}{\partial \delta \rho}]其中关于旋转的部分可能包含像-(RPt)^{\wedge}这样的项。你如何确信自己推导或编程的雅可比是正确的黄金法则用数值求导进行验证这是调试优化程序最重要的手段之一。bool checkJacobian(const Eigen::Vector3d P, const Sophus::SE3d T) { Eigen::Vector2d e computeError(P, T); // 计算当前误差 Eigen::Matrixdouble, 2, 6 J_analytic; // 你的解析雅可比 computeAnalyticJacobian(P, T, J_analytic); // 计算解析雅可比 Eigen::Matrixdouble, 2, 6 J_numeric Eigen::Matrixdouble, 2, 6::Zero(); const double eps 1e-6; Eigen::Matrixdouble, 6, 1 xi T.log(); for (int i 0; i 6; i) { Eigen::Matrixdouble, 6, 1 delta_xi Eigen::Matrixdouble, 6, 1::Zero(); delta_xi[i] eps; // 对第i个李代数分量加一个小扰动 Sophus::SE3d T_perturbed Sophus::SE3d::exp(delta_xi) * T; // 左扰动 Eigen::Vector2d e_perturbed computeError(P, T_perturbed); J_numeric.col(i) (e_perturbed - e) / eps; // 数值导数 } // 比较 J_analytic 和 J_numeric double diff (J_analytic - J_numeric).norm(); if (diff 1e-5) { std::cout Jacobian mismatch! Diff diff std::endl; std::cout Analytic:\n J_analytic std::endl; std::cout Numeric:\n J_numeric std::endl; return false; } return true; }如果数值雅可比和解析雅可比相差很大那一定是你的解析推导或代码实现有误。务必在程序初始化后用几个随机点和李代数验证通过再进行大规模优化。4.4 李代数更新与“流形”优化库的使用在高级的优化库如Ceres Solver, g2o中我们不需要手动实现李代数的更新逻辑。这些库提供了“流形”Manifold或“局部参数化”LocalParameterization的概念。以Ceres为例当你定义了一个SE3位姿作为优化变量时你需要告诉Ceres这个变量不生活在普通的欧氏空间而是生活在SE(3)流形上。你需要提供流形空间的维度全局是6维李代数但局部切空间也是6维。Plus操作对应我们的“李代数增量更新位姿”即T_new exp(delta_xi) * T_old。ComputeJacobian操作可选提供从局部切空间到全局流形的雅可比。// 一个简化的Ceres LocalParameterization for SE(3) 示例 class SE3Parameterization : public ceres::LocalParameterization { public: virtual bool Plus(const double* T_raw, const double* delta_raw, double* T_plus_delta_raw) const { Eigen::Mapconst Sophus::SE3d T(T_raw); Eigen::Mapconst Eigen::Matrixdouble, 6, 1 delta(delta_raw); Sophus::SE3d delta_se3 Sophus::SE3d::exp(delta); Eigen::MapSophus::SE3d T_plus_delta(T_plus_delta_raw); T_plus_delta delta_se3 * T; // 左扰动更新 return true; } virtual bool ComputeJacobian(const double* T_raw, double* jacobian_raw) const { // 通常可以设为单位矩阵或者实现更精确的雅可比 Eigen::MapEigen::Matrixdouble, 7, 6, Eigen::RowMajor jacobian(jacobian_raw); jacobian.setIdentity(); // 简化处理 return true; } virtual int GlobalSize() const { return 7; } // SE3存储常用7维四元数(4) 平移(3) virtual int LocalSize() const { return 6; } // 局部李代数维度是6 };重要提示在Ceres中SE3常用一个7维向量表示前4个是四元数[qw, qx, qy, qz]后3个是平移。我们的Plus函数接收这个7维的T_raw和6维的delta_raw输出更新后的7维T_plus_delta_raw。这背后的数学正是李群李代数在起作用。5. 常见问题与调试技巧实录在实际编写和调试基于李代数的SLAM模块时以下是我踩过坑后总结出的检查清单。5.1 优化为什么不收敛这是最令人头疼的问题。如果优化发散或震荡请按以下顺序排查雅可比矩阵是否正确用4.3节的数值求导方法验证这是第一要务。90%的不收敛问题源于雅可比错误。扰动模型是否一致检查你的残差计算、雅可比推导和参数化更新或Plus函数是否使用了同一种扰动模型左乘。李代数更新公式是否正确确认是T_new exp(delta) * T_old左扰动还是T_new T_old * exp(delta)右扰动。与你的雅可比推导匹配。增量delta_xi是否过大非线性优化假设在当前点附近局部线性。如果步长太大线性近似失效。检查LM算法中的阻尼因子lambda是否在合理范围或者尝试使用更保守的线搜索。初始值是否太差李代数的局部性很强。如果初始位姿离真实值太远优化可能陷入局部极小值或发散。确保前端如特征匹配、PnP提供了一个还不错的初始位姿。数值稳定性问题检查指数映射exp和对数映射log在theta很小时的实现。使用Sophus这类经过充分测试的库可以避免很多问题。5.2 使用Sophus库的版本陷阱Sophus库有多个版本API有变化。Sophus 1.x (经典版本)需要依赖Eigen和fmtSE3类型是模板类Sophus::SE3Scalar常用Sophus::SE3d。Sophus 2.x (现代版本推荐)采用了更现代的C风格通常通过find_package(Sophus REQUIRED)引入命名空间和API可能略有调整。关键区别在 Sophus 1.x 中SE3的存储内部是4x4矩阵但构造函数和log()返回的类型需要注意。在 Sophus 2.x 中对Eigen的集成更紧密。务必查阅你所使用版本的官方文档或头文件。一个常见的编译错误是找不到Sophus::SE3很可能是因为你包含了错误的头文件路径或链接了错误版本的库。确保你的CMakeLists.txt正确配置。5.3 李代数与四元数的关系你可能会问既然四元数也能无奇异地表示旋转为什么还要学李代数四元数是旋转的一种紧凑、无奇异的表示单位四元数。它非常适合存储和插值。李代数是旋转或位姿在单位元处的切空间是用于优化计算的增量空间。它们不是二选一的关系而是协作关系存储和内部表示在SLAM系统中我们经常用四元数和平移向量一起存储一个位姿例如[qw, qx, qy, qz, tx, ty, tz]因为这样更紧凑、数值稳定。优化计算当进行非线性优化时我们将这个位姿无论是从矩阵还是四元数得来转换到其李代数表示\xi6维向量在这个向量空间里计算增量\delta\xi。更新用\delta\xi通过指数映射更新位姿这个更新过程在数学上等价于用李代数增量更新四元数但通过Sophus::SE3类可以屏蔽细节。最终存储将更新后的SE3再转换回四元数和平移的格式进行存储。Sophus库完美地封装了SO3基于四元数、SE3和它们的李代数so3、se3之间的所有转换。你只需要关心高层逻辑。5.4 从理论到全流程在视觉里程计中整合李代数为了形成闭环我们来看一个简化的视觉里程计VO单帧位姿估计流程看看李代数如何嵌入其中输入上一帧位姿T_k-1Sophus::SE3d当前帧特征点与上一帧的特征匹配对。初始化将T_k-1作为当前帧位姿T_k的初始估计。构建优化问题参数块T_k对应的李代数xi_k6维向量。残差块对于每一个匹配特征点对(p_i_prev, p_i_curr)计算重投影误差。将上一帧的3D点P_i通过逆深度或三角化得到用T_k变换到当前帧坐标系P_i_curr T_k * P_i。将P_i_curr投影到当前帧像素坐标u_i_proj \pi(K * P_i_curr)。残差e_i u_i_curr - u_i_proj2维向量。计算雅可比对于每个残差计算其关于李代数扰动\delta\xi的雅可比矩阵J_i2x6维使用左扰动模型公式。迭代优化将所有残差和雅可比堆叠构建增量方程H \delta\xi b。求解\delta\xi。更新李代数xi_k xi_k \delta\xi或更稳健的xi_k Sophus::SE3d::exp(delta_xi).log() * xi_k的某种形式取决于参数化。将更新后的xi_k指数映射回T_k T_k Sophus::SE3d::exp(xi_k)。判断是否收敛\delta\xi足够小或达到最大迭代次数。输出优化后的T_k作为当前帧的最终位姿。在整个过程中李代数xi充当了优化变量的角色而Sophus::SE3d对象T则是用于实际变换计算的工具。它们之间的来回转换通过.log()和Sophus::SE3d::exp()完成是流程的核心。理解并实现了这个流程你才算是真正把《视觉SLAM十四讲》第三讲的精髓融会贯通了。它不再是抽象的数学而是你手中解决SLAM位姿优化问题的利器。记住多推导多写代码验证多思考几何意义这块内容就一定能攻克。 SLAM中的李群与李代数：从数学原理到代码实现的位姿优化指南
1. 项目概述为什么SLAM绕不开李群与李代数如果你正在学习视觉SLAM尤其是《视觉SLAM十四讲》这本书那么“第三讲 李群和李代数”绝对是一个让你又爱又恨的坎。爱的是一旦跨过去你对机器人姿态、旋转、优化的理解会瞬间提升一个维度恨的是它可能是你遇到的第一个真正意义上的数学难关满篇的符号和抽象概念很容易让人望而却步。我自己当年啃这部分的时候也是反复琢磨了好几遍才打通任督二脉。今天我就以一个过来人的身份结合大量实际编程和调试的经验帮你把这块硬骨头嚼碎了、消化掉。简单来说这一讲的核心目标是解决一个在SLAM中至关重要的问题如何对机器人的旋转和平移合称“位姿”进行“微小”的调整和优化我们直观上知道机器人的位姿可以用一个旋转矩阵R和一个平移向量t来表示。但是当我们在后端优化中需要根据观测数据去微调这个位姿时问题就来了旋转矩阵R本身有9个数但它只代表3个自由度的旋转这9个数之间还存在复杂的约束正交且行列式为1。直接对这9个数求导、做加法进行优化会破坏这些约束导致结果不再是一个合法的旋转。这就好比你想微调一个已经校准好的精密仪器的一个螺丝结果直接操作却把整个结构弄散架了。李群和李代数就是为解决这个“带约束的优化”问题而引入的一套优雅的数学工具。它提供了一种方法让我们能把复杂的、带约束的旋转矩阵李群SO(3)映射到一个简单的、无约束的向量空间李代数so(3)里。在这个向量空间里我们可以放心大胆地做加法、求导数进行优化计算。算完之后再通过一个指数映射把结果变回合法的旋转矩阵。这个过程是理解后续非线性优化如高斯牛顿法、列文伯格-马夸尔特法如何应用于位姿估计的基石。没有它SLAM的后端优化就无法进行。所以无论你感觉多抽象请务必坚持学下去。接下来我会抛开复杂的数学推导外壳用最直白的语言和大量编程视角的解读带你掌握李群李代数的核心思想、关键公式以及它们在SLAM中的实际用法。2. 核心概念拆解群、李群、李代数到底在说什么很多教程一上来就扔出一堆定义让人云里雾里。我们换个顺序先从实际问题出发看看这些概念是如何被“逼”出来的。2.1 从旋转矩阵的麻烦说起为什么需要李代数假设我们有一个旋转矩阵R它满足R^T R I且det(R) 1。现在相机在运动R随时间t连续变化即R(t)是一个关于时间的函数并且始终是一个旋转矩阵。我们对R(t)求导看看会发生什么。对约束R(t)^T R(t) I两边关于时间求导\dot{R}(t)^T R(t) R(t)^T \dot{R}(t) 0令\phi(t) R(t)^T \dot{R}(t)代入上式得\phi(t)^T \phi(t) 0这说明\phi(t)是一个反对称矩阵对于三维空间任何一个反对称矩阵都对应一个三维向量我们记这个向量为\phi实际上书里常用\omega或\xi的上三角部分。于是有R(t)^T \dot{R}(t) \phi(t)^{\wedge}等式两边右乘R(t)得到\dot{R}(t) R(t) \phi(t)^{\wedge}看这个微分方程它告诉我们旋转矩阵R的导数等于R本身右乘一个由向量\phi生成的反对称矩阵。这个向量\phi就蕴含了旋转的瞬时方向和速度你可以把它想象成瞬时角速度。而所有可能的\phi构成的这个向量空间加上一个特定的运算李括号就是李代数 so(3)。所以李代数 so(3) 的物理意义非常直观它就是旋转的“瞬时变化率”所在的空间是一个三维向量空间没有讨厌的约束。我们可以在这个空间里自由地做加法、数乘来表征旋转的微小变化。2.2 李群 SO(3) 与李代数 so(3) 的“桥梁”指数与对数映射既然李代数 so(3) 是“变化率”那么如何从“变化率”积分得到最终的“旋转”呢这就要用到指数映射。对于上面的微分方程\dot{R}(t) R(t) \phi^{\wedge}给定初始条件R(0)I它的解是R(t) \exp(\phi^{\wedge} t)当t1时R \exp(\phi^{\wedge})。这意味着一个李代数向量\phi通过指数运算exp可以对应到李群旋转矩阵SO(3)中的一个元素R。反过来给定一个旋转矩阵R我们也可以通过对数映射log找到对应的李代数向量\phi。这两个映射构成了李群和李代数之间的核心对应关系指数映射 Exp:\phi \in \mathbb{R}^3 \rightarrow R \in SO(3)对数映射 Log:R \in SO(3) \rightarrow \phi \in \mathbb{R}^3注意这里的指数exp不是简单的标量指数而是矩阵指数。对于 so(3)它有闭式解这就是著名的罗德里格斯公式。这是需要你亲手推导和编程实现的关键公式。一个极其重要的类比你可以把 SO(3) 想象成一个三维的球面虽然实际是三维流形球面上的每一个点代表一个旋转姿态。而 so(3) 就是这个球面在“原点”单位矩阵I处的切空间。这个切空间是一个平坦的二维平面对于球面或三维空间对于SO(3)。指数映射就是把切空间so(3)里的一小段向量\phi沿着测地线“缠绕”到球面SO(3)上去。对数映射则是反过来。优化就是在切空间平坦、无约束里调整向量\phi然后映射回球面得到新的合法旋转。2.3 李代数求导解决位姿优化的核心武器终于到了最实战的部分。假设我们有一个空间点P经过旋转R和平移t后得到观测点P RP t。现在我们有一个观测误差e u - \pi(P)其中u是像素坐标\pi是投影函数。我们的目标是调整R和t使得误差e最小。关键问题误差e关于旋转R的导数怎么求我们不能直接对R的9个元素求导因为会破坏约束。李代数提供了完美的解决方案我们不直接优化R而是优化其对应的李代数\phi。当R有一个微小的扰动\Delta R时这个扰动可以对应到李代数空间的一个微小增量\delta \phi。即\Delta R \exp(\delta \phi^{\wedge})。那么扰动后的函数值为f(R \oplus \delta \phi) f(\exp(\delta \phi^{\wedge}) R)。这里的\oplus表示在李群上的左乘扰动。我们对\delta \phi求导就是李代数上的导数。书中引入了两个重要的扰动模型左乘扰动模型对R左乘一个微小扰动\Delta R \exp(\delta \phi^{\wedge})。右乘扰动模型对R右乘一个微小扰动。以左乘扰动为例经过推导这是必须掌握的推导过程误差关于李代数扰动的雅可比矩阵为\frac{\partial e}{\partial \delta \phi} -\frac{\partial \pi}{\partial P} \frac{\partial P}{\partial \delta \phi} -\frac{\partial \pi}{\partial P} [I, -(RPt)^{\wedge}]这个雅可比矩阵的维度是2x6对于重投影误差像素误差是2维扰动量\delta \phi和\delta p合起来是6维。有了这个雅可比我们就可以把它代入高斯牛顿或LM等优化算法中迭代求解最优的\delta \phi和\delta p平移扰动。每次迭代得到增量后用这个增量去更新李代数\phi \leftarrow \phi \delta \phi然后再通过指数映射更新旋转矩阵R \exp(\phi^{\wedge})。实操心得在代码中我们几乎从不直接存储和优化旋转矩阵R。对于位姿T属于SE(3)我们通常用一个Eigen::Matrix4d表示但在优化变量中我们优化的是一个6维向量\xi前3维是旋转对应的李代数\phi后3维是平移\rho。Sophus库中的SE3类就完美封装了T和\xi之间的转换。3. 从理论到代码手撕SO(3)和SE(3)的运算理论懂了不写代码等于没学。我们结合《十四讲》中的代码深入几个关键实现。3.1 实现SO(3)的指数映射罗德里格斯公式指数映射的闭式解就是罗德里格斯公式。给定李代数\phi \theta \mathbf{a}其中\theta是旋转角\mathbf{a}是单位旋转轴向量。Eigen::Matrix3d Exp(const Eigen::Vector3d phi) { double theta phi.norm(); // 旋转角 Eigen::Vector3d a phi.normalized(); // 旋转轴注意phi为零向量时的处理 Eigen::Matrix3d a_hat Sophus::SO3d::hat(a); // 反对称矩阵 Eigen::Matrix3d R Eigen::Matrix3d::Identity() sin(theta) * a_hat (1 - cos(theta)) * a_hat * a_hat; return R; }关键点需要处理phi为零向量的特殊情况此时theta0应直接返回单位阵。a_hat * a_hat这个运算等于a*a^T - I这个恒等式可以用于化简和验证能加深你对公式的理解。这个公式非常直观I是基础sin(theta)*a_hat是旋转的主体线性部分(1-cos(theta))*a_hat*a_hat是补偿项。3.2 实现SO(3)的对数映射对数映射是指数映射的逆过程。给定旋转矩阵R求对应的李代数\phi。Eigen::Vector3d Log(const Eigen::Matrix3d R) { double theta acos((R.trace() - 1) / 2); // 通过迹求旋转角 // 处理theta为0或π的边界情况 if (theta 1e-10) { return Eigen::Vector3d::Zero(); } if (fabs(theta - M_PI) 1e-6) { // 当旋转角为π时轴的方向求解需要特殊处理可能不唯一 // 通常通过 (R - R^T) 的非零元素提取轴这里简化处理 Eigen::Matrix3d lnR (theta / (2 * sin(theta))) * (R - R.transpose()); return Sophus::SO3d::vee(lnR); } Eigen::Matrix3d lnR (theta / (2 * sin(theta))) * (R - R.transpose()); return Sophus::SO3d::vee(lnR); }注意事项奇异点处理当theta 0时李代数为零向量。当theta接近π时公式中的sin(theta)接近零会带来数值不稳定且旋转轴方向可能不唯一。这是李群的一个性质类似于三维旋转中的“万向节死锁”但含义不同。在实际SLAM中由于旋转是连续的、微小的很少会遇到θπ的情况但代码中必须有边界判断。vee算子是hat的逆运算将反对称矩阵变回向量。3.3 使用Sophus库进行李代数扰动在实际的SLAM优化中我们直接使用成熟的Sophus库。下面是一个在非线性优化中更新位姿的典型代码片段#include sophus/se3.hpp #include sophus/so3.hpp // 假设我们有一个初始位姿 T_init Sophus::SE3d T(T_init); // 优化变量是李代数 se3一个6维向量 Eigen::Matrixdouble, 6, 1 xi T.log(); // 对数映射得到李代数 // 在优化迭代中... for (int iter 0; iter iterations; iter) { Eigen::Matrixdouble, 6, 1 delta_xi; // 优化算法计算出的增量例如 -H^{-1} * b // ... 计算雅可比构建增量方程 H * delta_xi b ... // 更新位姿李代数左乘扰动更新 // T_new exp(delta_xi^{\wedge}) * T_old T Sophus::SE3d::exp(delta_xi) * T; // 或者更常见的是用“盒式加法”更新李代数变量本身 // xi_new xi_old \boxplus delta_xi // 对于李代数在切空间上的“加法”就是普通向量加法然后再指数映射回李群 xi delta_xi; // 在切空间李代数做加法 T Sophus::SE3d::exp(xi); // 映射回李群 }核心技巧Sophus::SE3d::exp(delta_xi) * T对应的是左乘扰动模型。优化时我们既可以直接更新李群T也可以更新李代数xi。后者在概念上更清晰优化变量始终是李代数xi每次迭代在xi所在的向量空间里做加法然后再用指数映射“拉回”到李群上得到用于计算的位姿T。Sophus库的manifold模块就是为了这种“流形优化”设计的。4. 实战中的深坑与精要那些书里没细说的掌握了基本公式和代码后要真正用好还得踩过一些坑。下面是我总结的几个关键点。4.1 左扰动 vs 右扰动到底用哪个这是初学者最容易混淆的地方。书中给出了两种扰动模型下的雅可比矩阵形式。左扰动扰动量\delta \phi乘在旋转矩阵的左边R \exp(\delta \phi^{\wedge}) R。右扰动扰动量乘在右边R R \exp(\delta \phi^{\wedge})。如何选择一致性在同一个项目中务必统一使用一种扰动模型。混用会导致雅可比矩阵错误优化不收敛。惯例在视觉SLAM领域左扰动模型是更常见的选择。很多经典文献和库如g2o, ceres的某些位姿参数化默认或推荐使用左扰动。推导来源左扰动模型通常来源于对旋转矩阵R的扰动而右扰动可能来源于对点坐标的扰动即RP中的R被扰动。两者最终得到的雅可比矩阵形式不同但本质上是等价的只是导数定义的位置不同。对于SO(3)左扰动雅可比和右扰动雅可比之间只差一个负号和一个伴随矩阵。简单原则如果你是跟着《十四讲》学习书中主要推导和示例代码如Ceres, g2o的例子都基于左扰动模型。所以无脑先用左扰动直到你需要在某个特定库如某些版本的g2o中配置右扰动时再去深入研究区别。4.2 李代数参数化中的“万向锁”问题注意这里说的不是欧拉角的万向锁。对于李代数\phi \theta \mathbf{a}当旋转角\theta接近2k\pi时特别是\theta0会出现问题。θ0此时旋转轴\mathbf{a}是未定义的零向量无法归一化。对应的指数映射exp(0)I但对数映射log(I)可以返回任意方向的轴因为没旋转。Sophus库通常定义log(I)返回零向量。这在优化中没问题因为零增量不会改变位姿。θ接近2π物理上旋转θ和旋转θ2π是一样的。但在李代数表示中\phi和\phi 2\pi\mathbf{a}对应同一个旋转矩阵。这称为周期性或覆盖性。在优化迭代中如果增量使θ跳变到另一个周期虽然位姿没变但李代数变量发生了巨大变化可能导致优化算法误判为发散。应对策略在正常的视觉SLAM中帧间运动是微小的θ通常远小于π所以基本不会遇到这个问题。但在初始化或回环检测时如果估计的旋转很大需要注意。一些稳健的实现会在更新李代数后将其“折叠”到主值区间如[-π, π)的θ范围内但这需要小心处理旋转轴。4.3 雅可比矩阵的数值求导验证推导出来的解析雅可比矩阵左扰动长这样J \frac{\partial e}{\partial \delta \xi} [\frac{\partial e}{\partial \delta \phi}, \frac{\partial e}{\partial \delta \rho}]其中关于旋转的部分可能包含像-(RPt)^{\wedge}这样的项。你如何确信自己推导或编程的雅可比是正确的黄金法则用数值求导进行验证这是调试优化程序最重要的手段之一。bool checkJacobian(const Eigen::Vector3d P, const Sophus::SE3d T) { Eigen::Vector2d e computeError(P, T); // 计算当前误差 Eigen::Matrixdouble, 2, 6 J_analytic; // 你的解析雅可比 computeAnalyticJacobian(P, T, J_analytic); // 计算解析雅可比 Eigen::Matrixdouble, 2, 6 J_numeric Eigen::Matrixdouble, 2, 6::Zero(); const double eps 1e-6; Eigen::Matrixdouble, 6, 1 xi T.log(); for (int i 0; i 6; i) { Eigen::Matrixdouble, 6, 1 delta_xi Eigen::Matrixdouble, 6, 1::Zero(); delta_xi[i] eps; // 对第i个李代数分量加一个小扰动 Sophus::SE3d T_perturbed Sophus::SE3d::exp(delta_xi) * T; // 左扰动 Eigen::Vector2d e_perturbed computeError(P, T_perturbed); J_numeric.col(i) (e_perturbed - e) / eps; // 数值导数 } // 比较 J_analytic 和 J_numeric double diff (J_analytic - J_numeric).norm(); if (diff 1e-5) { std::cout Jacobian mismatch! Diff diff std::endl; std::cout Analytic:\n J_analytic std::endl; std::cout Numeric:\n J_numeric std::endl; return false; } return true; }如果数值雅可比和解析雅可比相差很大那一定是你的解析推导或代码实现有误。务必在程序初始化后用几个随机点和李代数验证通过再进行大规模优化。4.4 李代数更新与“流形”优化库的使用在高级的优化库如Ceres Solver, g2o中我们不需要手动实现李代数的更新逻辑。这些库提供了“流形”Manifold或“局部参数化”LocalParameterization的概念。以Ceres为例当你定义了一个SE3位姿作为优化变量时你需要告诉Ceres这个变量不生活在普通的欧氏空间而是生活在SE(3)流形上。你需要提供流形空间的维度全局是6维李代数但局部切空间也是6维。Plus操作对应我们的“李代数增量更新位姿”即T_new exp(delta_xi) * T_old。ComputeJacobian操作可选提供从局部切空间到全局流形的雅可比。// 一个简化的Ceres LocalParameterization for SE(3) 示例 class SE3Parameterization : public ceres::LocalParameterization { public: virtual bool Plus(const double* T_raw, const double* delta_raw, double* T_plus_delta_raw) const { Eigen::Mapconst Sophus::SE3d T(T_raw); Eigen::Mapconst Eigen::Matrixdouble, 6, 1 delta(delta_raw); Sophus::SE3d delta_se3 Sophus::SE3d::exp(delta); Eigen::MapSophus::SE3d T_plus_delta(T_plus_delta_raw); T_plus_delta delta_se3 * T; // 左扰动更新 return true; } virtual bool ComputeJacobian(const double* T_raw, double* jacobian_raw) const { // 通常可以设为单位矩阵或者实现更精确的雅可比 Eigen::MapEigen::Matrixdouble, 7, 6, Eigen::RowMajor jacobian(jacobian_raw); jacobian.setIdentity(); // 简化处理 return true; } virtual int GlobalSize() const { return 7; } // SE3存储常用7维四元数(4) 平移(3) virtual int LocalSize() const { return 6; } // 局部李代数维度是6 };重要提示在Ceres中SE3常用一个7维向量表示前4个是四元数[qw, qx, qy, qz]后3个是平移。我们的Plus函数接收这个7维的T_raw和6维的delta_raw输出更新后的7维T_plus_delta_raw。这背后的数学正是李群李代数在起作用。5. 常见问题与调试技巧实录在实际编写和调试基于李代数的SLAM模块时以下是我踩过坑后总结出的检查清单。5.1 优化为什么不收敛这是最令人头疼的问题。如果优化发散或震荡请按以下顺序排查雅可比矩阵是否正确用4.3节的数值求导方法验证这是第一要务。90%的不收敛问题源于雅可比错误。扰动模型是否一致检查你的残差计算、雅可比推导和参数化更新或Plus函数是否使用了同一种扰动模型左乘。李代数更新公式是否正确确认是T_new exp(delta) * T_old左扰动还是T_new T_old * exp(delta)右扰动。与你的雅可比推导匹配。增量delta_xi是否过大非线性优化假设在当前点附近局部线性。如果步长太大线性近似失效。检查LM算法中的阻尼因子lambda是否在合理范围或者尝试使用更保守的线搜索。初始值是否太差李代数的局部性很强。如果初始位姿离真实值太远优化可能陷入局部极小值或发散。确保前端如特征匹配、PnP提供了一个还不错的初始位姿。数值稳定性问题检查指数映射exp和对数映射log在theta很小时的实现。使用Sophus这类经过充分测试的库可以避免很多问题。5.2 使用Sophus库的版本陷阱Sophus库有多个版本API有变化。Sophus 1.x (经典版本)需要依赖Eigen和fmtSE3类型是模板类Sophus::SE3Scalar常用Sophus::SE3d。Sophus 2.x (现代版本推荐)采用了更现代的C风格通常通过find_package(Sophus REQUIRED)引入命名空间和API可能略有调整。关键区别在 Sophus 1.x 中SE3的存储内部是4x4矩阵但构造函数和log()返回的类型需要注意。在 Sophus 2.x 中对Eigen的集成更紧密。务必查阅你所使用版本的官方文档或头文件。一个常见的编译错误是找不到Sophus::SE3很可能是因为你包含了错误的头文件路径或链接了错误版本的库。确保你的CMakeLists.txt正确配置。5.3 李代数与四元数的关系你可能会问既然四元数也能无奇异地表示旋转为什么还要学李代数四元数是旋转的一种紧凑、无奇异的表示单位四元数。它非常适合存储和插值。李代数是旋转或位姿在单位元处的切空间是用于优化计算的增量空间。它们不是二选一的关系而是协作关系存储和内部表示在SLAM系统中我们经常用四元数和平移向量一起存储一个位姿例如[qw, qx, qy, qz, tx, ty, tz]因为这样更紧凑、数值稳定。优化计算当进行非线性优化时我们将这个位姿无论是从矩阵还是四元数得来转换到其李代数表示\xi6维向量在这个向量空间里计算增量\delta\xi。更新用\delta\xi通过指数映射更新位姿这个更新过程在数学上等价于用李代数增量更新四元数但通过Sophus::SE3类可以屏蔽细节。最终存储将更新后的SE3再转换回四元数和平移的格式进行存储。Sophus库完美地封装了SO3基于四元数、SE3和它们的李代数so3、se3之间的所有转换。你只需要关心高层逻辑。5.4 从理论到全流程在视觉里程计中整合李代数为了形成闭环我们来看一个简化的视觉里程计VO单帧位姿估计流程看看李代数如何嵌入其中输入上一帧位姿T_k-1Sophus::SE3d当前帧特征点与上一帧的特征匹配对。初始化将T_k-1作为当前帧位姿T_k的初始估计。构建优化问题参数块T_k对应的李代数xi_k6维向量。残差块对于每一个匹配特征点对(p_i_prev, p_i_curr)计算重投影误差。将上一帧的3D点P_i通过逆深度或三角化得到用T_k变换到当前帧坐标系P_i_curr T_k * P_i。将P_i_curr投影到当前帧像素坐标u_i_proj \pi(K * P_i_curr)。残差e_i u_i_curr - u_i_proj2维向量。计算雅可比对于每个残差计算其关于李代数扰动\delta\xi的雅可比矩阵J_i2x6维使用左扰动模型公式。迭代优化将所有残差和雅可比堆叠构建增量方程H \delta\xi b。求解\delta\xi。更新李代数xi_k xi_k \delta\xi或更稳健的xi_k Sophus::SE3d::exp(delta_xi).log() * xi_k的某种形式取决于参数化。将更新后的xi_k指数映射回T_k T_k Sophus::SE3d::exp(xi_k)。判断是否收敛\delta\xi足够小或达到最大迭代次数。输出优化后的T_k作为当前帧的最终位姿。在整个过程中李代数xi充当了优化变量的角色而Sophus::SE3d对象T则是用于实际变换计算的工具。它们之间的来回转换通过.log()和Sophus::SE3d::exp()完成是流程的核心。理解并实现了这个流程你才算是真正把《视觉SLAM十四讲》第三讲的精髓融会贯通了。它不再是抽象的数学而是你手中解决SLAM位姿优化问题的利器。记住多推导多写代码验证多思考几何意义这块内容就一定能攻克。