李群李代数是视觉SLAM里几乎每个人都要翻一次的高墙。很多同学学到第三讲就开始劝退我当初也一样盯着SO(3)、se(3)这些符号看了两天完全想不明白为什么要这么折腾。但说实话这一讲是整个SLAM后端优化的地基搞不懂它之后看BA、看位姿图优化都是云里雾里。这篇笔记我按自己的理解重新梳理了一遍核心概念附带了课后典型习题的详细求解思路最后补充了一些动手实操时容易踩的坑。1. 李群与李代数到底在解决什么问题1.1 旋转矩阵的加法不封闭困局先问一个最基础的问题为什么要引入李群李代数答案要从旋转矩阵的运算性质说起。我们知道旋转矩阵构成的特殊正交群SO(3)满足两个关键性质正交性$R^T R I$和行列式为1$\det(R) 1$。这意味着两个旋转矩阵相乘的结果仍然是旋转矩阵群结构对乘法是封闭的。但问题来了——两个旋转矩阵相加结果还是旋转矩阵吗答案显然是否定的。这就是SLAM中最麻烦的一件事。我们在优化位姿时本质上是在调整旋转矩阵的数值让重投影误差最小化。最自然的做法是像优化普通向量一样给当前的$R$加上一个增量$\Delta R$然后继续迭代。但$R \Delta R$根本不在SO(3)上它的正交性和行列式约束全被破坏了。你强行把加法的结果当作旋转矩阵使用很快就会得到一堆毫无意义的数值。所以问题的核心就变成了如何在保持旋转矩阵约束的前提下对旋转量进行正常的加法式优化。李群李代数给出的答案是——把旋转矩阵换个坐标系表示在那个坐标系里可以做加法算完再映射回来。1.2 群的定义与李群的结构要理解李群先得知道群是什么。群是一个集合加上一种运算满足封闭性、结合律、存在幺元、存在逆元这四个条件。SO(3)和SE(3)都是典型的李群因为它们除了满足群的性质之外还具备流形的结构——也就是光滑可微。这句话听起来很抽象你可以把李群理解成一个带约束的光滑曲面每个旋转矩阵都是这个曲面上的一个点曲面在局部看起来像平直的欧氏空间。关键洞察来了既然曲面在每个点附近都像平面那我们就可以在这个局部平面上研究旋转的变化规律。这个局部平面就是该点处的切空间而李代数恰恰就是这个切空间。所以李代数并不是什么玄乎的东西它就是李群在单位元处的切空间描述了李群局部无穷小的运动方式。1.3 SLAM中实际使用的李群与李代数在视觉SLAM中我们最关心四样东西$SO(3)$、$SE(3)$以及它们对应的李代数$\mathfrak{so}(3)$、$\mathfrak{se}(3)$。它们的关系可以整理成一张表名称含义维度代表元素$SO(3)$三维旋转矩阵群3$R_{3\times3}$$SE(3)$三维刚体变换矩阵群6$T_{4\times4}$$\mathfrak{so}(3)$旋转矩阵对应的李代数3三维向量$\phi$$\mathfrak{se}(3)$变换矩阵对应的李代数6六维向量$\xi$有一点要特别注意$SE(3)$是6维的但变换矩阵$T$是4×4共16个元素所以$SE(3)$实际上是一个16维空间中的6维约束流形。这也就是为什么我们不能直接对$T$做加法优化——16个元素之间存在着很强的内在约束只有6个自由度是真正独立的。2. 指数映射与对数映射李代数与李群之间的桥梁2.1 从旋转向量到旋转矩阵的指数映射李代数$\mathfrak{so}(3)$里的元素是一个三维向量$\phi$它的物理意义其实就是旋转向量方向代表旋转轴模长代表旋转角度。所以从$\mathfrak{so}(3)$到$SO(3)$的指数映射本质上是把旋转向量变成旋转矩阵的过程。这个映射的公式就是大名鼎鼎的罗德里格斯公式$$R \cos\theta \cdot I (1 - \cos\theta) \cdot aa^T \sin\theta \cdot a^{\wedge}$$其中$\theta$是旋转向量的模长$a$是单位旋转轴向量。这个公式完美体现了旋转向量的几何含义。但很多同学并不知道这个公式是怎么来的这里我用自己的理解推导一遍关键思路。从矩阵指数的定义出发$e^{\phi^{\wedge}} \sum_{n0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^n$。直接展开这个级数是很难算的。关键在于利用反对称矩阵的一个性质三维反对称矩阵$A$满足$A^3 -|A|^2 A$这里的$|A|$不是矩阵的行列式而是对应向量的模长。利用这个性质$A$的奇数次幂都能化简成$A$的倍数偶数次幂都能化简成$A^2$的倍数。把无穷级数拆成奇数项和偶数项就能凑出$\sin\theta$和$\cos\theta$的级数展开式。整个推导过程就像把复杂的指数运算降维成三角函数运算这也是罗德里格斯公式的精髓所在。2.2 为什么矩阵指数能收敛成有限项你可能好奇一个无限级数求和为什么最后收敛成一个有限项的组合关键就隐藏在反对称矩阵的幂次周期性上。三维反对称矩阵的平方乘以自身的模长平方等于立方这使得幂次呈现出3阶循环的特征。换句话说$A$的幂次只存在两种独立形式$A$和$A^2$更高次的幂能不断化简回这两种形式。这样的性质大大简化了计算。理论上$e^{A}$涉及无穷多次矩阵乘法但因为幂次的周期性无穷级数被压缩成了对$\sin$和$\cos$两个标量三角函数的求和。这就是为什么罗德里格斯公式不仅形式优美而且计算效率极高——每次矩阵指数运算只需要计算两个三角函数值和三次矩阵加法乘法。2.3 SE(3)的指数映射而不仅仅是平移叠加$\mathfrak{se}(3)$的情况比$\mathfrak{so}(3)$更复杂。$\mathfrak{se}(3)$中的元素是一个六维向量$\xi [\rho, \phi]^T$其中$\rho$是平移相关的分量$\phi$是旋转分量。它的指数映射可以分块处理$$T \exp(\xi^{\wedge}) \begin{bmatrix} R J\rho \ 0 1 \end{bmatrix}$$这里的$R$就是由$\phi$决定的旋转矩阵$J$是一个3×3的矩阵通常被称为雅可比矩阵。注意$J\rho$并不等于$\rho$本身说明李代数中的平移分量和平移向量之间差了一个$J$变换。这件事在SLAM初始化代码中特别容易出错很多人直接从$\mathfrak{se}(3)$里取出平移分量去更新位置结果发现轨迹出现奇怪的漂移往往就是忽略了$J$的作用。$J$的具体表达式为$$J \frac{\sin\theta}{\theta}I \left(1-\frac{\sin\theta}{\theta}\right)aa^T \frac{1-\cos\theta}{\theta}a^{\wedge}$$当旋转角度很小时$J$趋近于单位阵但这个近似只有在角度趋近于0时才成立。实际处理中位姿变化较大时不能随便做近似。2.4 对数映射与李代数坐标提取反过来从李群到李代数的映射叫对数映射。给定旋转矩阵$R$可以通过$\theta \arccos\left(\frac{\mathrm{tr}(R)-1}{2}\right)$求出旋转角再通过$Rn n$求出旋转轴。这里$n$是$R$特征值为1时对应的特征向量换句话说旋转轴就是$R$不变的那条方向。从$SE(3)$提取$\mathfrak{se}(3)$也是两步先通过对数映射提取旋转部分得到$\phi$然后用$t J\rho$反解出$\rho J^{-1}t$。$J$的逆有解析表达式不需要做矩阵求逆。在实际工程里如果你用的是Sophus库这些映射都封装成了库函数不需要手推但理解原理对调试帮助极大——我遇到过好几次位姿图优化发散最后查出就是因为手动提取扰动增量时搞错了平移部分的坐标变换。3. 伴随性质与BCH公式理解扰动的两种姿势3.1 李群乘法与李代数加法的对应关系引入李代数之后有一个快乐把李群的乘法对应到李代数的加法。但实际计算中这个对应并不完美存在一个重要的修正公式——BCH公式。用数学公式表达的话$\exp(A)\exp(B)$并不等于$\exp(AB)$中间还差一些高阶项。这个差异在SLAM中不能被忽略因为它直接决定了优化迭代时的增量更新方式。BCH公式的完整形式极其复杂但在SLAM中我们通常只需要两个近似形式左乘小扰动的近似和右乘小扰动的近似。例如当$\phi_1$很小时有$$\ln(\exp(\phi_1^{\wedge})\exp(\phi_2^{\wedge}))^{\vee} \approx J_l^{-1}(\phi_2)\phi_1 \phi_2$$这里的$J_l$就是前面提到的左雅可比矩阵。这个公式的含义是在旋转矩阵上左乘一个小量对应的李代数增量近似等于在当前李代数上加一个经过$J_l^{-1}$修正的量。3.2 左扰动模型与右扰动模型的选择在视觉SLAM后端的求导中最常用的操作是对$\exp(\phi^{\wedge}) p$求$\phi$的导数。直接按定义求导需要处理矩阵指数的无穷级数推导过程非常痛苦。于是有了两个替代方案左扰动模型和右扰动模型。左扰动模型的核心想法是给$R$左乘一个微小的旋转扰动$\Delta R \exp(\delta\phi^{\wedge})$然后按导数定义展开$$\frac{\partial (Rp)}{\partial \phi} \lim_{\delta\phi\rightarrow 0} \frac{\exp(\delta\phi^{\wedge})\exp(\phi^{\wedge})p - \exp(\phi^{\wedge})p}{\delta\phi} \approx -(Rp)^{\wedge}$$右扰动模型则是右乘一个微小扰动结果类似于$-p^{\wedge}R^T$形式上差了一个旋转的共轭变换。两者都是有效的只是最终展开的雅可比矩阵形式不同。我个人的经验是在视觉SLAM残差模型中优先使用左扰动模型因为它的结论形式更简洁视觉几何中通常已经约定以左乘旋转为主这样维护代码不容易混乱。3.3 扰动模型与导数模型的内在区别很多同学区分不开扰动模型和导数模型这是两个完全不同的事。导数模型是对李代数直接求导相当于把旋转矩阵看成李代数的一个函数然后对这个函数取微分结果形式复杂必须借助左雅可比修正。扰动模型则是把李群上的乘法扰动当作变量通过极限过程求导数结果形式简单干净。如果类比理解的话导数模型相当于你坐在旋转的车上观察整个世界的变化扰动模型相当于你在车外轻轻推一下车再观察变化。后者在实际工程中更常用因为它不需要计算复杂的$J_l^{-1}$项每一步迭代的开销更小。SLAM这种对实时性要求极高的场景省掉一次3×3矩阵求逆都是值得的。4. 课后典型习题思路精讲4.1 旋转矩阵与反对称矩阵的性质验证题这类题目通常要求证明$(R\phi)^{\wedge} R\phi^{\wedge}R^T$或者验证$\mathbf{a}^{\wedge}\mathbf{b} -\mathbf{b}^{\wedge}\mathbf{a}$。核心思路只有一个把$\phi^{\wedge}$还原到叉乘的定义。叉乘满足$\mathbf{a} \times \mathbf{b} -\mathbf{b} \times \mathbf{a}$利用这个反交换性质很多看似复杂的矩阵恒等式能快速化简。举个例子要证明$u^{\wedge}v -v^{\wedge}u$你只需要明白左端等于$u \times v$右端等于$-v \times u$而$v \times u -u \times v$所以两边恰好相等。这类题目主要是帮助你建立反对称矩阵就是叉乘的矩阵表示这个直觉在后续推导李代数雅可比时这个直觉会反复用到。4.2 指数映射的级数展开计算题有一类高频习题是给定具体的$\phi$向量要求计算$e^{\phi^{\wedge}}$。常规做法是打开罗德里格斯公式硬算但不推荐因为计算量大且容易出错。更好的做法是分三步走先求模长$\theta$再求单位旋转轴$\mathbf{a}$最后代入罗德里格斯公式。这里有一个容易被忽略的小细节当$\theta$非常接近0时罗德里格斯公式中的$\sin\theta/\theta$和$(1-\cos\theta)/\theta^2$会面临数值不稳定的问题。工程上的处理方式是当$\theta$小于某个阈值时直接取近似$R \approx I \phi^{\wedge}$这个近似在微小旋转下精度完全够用。4.3 加速度计或角速度积分相关的推导题第三讲的部分习题会引出旋转的时间导数关系$\dot{R} \phi^{\wedge} R$。这其实是旋转矩阵的运动学方程。解题时从$R R^T I$出发两边对时间求导得到$\dot{R}R^T R\dot{R}^T 0$这说明$\dot{R}R^T$是反对称的可以写成某个向量$\omega$的反对称矩阵形式于是得到$\dot{R} \omega^{\wedge}R$。这个$\omega$就是角速度的坐标表示。这个结论在SLAM的IMU预积分中至关重要。IMU测得的角速度正是这里的$\omega$通过对$\omega^{\wedge}$做指数积分就能从起始旋转外推出后续时刻的旋转。搞懂了这道题的推导过程再看IMU预积分公式会轻松一大截不然那些带时间积分的旋转公式就像是天书。5. Sophus库实操与常见坑记录5.1 Sophus的安装与基本用法李群李代数的手推是一回事工程落地是另一回事。视觉SLAM中最常用的李代数库是Sophus它提供了$SO(3)$、$SE(3)$等类型的完整实现同时支持数值导数验证学习阶段极为好用。安装方式通常是通过源码构建依赖fmt库。在较新的系统中可以用CMake直接集成也可以在项目CMakeLists中通过FetchContent方式拉取。具体做法不再赘述但有一条建议一定要统一fmt版本我遇到过几次编译报错最后都是因为fmt新旧版本接口不兼容导致的。5.2 代码中的典型操作示例Sophus中常用操作可以整理成以下示例#include sophus/se3.hpp #include sophus/so3.hpp // 由旋转向量构造SO3 Sophus::SO3d R Sophus::SO3d::exp(phi); // 由旋转矩阵构造SO3 Sophus::SO3d R2(matrix_3d); // 从SO3提取旋转矩阵 Eigen::Matrix3d rotation_matrix R.matrix(); // 对数映射从SO3提取李代数 Eigen::Vector3d phi_again R.log(); // 左扰动更新 Sophus::SO3d updated_R Sophus::SO3d::exp(delta_phi) * R; // SE3的构造与分解 Sophus::SE3d T(Eigen::Quaterniond(qw, qx, qy, qz), translation_vector); Eigen::Vector3d t T.translation(); Sophus::SO3d R_part T.so3();有一个细节值得注意Sophus中T.translation()返回的是平移向量$t$也就是$J\rho$之后的结果而不是$\mathfrak{se}(3)$里的$\rho$。搞混这两个量会导致调试时出现明明位姿更新的量看起来没问题但轨迹就是不对的诡异现象。5.3 三个最常踩的坑第一个坑是更新顺序。刚接触SLAM时容易写成R R * exp(dphi)但优化器要求的是世界系到相机系的位姿扰动应该作用在合适的侧。视觉几何中重投影误差对相机位姿的雅可比是按左扰动推导的所以更新时必须用左乘方式否则整个梯度方向就反了迭代过程当场发散。第二个坑是旋转矩阵的数值归一化。有些外部库返回的旋转矩阵由于数值误差可能不再是严格正交的直接喂给Sophus的构造函数可能导致断言失败。我在调试中遇到过好几次奇怪的内存错误最后定位到是SO3构造时检查行列式不为1而崩溃。正确的做法是先用Eigen::Quaterniond做一次归一化再传给Sophus。第三个坑是矩阵与向量的数据类型不匹配。Sophus与Eigen结合时对浮点类型敏感单精度类型的SE3f和双精度的Eigen::Matrix3d混用会导致编译错误。建议整个工程统一使用double类型除非是在嵌入式环境中有明确的内存限制否则单精度在做非线性优化时精度真的不够用。6. 学习路径上的建议与避坑感悟6.1 李群李代数要不要手推公式很多同学纠结要不要把每一个公式都自己推导一遍。我的看法是必考的核心公式要会推应用型的公式会用就行。罗德里格斯公式、指数映射、扰动模型这些骨架必须亲自动手因为它们决定了你对后续内容的理解。而BCH公式的完整展开、$J$矩阵的逆推导这类细枝末节只需要知道存在性和物理含义用的时候查库即可。更合理的学习节奏是先通读一遍书把概念框架建立起来然后动手做第3讲的习题巩固推导最后上手Sophus写一段包含球面插值或位姿变换的测试代码。整个过程控制在3到5天比较合适拖太久容易遗忘前面的数学基础。6.2 学习中的常见误区第一个误区是死抠李代数完整定义和泛函分析背景。李群李代数在数学上是一个非常深刻的分支但SLAM领域用到的只是其中很小一部分。在两个小扰动近似的层面理解它远比研究群论本身更有利于你写出能跑的SLAM系统。不用因为数学背景不够而产生焦虑。第二个误区是跳过习题直接看代码。视觉SLAM十四讲最值钱的部分恰恰是那些课后推导题它们把概念和工程问题拧在了一起。直接看代码容易陷入代码能跑但是换一个场景就不会改的尴尬局面。我自己做了三遍习题每做一遍对李代数的理解都上一个台阶。第三个误区是从头手写李代数实现。学习阶段可以使用Sophus完全没有必要自己实现一个李代数库。自己写不等于理解了原理反而容易被各种边界条件和数值陷阱消耗大量时间。先会用再理解最后才是自己造轮子。6.3 这一讲与后续章节的联系接下来进入第四讲的相机模型与非线性优化时你会发现李代数无处不在。对空间点$P$投影到像素坐标系的过程求导本质上是同时对旋转量$\phi$和平移量$\rho$做扰动这就直接用到左扰动模型。第五讲的多视图几何和第七讲之后的BA优化也全部建立在位姿的李代数表示上。所以这一讲不是孤立的数学游戏而是整个SLAM系统优化框架的地基。如果你现在感觉概念还有点飘非常正常后续章节会反复使用这些概念用得多自然就熟了。遇到不懂的地方随时回来翻这一讲每次都会有新的收获。最后分享一个我在实际学习中的方法每学完一小节就在纸上用一句话总结这一小节到底在干什么比如指数映射是把旋转向量变成旋转矩阵扰动模型是换了一种求导姿势。这种概括能力会在你组合各个模块时爆发出巨大的作用。李群李代数这一关一旦跨过去后面的路就顺畅多了。 李群李代数入门：视觉SLAM位姿优化的数学基础与实操指南
李群李代数是视觉SLAM里几乎每个人都要翻一次的高墙。很多同学学到第三讲就开始劝退我当初也一样盯着SO(3)、se(3)这些符号看了两天完全想不明白为什么要这么折腾。但说实话这一讲是整个SLAM后端优化的地基搞不懂它之后看BA、看位姿图优化都是云里雾里。这篇笔记我按自己的理解重新梳理了一遍核心概念附带了课后典型习题的详细求解思路最后补充了一些动手实操时容易踩的坑。1. 李群与李代数到底在解决什么问题1.1 旋转矩阵的加法不封闭困局先问一个最基础的问题为什么要引入李群李代数答案要从旋转矩阵的运算性质说起。我们知道旋转矩阵构成的特殊正交群SO(3)满足两个关键性质正交性$R^T R I$和行列式为1$\det(R) 1$。这意味着两个旋转矩阵相乘的结果仍然是旋转矩阵群结构对乘法是封闭的。但问题来了——两个旋转矩阵相加结果还是旋转矩阵吗答案显然是否定的。这就是SLAM中最麻烦的一件事。我们在优化位姿时本质上是在调整旋转矩阵的数值让重投影误差最小化。最自然的做法是像优化普通向量一样给当前的$R$加上一个增量$\Delta R$然后继续迭代。但$R \Delta R$根本不在SO(3)上它的正交性和行列式约束全被破坏了。你强行把加法的结果当作旋转矩阵使用很快就会得到一堆毫无意义的数值。所以问题的核心就变成了如何在保持旋转矩阵约束的前提下对旋转量进行正常的加法式优化。李群李代数给出的答案是——把旋转矩阵换个坐标系表示在那个坐标系里可以做加法算完再映射回来。1.2 群的定义与李群的结构要理解李群先得知道群是什么。群是一个集合加上一种运算满足封闭性、结合律、存在幺元、存在逆元这四个条件。SO(3)和SE(3)都是典型的李群因为它们除了满足群的性质之外还具备流形的结构——也就是光滑可微。这句话听起来很抽象你可以把李群理解成一个带约束的光滑曲面每个旋转矩阵都是这个曲面上的一个点曲面在局部看起来像平直的欧氏空间。关键洞察来了既然曲面在每个点附近都像平面那我们就可以在这个局部平面上研究旋转的变化规律。这个局部平面就是该点处的切空间而李代数恰恰就是这个切空间。所以李代数并不是什么玄乎的东西它就是李群在单位元处的切空间描述了李群局部无穷小的运动方式。1.3 SLAM中实际使用的李群与李代数在视觉SLAM中我们最关心四样东西$SO(3)$、$SE(3)$以及它们对应的李代数$\mathfrak{so}(3)$、$\mathfrak{se}(3)$。它们的关系可以整理成一张表名称含义维度代表元素$SO(3)$三维旋转矩阵群3$R_{3\times3}$$SE(3)$三维刚体变换矩阵群6$T_{4\times4}$$\mathfrak{so}(3)$旋转矩阵对应的李代数3三维向量$\phi$$\mathfrak{se}(3)$变换矩阵对应的李代数6六维向量$\xi$有一点要特别注意$SE(3)$是6维的但变换矩阵$T$是4×4共16个元素所以$SE(3)$实际上是一个16维空间中的6维约束流形。这也就是为什么我们不能直接对$T$做加法优化——16个元素之间存在着很强的内在约束只有6个自由度是真正独立的。2. 指数映射与对数映射李代数与李群之间的桥梁2.1 从旋转向量到旋转矩阵的指数映射李代数$\mathfrak{so}(3)$里的元素是一个三维向量$\phi$它的物理意义其实就是旋转向量方向代表旋转轴模长代表旋转角度。所以从$\mathfrak{so}(3)$到$SO(3)$的指数映射本质上是把旋转向量变成旋转矩阵的过程。这个映射的公式就是大名鼎鼎的罗德里格斯公式$$R \cos\theta \cdot I (1 - \cos\theta) \cdot aa^T \sin\theta \cdot a^{\wedge}$$其中$\theta$是旋转向量的模长$a$是单位旋转轴向量。这个公式完美体现了旋转向量的几何含义。但很多同学并不知道这个公式是怎么来的这里我用自己的理解推导一遍关键思路。从矩阵指数的定义出发$e^{\phi^{\wedge}} \sum_{n0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^n$。直接展开这个级数是很难算的。关键在于利用反对称矩阵的一个性质三维反对称矩阵$A$满足$A^3 -|A|^2 A$这里的$|A|$不是矩阵的行列式而是对应向量的模长。利用这个性质$A$的奇数次幂都能化简成$A$的倍数偶数次幂都能化简成$A^2$的倍数。把无穷级数拆成奇数项和偶数项就能凑出$\sin\theta$和$\cos\theta$的级数展开式。整个推导过程就像把复杂的指数运算降维成三角函数运算这也是罗德里格斯公式的精髓所在。2.2 为什么矩阵指数能收敛成有限项你可能好奇一个无限级数求和为什么最后收敛成一个有限项的组合关键就隐藏在反对称矩阵的幂次周期性上。三维反对称矩阵的平方乘以自身的模长平方等于立方这使得幂次呈现出3阶循环的特征。换句话说$A$的幂次只存在两种独立形式$A$和$A^2$更高次的幂能不断化简回这两种形式。这样的性质大大简化了计算。理论上$e^{A}$涉及无穷多次矩阵乘法但因为幂次的周期性无穷级数被压缩成了对$\sin$和$\cos$两个标量三角函数的求和。这就是为什么罗德里格斯公式不仅形式优美而且计算效率极高——每次矩阵指数运算只需要计算两个三角函数值和三次矩阵加法乘法。2.3 SE(3)的指数映射而不仅仅是平移叠加$\mathfrak{se}(3)$的情况比$\mathfrak{so}(3)$更复杂。$\mathfrak{se}(3)$中的元素是一个六维向量$\xi [\rho, \phi]^T$其中$\rho$是平移相关的分量$\phi$是旋转分量。它的指数映射可以分块处理$$T \exp(\xi^{\wedge}) \begin{bmatrix} R J\rho \ 0 1 \end{bmatrix}$$这里的$R$就是由$\phi$决定的旋转矩阵$J$是一个3×3的矩阵通常被称为雅可比矩阵。注意$J\rho$并不等于$\rho$本身说明李代数中的平移分量和平移向量之间差了一个$J$变换。这件事在SLAM初始化代码中特别容易出错很多人直接从$\mathfrak{se}(3)$里取出平移分量去更新位置结果发现轨迹出现奇怪的漂移往往就是忽略了$J$的作用。$J$的具体表达式为$$J \frac{\sin\theta}{\theta}I \left(1-\frac{\sin\theta}{\theta}\right)aa^T \frac{1-\cos\theta}{\theta}a^{\wedge}$$当旋转角度很小时$J$趋近于单位阵但这个近似只有在角度趋近于0时才成立。实际处理中位姿变化较大时不能随便做近似。2.4 对数映射与李代数坐标提取反过来从李群到李代数的映射叫对数映射。给定旋转矩阵$R$可以通过$\theta \arccos\left(\frac{\mathrm{tr}(R)-1}{2}\right)$求出旋转角再通过$Rn n$求出旋转轴。这里$n$是$R$特征值为1时对应的特征向量换句话说旋转轴就是$R$不变的那条方向。从$SE(3)$提取$\mathfrak{se}(3)$也是两步先通过对数映射提取旋转部分得到$\phi$然后用$t J\rho$反解出$\rho J^{-1}t$。$J$的逆有解析表达式不需要做矩阵求逆。在实际工程里如果你用的是Sophus库这些映射都封装成了库函数不需要手推但理解原理对调试帮助极大——我遇到过好几次位姿图优化发散最后查出就是因为手动提取扰动增量时搞错了平移部分的坐标变换。3. 伴随性质与BCH公式理解扰动的两种姿势3.1 李群乘法与李代数加法的对应关系引入李代数之后有一个快乐把李群的乘法对应到李代数的加法。但实际计算中这个对应并不完美存在一个重要的修正公式——BCH公式。用数学公式表达的话$\exp(A)\exp(B)$并不等于$\exp(AB)$中间还差一些高阶项。这个差异在SLAM中不能被忽略因为它直接决定了优化迭代时的增量更新方式。BCH公式的完整形式极其复杂但在SLAM中我们通常只需要两个近似形式左乘小扰动的近似和右乘小扰动的近似。例如当$\phi_1$很小时有$$\ln(\exp(\phi_1^{\wedge})\exp(\phi_2^{\wedge}))^{\vee} \approx J_l^{-1}(\phi_2)\phi_1 \phi_2$$这里的$J_l$就是前面提到的左雅可比矩阵。这个公式的含义是在旋转矩阵上左乘一个小量对应的李代数增量近似等于在当前李代数上加一个经过$J_l^{-1}$修正的量。3.2 左扰动模型与右扰动模型的选择在视觉SLAM后端的求导中最常用的操作是对$\exp(\phi^{\wedge}) p$求$\phi$的导数。直接按定义求导需要处理矩阵指数的无穷级数推导过程非常痛苦。于是有了两个替代方案左扰动模型和右扰动模型。左扰动模型的核心想法是给$R$左乘一个微小的旋转扰动$\Delta R \exp(\delta\phi^{\wedge})$然后按导数定义展开$$\frac{\partial (Rp)}{\partial \phi} \lim_{\delta\phi\rightarrow 0} \frac{\exp(\delta\phi^{\wedge})\exp(\phi^{\wedge})p - \exp(\phi^{\wedge})p}{\delta\phi} \approx -(Rp)^{\wedge}$$右扰动模型则是右乘一个微小扰动结果类似于$-p^{\wedge}R^T$形式上差了一个旋转的共轭变换。两者都是有效的只是最终展开的雅可比矩阵形式不同。我个人的经验是在视觉SLAM残差模型中优先使用左扰动模型因为它的结论形式更简洁视觉几何中通常已经约定以左乘旋转为主这样维护代码不容易混乱。3.3 扰动模型与导数模型的内在区别很多同学区分不开扰动模型和导数模型这是两个完全不同的事。导数模型是对李代数直接求导相当于把旋转矩阵看成李代数的一个函数然后对这个函数取微分结果形式复杂必须借助左雅可比修正。扰动模型则是把李群上的乘法扰动当作变量通过极限过程求导数结果形式简单干净。如果类比理解的话导数模型相当于你坐在旋转的车上观察整个世界的变化扰动模型相当于你在车外轻轻推一下车再观察变化。后者在实际工程中更常用因为它不需要计算复杂的$J_l^{-1}$项每一步迭代的开销更小。SLAM这种对实时性要求极高的场景省掉一次3×3矩阵求逆都是值得的。4. 课后典型习题思路精讲4.1 旋转矩阵与反对称矩阵的性质验证题这类题目通常要求证明$(R\phi)^{\wedge} R\phi^{\wedge}R^T$或者验证$\mathbf{a}^{\wedge}\mathbf{b} -\mathbf{b}^{\wedge}\mathbf{a}$。核心思路只有一个把$\phi^{\wedge}$还原到叉乘的定义。叉乘满足$\mathbf{a} \times \mathbf{b} -\mathbf{b} \times \mathbf{a}$利用这个反交换性质很多看似复杂的矩阵恒等式能快速化简。举个例子要证明$u^{\wedge}v -v^{\wedge}u$你只需要明白左端等于$u \times v$右端等于$-v \times u$而$v \times u -u \times v$所以两边恰好相等。这类题目主要是帮助你建立反对称矩阵就是叉乘的矩阵表示这个直觉在后续推导李代数雅可比时这个直觉会反复用到。4.2 指数映射的级数展开计算题有一类高频习题是给定具体的$\phi$向量要求计算$e^{\phi^{\wedge}}$。常规做法是打开罗德里格斯公式硬算但不推荐因为计算量大且容易出错。更好的做法是分三步走先求模长$\theta$再求单位旋转轴$\mathbf{a}$最后代入罗德里格斯公式。这里有一个容易被忽略的小细节当$\theta$非常接近0时罗德里格斯公式中的$\sin\theta/\theta$和$(1-\cos\theta)/\theta^2$会面临数值不稳定的问题。工程上的处理方式是当$\theta$小于某个阈值时直接取近似$R \approx I \phi^{\wedge}$这个近似在微小旋转下精度完全够用。4.3 加速度计或角速度积分相关的推导题第三讲的部分习题会引出旋转的时间导数关系$\dot{R} \phi^{\wedge} R$。这其实是旋转矩阵的运动学方程。解题时从$R R^T I$出发两边对时间求导得到$\dot{R}R^T R\dot{R}^T 0$这说明$\dot{R}R^T$是反对称的可以写成某个向量$\omega$的反对称矩阵形式于是得到$\dot{R} \omega^{\wedge}R$。这个$\omega$就是角速度的坐标表示。这个结论在SLAM的IMU预积分中至关重要。IMU测得的角速度正是这里的$\omega$通过对$\omega^{\wedge}$做指数积分就能从起始旋转外推出后续时刻的旋转。搞懂了这道题的推导过程再看IMU预积分公式会轻松一大截不然那些带时间积分的旋转公式就像是天书。5. Sophus库实操与常见坑记录5.1 Sophus的安装与基本用法李群李代数的手推是一回事工程落地是另一回事。视觉SLAM中最常用的李代数库是Sophus它提供了$SO(3)$、$SE(3)$等类型的完整实现同时支持数值导数验证学习阶段极为好用。安装方式通常是通过源码构建依赖fmt库。在较新的系统中可以用CMake直接集成也可以在项目CMakeLists中通过FetchContent方式拉取。具体做法不再赘述但有一条建议一定要统一fmt版本我遇到过几次编译报错最后都是因为fmt新旧版本接口不兼容导致的。5.2 代码中的典型操作示例Sophus中常用操作可以整理成以下示例#include sophus/se3.hpp #include sophus/so3.hpp // 由旋转向量构造SO3 Sophus::SO3d R Sophus::SO3d::exp(phi); // 由旋转矩阵构造SO3 Sophus::SO3d R2(matrix_3d); // 从SO3提取旋转矩阵 Eigen::Matrix3d rotation_matrix R.matrix(); // 对数映射从SO3提取李代数 Eigen::Vector3d phi_again R.log(); // 左扰动更新 Sophus::SO3d updated_R Sophus::SO3d::exp(delta_phi) * R; // SE3的构造与分解 Sophus::SE3d T(Eigen::Quaterniond(qw, qx, qy, qz), translation_vector); Eigen::Vector3d t T.translation(); Sophus::SO3d R_part T.so3();有一个细节值得注意Sophus中T.translation()返回的是平移向量$t$也就是$J\rho$之后的结果而不是$\mathfrak{se}(3)$里的$\rho$。搞混这两个量会导致调试时出现明明位姿更新的量看起来没问题但轨迹就是不对的诡异现象。5.3 三个最常踩的坑第一个坑是更新顺序。刚接触SLAM时容易写成R R * exp(dphi)但优化器要求的是世界系到相机系的位姿扰动应该作用在合适的侧。视觉几何中重投影误差对相机位姿的雅可比是按左扰动推导的所以更新时必须用左乘方式否则整个梯度方向就反了迭代过程当场发散。第二个坑是旋转矩阵的数值归一化。有些外部库返回的旋转矩阵由于数值误差可能不再是严格正交的直接喂给Sophus的构造函数可能导致断言失败。我在调试中遇到过好几次奇怪的内存错误最后定位到是SO3构造时检查行列式不为1而崩溃。正确的做法是先用Eigen::Quaterniond做一次归一化再传给Sophus。第三个坑是矩阵与向量的数据类型不匹配。Sophus与Eigen结合时对浮点类型敏感单精度类型的SE3f和双精度的Eigen::Matrix3d混用会导致编译错误。建议整个工程统一使用double类型除非是在嵌入式环境中有明确的内存限制否则单精度在做非线性优化时精度真的不够用。6. 学习路径上的建议与避坑感悟6.1 李群李代数要不要手推公式很多同学纠结要不要把每一个公式都自己推导一遍。我的看法是必考的核心公式要会推应用型的公式会用就行。罗德里格斯公式、指数映射、扰动模型这些骨架必须亲自动手因为它们决定了你对后续内容的理解。而BCH公式的完整展开、$J$矩阵的逆推导这类细枝末节只需要知道存在性和物理含义用的时候查库即可。更合理的学习节奏是先通读一遍书把概念框架建立起来然后动手做第3讲的习题巩固推导最后上手Sophus写一段包含球面插值或位姿变换的测试代码。整个过程控制在3到5天比较合适拖太久容易遗忘前面的数学基础。6.2 学习中的常见误区第一个误区是死抠李代数完整定义和泛函分析背景。李群李代数在数学上是一个非常深刻的分支但SLAM领域用到的只是其中很小一部分。在两个小扰动近似的层面理解它远比研究群论本身更有利于你写出能跑的SLAM系统。不用因为数学背景不够而产生焦虑。第二个误区是跳过习题直接看代码。视觉SLAM十四讲最值钱的部分恰恰是那些课后推导题它们把概念和工程问题拧在了一起。直接看代码容易陷入代码能跑但是换一个场景就不会改的尴尬局面。我自己做了三遍习题每做一遍对李代数的理解都上一个台阶。第三个误区是从头手写李代数实现。学习阶段可以使用Sophus完全没有必要自己实现一个李代数库。自己写不等于理解了原理反而容易被各种边界条件和数值陷阱消耗大量时间。先会用再理解最后才是自己造轮子。6.3 这一讲与后续章节的联系接下来进入第四讲的相机模型与非线性优化时你会发现李代数无处不在。对空间点$P$投影到像素坐标系的过程求导本质上是同时对旋转量$\phi$和平移量$\rho$做扰动这就直接用到左扰动模型。第五讲的多视图几何和第七讲之后的BA优化也全部建立在位姿的李代数表示上。所以这一讲不是孤立的数学游戏而是整个SLAM系统优化框架的地基。如果你现在感觉概念还有点飘非常正常后续章节会反复使用这些概念用得多自然就熟了。遇到不懂的地方随时回来翻这一讲每次都会有新的收获。最后分享一个我在实际学习中的方法每学完一小节就在纸上用一句话总结这一小节到底在干什么比如指数映射是把旋转向量变成旋转矩阵扰动模型是换了一种求导姿势。这种概括能力会在你组合各个模块时爆发出巨大的作用。李群李代数这一关一旦跨过去后面的路就顺畅多了。