1. 从旋转矩阵到李群为什么我们需要李代数如果你正在学习视觉SLAM或者任何涉及三维空间刚体运动的领域那么“李群”和“李代数”这两个词一定让你既敬畏又头疼。在《视觉SLAM十四讲》的第三讲中高翔博士将这两个数学概念从理论神坛拉到了工程实践的地面。很多人学到这里会卡住感觉公式推导眼花缭乱不明白为什么好好的旋转矩阵不用非要引入这么一套复杂的数学工具。今天我就以一个过来人的身份结合我自己的踩坑和实战经验帮你把这块硬骨头啃下来。核心就一句话李群和李代数是用来解决“旋转矩阵不好求导”这个工程死结的钥匙。想象一下这个场景你的视觉里程计通过特征匹配估计出相机在两个相邻帧之间有一个微小的旋转和平移。为了得到最优的估计你需要构建一个优化问题比如Bundle Adjustment不断调整这个旋转和平移使得投影误差最小。这里的“调整”在数学上就是对旋转矩阵R求导。问题来了旋转矩阵R本身有严格的约束R^T R I, det(R)1它是一个“干净”但“僵硬”的集合我们称之为特殊正交群SO(3)。直接对R加一个微小的扰动ΔR得到的新矩阵(RΔR)很可能就不再是旋转矩阵了它破坏了正交性约束。这就好比你想微调一个精密钟表的齿轮角度但你的工具只能对整个齿轮进行粗暴的敲打结果齿轮变形整个钟表报废。所以在SO(3)这个“干净”的空间里我们没法定义那种“微小变化”的概念也就没法进行基于梯度的优化。那么工程师们是怎么解决的呢他们绕了个弯子不去直接捣鼓那个“干净”的旋转矩阵R而是去研究产生这个旋转的“过程”。任何一个旋转都可以看成是绕着某个单位轴n旋转一个角度θ。这个旋转过程是连续的、可微的。李代数so(3)描述的就是这个旋转的“瞬时速度”或者说“无穷小旋转”。它位于一个向量空间里没有讨厌的约束可以自由地加加减减、求导。我们熟悉的旋转矩阵R李群SO(3)的元素和这个旋转向量φ李代数so(3)的元素之间通过一个叫指数映射的神奇公式联系起来R exp(φ^∧)。这个公式的威力在于它把在“干净但僵硬”的李群上的优化问题转化到了“杂乱但自由”的李代数向量空间里去进行。我们在向量空间里对李代数φ求导、更新然后再通过指数映射变回李群上的旋转矩阵R。这就是整个SLAM后端优化得以实现的数学基石。所以别再被那一堆∧、∨、exp、log符号吓到。它们本质上是一套精巧的“翻译”机制让我们能用熟悉的微积分工具去处理那些带有特殊约束的几何对象。接下来我们就一层层剥开它的外壳。2. SO(3)与SE(3)理解三维运动的两种“语言”在进入具体的李代数运算之前我们必须先打好地基彻底理解我们描述的对象是什么。在三维空间中刚体运动相机位姿包含旋转和平移。数学上我们用两种结构来精确描述它们SO(3)描述纯旋转SE(3)描述旋转加平移。你可以把它们理解为描述同一事物的两种不同“语言”或“坐标系”各有各的适用场景和优缺点。2.1 SO(3)旋转的“标准普通话”特殊正交群SO(3)是所有3x3旋转矩阵R的集合满足两个核心约束正交性R^T R I。这意味着旋转是保长的不会拉伸或压缩物体。行列式为1det(R) 1。这保证了旋转是“手性保持”的不会变成镜像。一个旋转矩阵有9个数但由于6个约束正交性提供6个独立方程它实际上只有3个自由度。这对应着旋转的三个角度例如欧拉角偏航、俯仰、滚动。SO(3)作为一个“群”意味着两个旋转矩阵相乘结果还是旋转矩阵存在单位元单位矩阵I每个旋转都有逆转置矩阵R^T。为什么SO(3)在优化中是个“坑”假设我们有一个优化目标函数f(R)需要对R求导。我们尝试用导数的定义f(R ΔR)。但ΔR是一个任意的小矩阵加上去之后(RΔR)几乎肯定不满足R^T R I了。也就是说我们迈出的“一小步”已经离开了SO(3)这个流形表面掉到了“非法”的区域。在这个非法区域计算导数是没有意义的。这就好比你想沿着地球表面一个球面从北京走到上海但你的每一步都要求是“直线”向量加法结果第二步就钻到地底下去了这显然不是沿着地表行走的正确方式。SO(3)就是一个弯曲的“球面”空间更准确地说是一个三维流形我们不能用平坦空间里的向量加法来定义它上面的变化。2.2 SE(3)位姿的“完整表达”特殊欧氏群SE(3)则用来描述完整的刚体变换位姿。它由一个旋转矩阵R属于SO(3)和一个平移向量t组成通常写成齐次坐标形式的一个4x4矩阵TT [ R t ] [ 0^T 1 ]其中R是3x3旋转矩阵t是3x1平移向量。SE(3)同样构成一个群两个变换矩阵相乘代表变换的复合。它的自由度是63个旋转3个平移。SE(3)的优化之痛SE(3)继承了SO(3)的所有问题并且因为结合了旋转和平移变得更复杂。我们同样不能直接对T进行加法扰动。我们需要一种能够统一处理R和t并且满足它们各自几何约束的优化方法。注意这里常见的误解是试图用四元数代替旋转矩阵来避免求导问题。四元数确实只有4个数和1个约束比矩阵形式简洁常用于插值和存储。但在优化中四元数同样有单位范数约束q^T q1这同样是一个非线性约束在优化中需要额外处理如使用增量δθ在切空间更新。而李代数的方法提供了一种更统一、更理论坚实的框架来处理SO(3)和SE(3)的约束。2.3 李代数为“弯曲空间”定义的“切向量”为了解决在SO(3)和SE(3)上求导的问题数学家引入了李代数。直观上你可以把李代数想象成在流形如SO(3)球面上某一点如单位旋转I的切空间。李群如SO(3)是那个弯曲的流形本身。李代数如so(3)是在流形上某点通常选单位元的切平面。这个切平面是一个向量空间。关键来了虽然流形本身是弯曲的不能做加法但切空间是平坦的向量空间可以做标准的向量加法李代数so(3)的元素就是一个3维向量φ (φ1, φ2, φ3)^T。这个φ有一个非常漂亮的几何解释它的方向代表旋转轴n它的模长代表旋转角度θ。即 φ θn。那么这个切空间里的向量φ和流形上的点R是怎么对应的呢这就引出了李群与李代数之间最重要的两个映射指数映射和对数映射。3. 指数与对数映射连接李群与李代数的桥梁指数映射exp和对数映射log是穿梭于李群弯曲空间和李代数平坦切空间之间的“传送门”。理解了它们你就掌握了这套工具的核心。3.1 SO(3)上的指数映射从旋转向量到旋转矩阵对于SO(3)和它的李代数so(3)指数映射的公式是R exp(φ^∧)这里的∧是一个算子它把一个3维向量φ映射成一个3x3的反对称矩阵φ^∧φ [φ1, φ2, φ3]^T φ^∧ [ 0 -φ3 φ2 ] [ φ3 0 -φ1 ] [ -φ2 φ1 0 ]那么exp(φ^∧)具体怎么算呢它有一个闭式解这就是著名的罗德里格斯公式R cosθ I (1 - cosθ) n n^T sinθ n^∧其中θ ||φ||n φ/θ。这个公式直接给出了从旋转向量(θn)到旋转矩阵R的转换。所以指数映射的物理意义非常明确它把描述“旋转方向和角度”的向量φ变换成了对应的旋转矩阵R。反过来对数映射log则是从旋转矩阵R求解旋转向量φ。实际上就是罗德里格斯公式的逆过程。在代码实现中我们可以通过以下公式计算θ arccos( (tr(R) - 1)/2 ) R n n (这里n是旋转轴是矩阵R特征值1对应的特征向量)那么 φ θn。一个关键洞见当旋转角度θ很小时根据罗德里格斯公式sinθ ≈ θ cosθ ≈ 1。此时指数映射可以近似为R ≈ I φ^∧这正好对应了李代数φ是旋转矩阵R在单位元I处的切向量这一几何图像。微小的旋转φ几乎就是直接在单位矩阵上加一个反对称矩阵扰动。3.2 SE(3)上的指数映射更复杂但模式统一对于SE(3)它的李代数se(3)是一个6维向量ξ [ρ, φ]^T其中ρ是3维向量与平移有关φ就是so(3)中的旋转向量。 同样我们定义∧算子将6维向量ξ映射为4x4矩阵ξ^∧ξ^∧ [ φ^∧ ρ ] [ 0^T 0 ]SE(3)的指数映射公式为T exp(ξ^∧)它也有闭式解但比SO(3)复杂一些T [ R Jρ ] [ 0^T 1 ]其中R就是由φ通过SO(3)的指数映射得到的旋转矩阵而J是一个3x3的矩阵称为SO(3)的左雅可比矩阵其表达式为J (sinθ/θ) I (1 - sinθ/θ) n n^T ((1-cosθ)/θ) n^∧当θ很小时J ≈ I。这个公式揭示了SE(3)李代数ξ的几何意义φ部分仍然对应旋转而ρ并不直接等于平移t。在指数映射下t Jρ。只有当旋转φ为0时JI此时ρ才等于t。这意味着在SE(3)中旋转和平移在李代数层面是耦合的不是一个简单的并列关系。这是很多初学者容易混淆的地方。实操心得在实际SLAM代码中如g2o, Ceres, GTSAM等优化库我们几乎从不直接使用这些闭式解进行指数/对数运算。因为这些库内部已经为我们高效、稳定地实现了这些转换。我们的任务是理解当我们定义一个Pose变量并在优化中声明其更新量是一个6维向量δξ时优化库在底层正是通过T_new exp(δξ^∧) * T_old来更新位姿的。理解这个原理才能正确设置参数和解读结果。4. 李代数求导扰动模型的工程实现前面我们铺陈了这么多理论最终都是为了服务一个目标对旋转矩阵或变换矩阵求导。在SLAM的图优化中误差函数通常关于位姿T的导数。由于T不能用加法更新我们采用李代数的方法。这里有两种等价的模型李代数求导和扰动模型。扰动模型在形式上更简洁是工程实践中的首选。4.1 SO(3)李代数求导直接法设空间点p经过旋转R后变为p‘ Rp。现在考虑旋转R对应的李代数为φ我们计算p‘关于φ的导数。 根据导数的定义我们给φ加上一个小扰动δφ看看p‘的变化李代数更新φ → φ δφ对应李群更新左乘R → exp(δφ^∧) R点p的新位置p‘ exp(δφ^∧) R p ≈ (I δφ^∧) R p扰动后的变化量p‘ - Rp ≈ δφ^∧ (R p)利用叉积的性质 a^∧ b -b^∧ a可以将δφ^∧ (Rp) 转化为 -(Rp)^∧ δφ。因此我们得到导数∂(Rp) / ∂φ -(Rp)^∧这个导数是一个3x3的矩阵。它意味着当李代数φ发生微小变化δφ时点Rp的变化量大约是 -(Rp)^∧ δφ。4.2 SO(3)扰动模型左扰动直接对李代数φ求导导数形式比较复杂出现了负号和外积。更常用的方法是扰动模型。我们不对李代数φ加扰动而是直接对李群R加一个左乘的微小扰动ΔR。这个扰动ΔR本身也对应一个李代数δφ即ΔR exp(δφ^∧)。那么加扰动后的点为p‘ (ΔR) R p exp(δφ^∧) R p ≈ (I δφ^∧) R p 同样计算变化量p‘ - Rp ≈ δφ^∧ (R p) -(Rp)^∧ δφ。我们发现最终导数的形式和李代数求导一模一样这是因为无论扰动加在李群还是李代数上当扰动无穷小时它们产生的效果是等价的。扰动模型的优势在于它的推导更直观我们直接想象在现有的旋转R上再左乘一个微小的旋转ΔR。4.3 SE(3)上的扰动模型求导在SLAM中更常见的是对变换矩阵T的求导。设世界坐标系下的点p经过变换T后得到相机坐标系下的点p‘ T p R p t。 我们给T一个左扰动ΔT exp(δξ^∧)扰动后的点为 p‘ ΔT T p ≈ (I δξ^∧) T p T p δξ^∧ (T p)现在计算变化量δp p‘ - T p δξ^∧ (T p)。 我们把δξ [δρ, δφ]^T T p [Rpt, 1]^T齐次坐标的最后一项是1代入δξ^∧的公式经过一系列推导核心是利用叉积的性质可以得到一个非常整洁的结果δp [ I -(Rpt)^∧ ] δξ [ I -(Rpt)^∧ ] [ δρ ][ δφ ]我们把那个3x6的矩阵记为一个算符 (T p)⊙ 的前三行第四行齐次坐标求导为0。于是导数可以写为∂(T p) / ∂δξ [ I, -(Rpt)^∧ ]这是一个3x6的矩阵。它就是我们一直在寻找的、关于位姿李代数扰动的导数这个雅可比矩阵在优化中至关重要前3列是单位阵I对应平移部分δρ的导数。这很直观平移扰动直接导致点发生相同的偏移。后3列是 -(Rpt)^∧对应旋转部分δφ的导数。这和SO(3)的导数形式一致只不过作用点变成了变换后的点(Rpt)。4.4 在非线性优化中的应用雅可比矩阵的计算在视觉SLAM的重投影误差模型中我们有一个三维地图点P_w世界坐标通过估计的相机位姿T_cw变换到相机坐标系P_c T_cw P_w。然后再通过相机内参K投影到像素平面p_uv K (P_c / P_c.z)这里进行了归一化。我们的误差函数e是观测到的像素坐标z与预测的像素坐标p_uv之差e z - p_uv。 在优化中我们需要计算误差e关于位姿李代数扰动δξ的雅可比矩阵J。根据链式法则∂e/∂δξ (∂e/∂p_uv) * (∂p_uv/∂P_c) * (∂P_c/∂δξ)∂e/∂p_uv -I (2x2矩阵因为e和p_uv都是2维像素坐标)。∂p_uv/∂P_c 是像素坐标关于相机坐标系下三维点的导数这涉及相机投影模型针孔模型畸变的雅可比是一个2x3的矩阵。∂P_c/∂δξ就是我们上一节推导的核心结果P_c T P_w所以 ∂(T P_w)/∂δξ [ I, -(Rpt)^∧ ]这是一个3x6的矩阵。将这三部分相乘我们就得到了一个2x6的雅可比矩阵J。这个J告诉优化器如高斯-牛顿法、列文伯格-马夸尔特法当位姿沿着李代数空间中的某个方向δξ做微小移动时重投影误差e会如何变化。优化器正是利用这个信息反复迭代求解最优的位姿T。踩坑实录在自己实现BA时最容易出错的地方就是雅可比矩阵的维度对齐和正负号。务必注意左扰动 vs 右扰动上述推导默认使用左扰动模型即扰动乘在变换矩阵的左边ΔT * T。如果你的参数化方式是T_wc世界到相机那么左扰动是合理的。如果你的参数化是T_cw相机到世界则需要使用右扰动模型 T * ΔT其导数形式会略有不同。必须前后一致否则优化会发散。李代数的顺序通常李代数ξ [ρ, φ]^T即平移在前旋转在后。但在某些库如Sophus中顺序可能是[φ, ρ]^T。在计算雅可比时必须和你使用的李代数库的顺序保持一致。归一化平面的导数在计算∂p_uv/∂P_c时P_c是三维点需要先投影到归一化平面除以Z坐标再应用内参。这一步的求导容易忘记除以Z²项导致雅可比计算错误。5. 实践指南在C中操作SO(3)、SE(3)与李代数理论最终要落地为代码。在C中我们通常不会从头实现李群李代数的运算而是依赖成熟的数学库。最常用的就是Eigen库配合Sophus库。Eigen提供高效的矩阵运算Sophus则在Eigen的基础上封装了SO(3)、SE(3)、李代数以及它们之间的转换。5.1 使用Sophus库的基本操作首先确保你的项目包含了Eigen和Sophus。Sophus的安装通常很简单可以从GitHub克隆源码编译。#include iostream #include Eigen/Core #include Eigen/Geometry #include sophus/so3.hpp #include sophus/se3.hpp int main() { // 1. 旋转矩阵与SO(3) Eigen::Matrix3d R Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix(); // 绕Z轴旋转90度 Sophus::SO3d SO3_R(R); // 从旋转矩阵构造SO(3) std::cout SO(3) from matrix:\n SO3_R.matrix() std::endl; // 2. 李代数 so(3) Eigen::Vector3d so3 SO3_R.log(); // 对数映射SO(3) - so(3) std::cout so3 so3.transpose() std::endl; // 应该近似为 (0,0,pi/2) std::cout so3 hat \n Sophus::SO3d::hat(so3) std::endl; // 向量到反对称矩阵 // 3. 指数映射 Sophus::SO3d SO3_R2 Sophus::SO3d::exp(so3); // 指数映射so(3) - SO(3) std::cout SO3_R2 \n SO3_R2.matrix() std::endl; // 应与R相同 // 4. 扰动模型更新 Eigen::Vector3d update_so3(0.01, 0.02, 0.03); // 假设更新量为一个小的旋转向量 Sophus::SO3d SO3_updated Sophus::SO3d::exp(update_so3) * SO3_R; // 左乘扰动 std::cout SO3_updated \n SO3_updated.matrix() std::endl; // 5. SE(3) 操作 Eigen::Vector3d t(1, 0, 0); // 平移 Sophus::SE3d SE3_Rt(R, t); // 从R,t构造SE(3) std::cout SE3 pose \n SE3_Rt.matrix() std::endl; // 6. SE(3) 的李代数 se(3) 是一个6维向量 typedef Eigen::Matrixdouble, 6, 1 Vector6d; Vector6d se3 SE3_Rt.log(); // 前三维是平移相关rho后三维是旋转phi std::cout se3 se3.transpose() std::endl; // 7. SE(3) 的指数映射和扰动更新 Vector6d update_se3; update_se3.setZero(); update_se3(0, 0) 0.1; // 给平移部分一个小扰动 update_se3(3, 0) 0.01; // 给旋转部分一个小扰动 Sophus::SE3d SE3_updated Sophus::SE3d::exp(update_se3) * SE3_Rt; // 左乘扰动更新 std::cout SE3_updated \n SE3_updated.matrix() std::endl; // 8. 使用SE(3)变换一个点 Eigen::Vector3d p_w(1, 0, 0); // 世界坐标系下的点 Eigen::Vector3d p_c SE3_Rt * p_w; // 变换到相机坐标系等价于 R*p_w t std::cout p in camera frame p_c.transpose() std::endl; return 0; }这段代码展示了Sophus库最基本的使用方法。关键点在于理解.log()方法实现了从李群到李代数的映射对数映射。.exp()静态方法实现了从李代数到李群的映射指数映射。Sophus::SO3d::hat(v)将3维向量变为反对称矩阵。更新位姿的标准模式是T_new Sophus::SE3d::exp(δξ) * T_old。这里的δξ就是优化器中计算得到的更新量一个6维向量。5.2 在优化框架中集成李代数在实际的SLAM系统中我们使用g2o、Ceres或GTSAM等优化库。这些库已经为我们定义好了VertexSE3Expmap这样的顶点类型它内部使用的正是李代数的参数化方式。以g2o为例当你定义一个位姿顶点并设置其估计值时你设置的是SE(3)矩阵T。但在优化过程中g2o内部存储和更新的是该顶点对应的李代数ξ。当你调用vertex-oplus(update)时update就是一个6维向量δξg2o内部执行的操作正是T_new exp(δξ^∧) * T_old。你的任务是正确实现误差边Edge中的computeError()和linearizeOplus()函数。在linearizeOplus()中你需要计算的就是我们第4节推导的雅可比矩阵——误差关于李代数扰动δξ的导数。// 伪代码展示在g2o边中计算雅可比的思想 void linearizeOplus() override { // 取出顶点位姿顶点和地图点顶点 VertexSE3Expmap* poseVertex static_castVertexSE3Expmap*(_vertices[0]); VertexPointXYZ* pointVertex static_castVertexPointXYZ*(_vertices[1]); // 取出当前估计值 Sophus::SE3d T poseVertex-estimate(); Eigen::Vector3d P_w pointVertex-estimate(); // 将点变换到相机坐标系 Eigen::Vector3d P_c T * P_w; // 齐次变换等效于 R*P_w t double X P_c[0], Y P_c[1], Z P_c[2]; double Z2 Z * Z; // 计算投影雅可比 ∂e/∂P_c (2x3) Eigen::Matrixdouble, 2, 3 de_dPc; de_dPc(0,0) fx / Z; de_dPc(0,1) 0; de_dPc(0,2) -fx * X / Z2; de_dPc(1,0) 0; de_dPc(1,1) fy / Z; de_dPc(1,2) -fy * Y / Z2; // fx, fy为相机内参 // 计算 ∂P_c/∂δξ (3x6) 这就是核心的李代数求导 Eigen::Matrixdouble, 3, 6 dPc_dxi; dPc_dxi.block3,3(0,0) Eigen::Matrix3d::Identity(); // 关于平移部分δρ的导数 dPc_dxi.block3,3(0,3) -Sophus::SO3d::hat(P_c); // 关于旋转部分δφ的导数注意是-P_c的反对称矩阵 // 链式法则得到最终的雅可比 ∂e/∂δξ (2x6) _jacobianOplusXi de_dPc * dPc_dxi; // 关于位姿的雅可比 // 关于地图点的雅可比 _jacobianOplusXj 计算方式类似是 ∂e/∂P_w ∂e/∂P_c * ∂P_c/∂P_w ∂e/∂P_c * R }这段伪代码清晰地展示了理论如何转化为实践。dPc_dxi矩阵的构造就是第4.3节结论的直接代码体现。重要提醒不同的优化库和不同的李代数参数化顺序可能会导致雅可比矩阵的列顺序不同。例如Sophus默认的se(3)顺序是[ρ, φ]^T平移在前因此我们构造的dPc_dxi矩阵前3列是平移导数后3列是旋转导数。如果你的库顺序是[φ, ρ]^T那么这两块需要对调。务必查阅你所使用的库的文档并与你推导的公式进行比对和单元测试这是保证优化收敛不报错的关键一步。我个人的习惯是编写一个简单的测试函数用数值微分给李代数一个微小扰动计算误差变化来验证我解析求导代码的正确性这是一个非常有效的Debug手段。