1. 刚体运动的本质：三维空间里“怎么动”这件事学习《视觉SLAM十四讲》第三章之前，我一直以为旋转不过是一个“角度”的问题——绕X轴转30度，绕Y轴转60度，听起来简单得很。直到用相机把真实世界的一帧一帧图像关联起来，我才意识到：旋转和位移一旦叠加在一起，数学表达就变得极其微妙。同一块石头，相机近一点、远一点、歪一点、正一点，像素坐标完全不一样，但石头还是那块石头。SLAM要解决的第一个灵魂拷问就是：怎么用一个严谨的数学结构描述相机的运动状态，让机器能算、能优化、能更新。ch3这章的名字叫“三维空间刚体运动”，它几乎是整个SLAM几何体系的基石。后面所有内容——对极几何、PnP、BA优化、回环检测——底层都在反复使用这一章建立的坐标系变换关系。我甚至觉得，如果这章的概念没有彻底嚼碎，后面读每章都会有一种“似乎懂了但又算不出来”的悬空感。这一章适合三类人：一是刚开始接触SLAM、想把坐标变换底子打牢的学生；二是做机器人或自动驾驶相关开发、需要用代码实现位姿解算的工程师；三是在读论文时被各类旋转表达方式绕晕、想系统梳理一遍的进阶读者。我会结合自己对这章的研读和复现过程，把概念、公式、代码和踩过的坑一次讲透。1.1 从“点的坐标”到“坐标系之间的变换”我们生活在一个三维世界，姑且认为它是欧氏空间。一个空间点，在某个坐标系下有坐标 p；相机运动后，同一个物理点在新坐标系下坐标变成了 p'。那么 p 和 p' 之间一定存在一个关系，这个关系只取决于相机坐标系发生的变化，和点的具体位置无关。直观类比一下：你在教室第一排看黑板，然后走到最后一排看同一块黑板。字的物理位置没变，但你的视角变了，你眼中字的“位置”就变了。这里，你的走动就是“坐标系变换”，字的物理位置是“世界坐标”，你眼中看到的坐标就是“相机坐标”。SLAM里最核心的几何问题，就是在求解这个过程的反问题：只知道两帧图片上同一个特征点的像素位置，反推出相机在这两帧之间到底怎么动的。第三章开篇就摆出了两个最基本的概念——刚体运动与坐标变换。刚体运动意味着物体内部任意两点之间的距离在运动中保持不变，所以相机运动不会让现实世界变成哈哈镜效果。这样一个运动可以分解为旋转加平移，旋转描述“朝向”的改变，平移描述“位置”的改变。两者合在一起，构成一个特殊的变换，叫欧氏变换。1.2 为什么要搞出这么多种旋转表达方式这章最大的特点，就是一口气讲了旋转矩阵、旋转向量、欧拉角、四元数四种表达方式，外加李群 SO(3) 和李代数 so(3)。很多人第一次读到这里会问：一个旋转而已，干嘛要搞四种表达？直接用一种不就行了？这个问题我自己也反复想过。答案其实很现实：每种表达方式都有自己的“统治区”。旋转矩阵：最直观，适合做坐标变换的计算，但它有9个分量，只有3个自由度，冗余严重，而且迭代优化时很难保证始终是合法的旋转矩阵。旋转向量：只有3个参数，紧凑，但用它做连续插值很别扭，方向变化大时容易出问题。欧拉角：人机交互友好，飞行员、游戏引擎里到处用，但它有万向锁问题，而且不同旋转顺序会得到不同结果，工程上极为坑人。四元数：紧凑、无奇异性、适合插值和优化，是SLAM里使用最广泛的旋转表示，可惜概念抽象，初次接触时“不适感”很强。打个比方，这就好比描述一个人的身高体重。用“一米八、七十公斤”这种数字很直接，但如果要计算身体质量指数，你得先套公式；如果要做健身记录，你可能想用“身高体重+体脂率”的组合。描述同一个事实，不同场景用不同指标，没有哪个指标在所有场景里都是最优的。2. 旋转矩阵与变换矩阵：SLAM的“乘法世界观”先看旋转矩阵这条线。一个三维旋转可以用一个 3×3 的矩阵 R 表示，它满足正交性（$R^T R = I$）和行列式为1的条件。所有这样的矩阵构成一个群，叫特殊正交群 SO(3)。这个“群”的概念第一次出现时我有点懵，后来才明白，它本质上是说：旋转和旋转复合之后还是旋转，而且存在逆变换，这正好描述了一个连续运动状态的集合。2.1 旋转矩阵为什么是正交的从物理直觉来理解正交性很简单：旋转不会改变向量的长度和夹角。两个向量旋转前做内积得到一个数，旋转后做内积应该得到同样的数。用数学语言写，就是 $(R a)^T (R b) = a^T b$，整理一下得到 $a^T (R^T R - I) b = 0$ 对任意 a、b 成立，所以 $R^T R = I$。这就是旋转矩阵正交性的来源。行列式为1则是为了保证旋转不引入镜像。想象一下照镜子：镜像是把左右互换的一种变换，它也满足 $R^T R = I$，但行列式是 -1。刚体运动不可能把世界变成镜像世界，所以要消除这一半可能性。SO(3) 这个词里的“特殊”，指的就是行列式为1这个约束。2.2 变换矩阵 T 与齐次坐标光有旋转不够，还有平移。从一个坐标系到另一个坐标系，完整变换是 $p' = R p + t$。但这个写法在多次变换时很啰嗦：$p'' = R_2 (R_1 p + t_1) + t_2$，每复合一次，旋转和平移就纠缠一次，越写越乱。所以第三章引出了齐次坐标和变换矩阵 T。把三维点补一维变成 $(p, 1)^T$，旋转和平移就统一写进一个 4×4 的矩阵：$$ T = \begin{bmatrix} R & t \ 0^T & 1 \end{bmatrix} $$这样一来，多次变换变成矩阵连乘：$p'' = T_2 T_1 p$。这种统一性的价值在SLAM后端优化里会体现得淋漓尽致——你维护一个相机位姿，本质上就是在维护一个变换矩阵 T 的估计值。不过实操时我给自己立了一个规矩：只用变换矩阵做坐标变换，不用它做优化变量。原因很简单，T 里包含旋转矩阵 R，而 R 必须满足正交约束。如果你直接对 T 的9个旋转分量做梯度下降，几步迭代下来 R 就不再是旋转矩阵了。这也是后面为什么要引入李代数的一个重要动机。2.3 坐标变换的三个关键名词：世界系、相机系、归一化系读这章的时候，有几个坐标系概念必须分清楚：世界坐标系：固定在地面上的参考系，SLAM里所有地图点都用它描述。相机坐标系：以相机光心为原点、光轴为 Z 轴的坐标系。归一化平面坐标系：把相机坐标投影到 Z=1 平面上得到的虚拟坐标，是连接3D点和2D像素之间的桥梁。这三个坐标系之间的关系，就是 SLAM 前端的日常。一个三维世界点 P_w，先经过变换矩阵 T 到相机坐标 P_c，再通过内参矩阵 K 和归一化处理得到像素坐标。很多初学者在写代码时把内参和外参混在一起，后面第九章标定的时候就会很难受。我自己的经验是：每次写坐标变换的代码，先明确当前变量到底属于哪个坐标系，再动手写矩阵乘法。3. 旋转向量、欧拉角与四元数：绕不开的“表达之争”旋转矩阵作为唯一表达的问题，来自一个基本事实:三维旋转只有3个自由度，而旋转矩阵有9个数字，冗余的6个数字被约束关系锁死。如果你想用一个3维向量直接表示旋转，最自然的想法就是旋转向量：方向代表旋转轴，模长代表旋转角度。这个想法本身很优雅，但从旋转向量到旋转矩阵的转换公式（罗德里格斯公式）第一次看到时还是觉得有点神奇。公式长这样：$$ R = \cos\theta I + (1 - \cos\theta) n n^T + \sin\theta n^\wedge $$其中 $n^\wedge$ 是 n 的反对称矩阵。我一开始对这个公式完全无感，直到自己动手推导了一遍，才发现它其实就是把向量分解成沿轴分量和垂直轴分量，分别做旋转再合起来。推导一遍之后，这个公式就变成了肌肉记忆。3.1 欧拉角：直观的代价是万向锁欧拉角的核心思想是：把一次旋转分解成绕三个坐标轴的依次旋转。比如按 Z-Y-X 顺序旋转，就得到所谓的“偏航-俯仰-横滚”角（yaw-pitch-roll），在无人机和车辆控制领域这个表达非常直觉化。但欧拉角有两个硬伤。一是旋转顺序依赖：按 Z-Y-X 转和按 X-Y-Z 转得到的结果不同，不同库的默认也不一样，极容易搞混。二是万向锁：当中间的旋转角接近 ±90° 时，第一次和第三次旋转的轴会重合，系统丢失一个自由度，出现奇异性。我在某次可视化调试中就遇到过类似的情况：用欧拉角控制一个模拟相机的姿态，当俯仰角接近90度时，画面开始“翻车”，偏航和横滚不再独立作用。从那以后我就很坚定：欧拉角只适合给人看，不适合做计算和存储。3.2 四元数：把旋转藏进四维复数四元数可以理解成复数的推广，由 $q = (s, x, y, z)$ 或 $q = s + x i + y j + z k$ 定义。单位四元数可以用来表示旋转，满足 $s^2 + x^2 + y^2 + z^2 = 1$。我第一次接触四元数时最大的困惑是：为什么一个四维的单位向量就能表示三维的旋转？后来想通了：单位四元数本身落在四维空间里的一个三维球面上（S^3），正好和三维旋转的自由度匹配。这个球面没有欧拉角那样的奇点，也没有旋转矩阵那样的冗余，所以成为SLAM位姿表示的事实标准。四元数旋转一个向量的公式是 $p' = q p q^{-1}$。其中 p 是纯四元数 $(0, v)$，$q^{-1}$ 对单位四元数就是共轭。这里有个细节值得多说一句：四元数乘法不满足交换律，$q p q^{-1}$ 的顺序不能随意换，否则旋转效果就变了。3.3 三种表达方式的互转，实战中最常用的计算在实际工程里，你往往拿到的是某一种表达，却需要换算成另一种。下面是我用代码验证过的一组互转关系，强烈建议自己也在代码里跑一遍：四元数转旋转矩阵：直接套公式，矩阵元素是四元数分量的二次组合。旋转矩阵转四元数：先算迹，根据迹的大小选择不同的计算分支，避免数值不稳定。旋转向量转四元数：$\theta$ 和 $n$ 直接映射到 $(\cos\frac{\theta}{2}, n \sin\frac{\theta}{2})$。特别提醒一点：如果旋转矩阵带有微小的误差（比如来自数值积分），直接转四元数前最好先做正交化处理，否则得到的四元数不在单位球面上，后续运算会累积漂移。4. 李群与李代数：SLAM里的“连续旋转微积分”这一节是第三章的分水岭。前面讲的都是“怎么表示一个旋转状态”，李群李代数则要回答另一个问题：“当你对旋转求导时，该怎么办？”这个需求在SLAM里极其现实。后端优化要求目标函数对位姿求导数，而求导的对象是旋转矩阵这种带约束的量。你在正交矩阵这个流形上做梯度下降，一步迈出去就离开了流形。李代数给出的方案是：把旋转映射成3维向量，在这个无约束的向量空间里做优化，再通过指数映射回到旋转矩阵。4.1 SO(3) 与 so(3)：指数映射和对数映射so(3) 是三维向量 $\phi$ 的集合，每个向量通过反对称矩阵构造出一个矩阵，再指数映射就得到 SO(3)。这个指数映射和常规矩阵指数的关系是：$$ \exp(\phi^\wedge) = R $$实际计算时不需要真的求矩阵指数级数，直接用罗德里格斯公式就能算。反过来，旋转矩阵通过对数映射得到三维向量，本质上就是求旋转轴和旋转角。所以你可以把 SO(3) 和 so(3) 的关系想象成“曲面和平面”的关系：平面上的每一点，通过一个映射贴在曲面上，曲面上的每个点附近，也有一块平面局部坐标系。4.2 BCH公式与扰动模型既然要优化，就必须面对一个问题：当李代数增加一个小量 $\delta \phi$ 时，旋转矩阵变化多少？这里不能直接写成 $R \exp(\delta \phi^\wedge)$ 这样天真的形式，因为矩阵乘法不满足交换律。真正的答案是 BCH 公式，它给出两个矩阵指数的乘积在取对数时的近似。对SLAM来说，通常只需要一阶近似。一阶近似有两种常见形式：左乘模型和右乘模型。左乘模型的意思是，当我们在李代数上加上一个小扰动 $\delta \phi$，在 SO(3) 上相当于左乘一个 $\exp((J_l \delta \phi)^\wedge)$。这里的 $J_l$ 就是左雅可比。很多教材默认用左乘模型，但代码实现里有时用右乘模型更方便，比如更新相机位姿时我习惯用右乘扰动来避免对世界坐标系的额外变换。4.3 SE(3) 与 se(3):位姿的完整表达旋转之外再连上平移，就进入了 SE(3)。SE(3) 的元素是4×4变换矩阵，se(3) 的元素是6维向量：前3维是平移相关，后3维是旋转相关。但这个分组在大扰动时并不严格，实际优化时要注意区分。在优化求解过程中，我常用的思路是：用一个6维向量表示位姿增量。在每次迭代里，把增量通过指数映射变成变换矩阵，左乘或右乘到当前位姿上。因为增量是6维无约束变量，梯度下降、高斯牛顿这些经典算法都能直接套用。这个方法比直接优化变换矩阵的12个参数要稳得多，也是G2O、Ceres等优化库内部的核心思路。学到这里时我终于理解了为什么那么多人说“SLAM的数学核心在旋转”——因为所有优化迭代都绕不开这一步。5. Eigen与Sophus实践：把公式变成可运行的代码概念说得再多，不如跑一遍代码。第三章的实践内容主要围绕 Eigen 库展开，Sophus 库用于李群李代数部分。我自己复现时用的是 C++17 + Eigen 3.4 + Sophus 1.22（模板版本），这里把核心代码和一些容易踩坑的点记录下来。5.1 旋转矩阵与四元数的初始化陷阱Eigen 里四元数的构造函数参数顺序是 (w, x, y, z)，但注意：Quaterniond内部存储顺序却是 (x, y, z, w)。这在直接给数组赋初值时极易出错。另一个常见坑是：旋转矩阵转四元数时，如果矩阵不是严格的单位正交阵，得到的结果可能不在单位球面上，需要做 normalize。#include <Eigen/Core> #include <Eigen/Geometry> Eigen::Matrix3d R; R << 0.866, -0.5, 0, 0.5, 0.866, 0, 0, 0, 1; Eigen::Quaterniond q(R); q.normalize(); std::cout << "Quaternion: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;这个例子演示了绕 Z 轴旋转30度的旋转矩阵。注意q(R)这一步，Eigen 内部会做转换，但不会自动检查矩阵的合法性。如果你传入的矩阵带了微小数值错误，建议先做一次 SVD 分解来投影到 SO(3)：Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d R_ortho = svd.matrixU() * svd.matrixV().transpose();5.2 用 AngleAxis 处理旋转向量Eigen 的AngleAxis是旋转向量的一个很好的封装类型。初始化时指定角度和轴：Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1)); Eigen::Matrix3d R_from_angleaxis = rotation_vector.toRotationMatrix();这里要注意角度单位是弧度，而且轴向量不要求单位化——AngleAxis 内部会自动归一化。但为了代码可读性，我建议始终传单位向量。5.3 Sophus 的基本用法：SO3 和 SE3Sophus 的模板版本用起来很直观。下面是一个 SO3 的小例子：#include <sophus/so3.hpp> #include <sophus/se3.hpp> Sophus::SO3d R1(Sophus::SO3d::exp(Eigen::Vector3d(0.1, 0.2, 0.3))); Eigen::Vector3d so3_vee = R1.log(); Eigen::Matrix3d R_matrix = R1.matrix(); // 增量更新 Eigen::Vector3d update_vec(0.01, 0, 0); Sophus::SO3d R2 = Sophus::SO3d::exp(update_vec) * R1;SE3 的用法类似，但 6 维向量的顺序需要特别注意：前3维对应平移，后3维对应旋转，这是 Sophus 的约定。如果和你的优化库约定不同，做数据对接时一定要先确认顺序，不然位姿会错得莫名其妙。5.4 代码实践中的关键心得这一部分我想把几个影响调试效率的经验集中说一下：第一，坐标变换的代码一定要写好注释。比如T_cw表示从世界坐标到相机坐标的变换，字面含义容易和T_wc混淆。我在代码里统一使用T_wc、T_cw这样的命名习惯，并在每个矩阵乘法的地方标注“此处的点处于哪个坐标系”。第二，不要直接用 float。SLAM 位姿计算对精度要求极高，float 在多次矩阵连乘后误差会迅速放大。我早期用 float 跑过一段小 Demo，轨迹漂移明显比 double 版本大一个量级。第三，四元数插值尽量用slerp而不是lerp。线性插值在旋转角较大时会产生非单位四元数，且中间帧看起来像“加速旋转”。Eigen 自带slerp，但如果你自己实现插值逻辑，请务必做单位化处理。6. 这一章最常见的“看起来懂了，一跑就废”问题许多读者反馈，第三章前半部分概念都懂，但到代码部分，不是打印出来的矩阵跟自己手算的不一致，就是四元数转来转去结果不对。我把几个高频问题整理成一张速查表，你在排查时可以对照着看。现象可能原因解决办法旋转矩阵转四元数后，再用四元数转回矩阵，结果不一致输入的旋转矩阵不严格正交先做 SVD 正交化四元数旋转向量，结果方向不对乘法顺序写错，没有用 $q p q^{-1}$检查是否是共轭乘在前欧拉角转矩阵结果和预期差很多旋转顺序不同（XYZ还是ZYX）统一确认库的默认顺序Sophus 更新位姿后轨迹跳跃se3 更新时6维向量顺序错位确认平移在前还是旋转在前优化迭代发散直接在旋转矩阵分量上做加法改用李代数增量更新6.1 四元数的“双倍覆盖”问题单位四元数 $q$ 和 $-q$ 表示同一个旋转。这个性质在数学上没有问题，但在工程里会导致很隐蔽的 bug。比如你做两个位姿的四元数插值，如果两个四元数点积小于0，说明它们在球面上走的是“长路径”，插值结果会绕远路甚至震荡。解决办法很简单：插值前检查点积符号，如果为负，取反一个四元数。我在之前做某段轨迹平滑时，连续两帧的四元数偶尔出现跳变，查了半天才发现是这个原因——两台设备对同一姿态的四元数输出可能符号不同。从那以后，我把“插值前统一符号”写成了自己的工具函数。6.2 “旋转增量是左乘还是右乘”SLAM 里经常要说“把当前位姿朝某个方向调整一点”。这个调整应该左乘还是右乘，取决于扰动是在世界坐标系还是在相机坐标系。左乘扰动相当于在世界系里沿着某个轴旋转，右乘扰动相当于在相机系里旋转。举个例子，如果你的优化变量是 $T_{wc}$，想对世界系下的位姿做增量，那应该用左乘模型；如果是在相机系下做增量，则用右乘模型。很多工程里，Landmark 的优化用到左乘，位姿之间的相对变换用到右乘。搞混了的话，程序不会报错，但精度会差一个量级，而且难以排查。6.3 我对第三章的整体评价视觉SLAM十四讲这本书前面几章读起来都相对轻松，但第三章像一道分水岭。它不教你任何具体算法，却为后面所有算法铺设了数学地基。第二章的高斯分布、第三章的刚体运动和李群李代数，两章叠加在一起，基本上构建了SLAM的最小数学宇宙。我个人在这章上花的时间比第一章到第二章加起来还多。最开始看李代数部分，每一行公式都认识，连在一起就不知道在干嘛。后来我是用一个笨办法啃下来的：不看证明，先看结论，再用代码验证每个公式，最后回头看证明。这个“先信结论、再验证、最后理解”的三步法，对后来读更难的论文依然管用。如果你正在读这一章，我建议你在四元数上多花点时间，因为后面的视觉里程计、回环检测、图优化，到处都是四元数的身影。把四元数的乘法、共轭、插值写熟练，后面的路会顺畅很多。
1. 刚体运动的本质：三维空间里“怎么动”这件事学习《视觉SLAM十四讲》第三章之前，我一直以为旋转不过是一个“角度”的问题——绕X轴转30度，绕Y轴转60度，听起来简单得很。直到用相机把真实世界的一帧一帧图像关联起来，我才意识到：旋转和位移一旦叠加在一起，数学表达就变得极其微妙。同一块石头，相机近一点、远一点、歪一点、正一点，像素坐标完全不一样，但石头还是那块石头。SLAM要解决的第一个灵魂拷问就是：怎么用一个严谨的数学结构描述相机的运动状态，让机器能算、能优化、能更新。ch3这章的名字叫“三维空间刚体运动”，它几乎是整个SLAM几何体系的基石。后面所有内容——对极几何、PnP、BA优化、回环检测——底层都在反复使用这一章建立的坐标系变换关系。我甚至觉得，如果这章的概念没有彻底嚼碎，后面读每章都会有一种“似乎懂了但又算不出来”的悬空感。这一章适合三类人：一是刚开始接触SLAM、想把坐标变换底子打牢的学生；二是做机器人或自动驾驶相关开发、需要用代码实现位姿解算的工程师；三是在读论文时被各类旋转表达方式绕晕、想系统梳理一遍的进阶读者。我会结合自己对这章的研读和复现过程，把概念、公式、代码和踩过的坑一次讲透。1.1 从“点的坐标”到“坐标系之间的变换”我们生活在一个三维世界，姑且认为它是欧氏空间。一个空间点，在某个坐标系下有坐标 p；相机运动后，同一个物理点在新坐标系下坐标变成了 p'。那么 p 和 p' 之间一定存在一个关系，这个关系只取决于相机坐标系发生的变化，和点的具体位置无关。直观类比一下：你在教室第一排看黑板，然后走到最后一排看同一块黑板。字的物理位置没变，但你的视角变了，你眼中字的“位置”就变了。这里，你的走动就是“坐标系变换”，字的物理位置是“世界坐标”，你眼中看到的坐标就是“相机坐标”。SLAM里最核心的几何问题，就是在求解这个过程的反问题：只知道两帧图片上同一个特征点的像素位置，反推出相机在这两帧之间到底怎么动的。第三章开篇就摆出了两个最基本的概念——刚体运动与坐标变换。刚体运动意味着物体内部任意两点之间的距离在运动中保持不变，所以相机运动不会让现实世界变成哈哈镜效果。这样一个运动可以分解为旋转加平移，旋转描述“朝向”的改变，平移描述“位置”的改变。两者合在一起，构成一个特殊的变换，叫欧氏变换。1.2 为什么要搞出这么多种旋转表达方式这章最大的特点，就是一口气讲了旋转矩阵、旋转向量、欧拉角、四元数四种表达方式，外加李群 SO(3) 和李代数 so(3)。很多人第一次读到这里会问：一个旋转而已，干嘛要搞四种表达？直接用一种不就行了？这个问题我自己也反复想过。答案其实很现实：每种表达方式都有自己的“统治区”。旋转矩阵：最直观，适合做坐标变换的计算，但它有9个分量，只有3个自由度，冗余严重，而且迭代优化时很难保证始终是合法的旋转矩阵。旋转向量：只有3个参数，紧凑，但用它做连续插值很别扭，方向变化大时容易出问题。欧拉角：人机交互友好，飞行员、游戏引擎里到处用，但它有万向锁问题，而且不同旋转顺序会得到不同结果，工程上极为坑人。四元数：紧凑、无奇异性、适合插值和优化，是SLAM里使用最广泛的旋转表示，可惜概念抽象，初次接触时“不适感”很强。打个比方，这就好比描述一个人的身高体重。用“一米八、七十公斤”这种数字很直接，但如果要计算身体质量指数，你得先套公式；如果要做健身记录，你可能想用“身高体重+体脂率”的组合。描述同一个事实，不同场景用不同指标，没有哪个指标在所有场景里都是最优的。2. 旋转矩阵与变换矩阵：SLAM的“乘法世界观”先看旋转矩阵这条线。一个三维旋转可以用一个 3×3 的矩阵 R 表示，它满足正交性（$R^T R = I$）和行列式为1的条件。所有这样的矩阵构成一个群，叫特殊正交群 SO(3)。这个“群”的概念第一次出现时我有点懵，后来才明白，它本质上是说：旋转和旋转复合之后还是旋转，而且存在逆变换，这正好描述了一个连续运动状态的集合。2.1 旋转矩阵为什么是正交的从物理直觉来理解正交性很简单：旋转不会改变向量的长度和夹角。两个向量旋转前做内积得到一个数，旋转后做内积应该得到同样的数。用数学语言写，就是 $(R a)^T (R b) = a^T b$，整理一下得到 $a^T (R^T R - I) b = 0$ 对任意 a、b 成立，所以 $R^T R = I$。这就是旋转矩阵正交性的来源。行列式为1则是为了保证旋转不引入镜像。想象一下照镜子：镜像是把左右互换的一种变换，它也满足 $R^T R = I$，但行列式是 -1。刚体运动不可能把世界变成镜像世界，所以要消除这一半可能性。SO(3) 这个词里的“特殊”，指的就是行列式为1这个约束。2.2 变换矩阵 T 与齐次坐标光有旋转不够，还有平移。从一个坐标系到另一个坐标系，完整变换是 $p' = R p + t$。但这个写法在多次变换时很啰嗦：$p'' = R_2 (R_1 p + t_1) + t_2$，每复合一次，旋转和平移就纠缠一次，越写越乱。所以第三章引出了齐次坐标和变换矩阵 T。把三维点补一维变成 $(p, 1)^T$，旋转和平移就统一写进一个 4×4 的矩阵：$$ T = \begin{bmatrix} R & t \ 0^T & 1 \end{bmatrix} $$这样一来，多次变换变成矩阵连乘：$p'' = T_2 T_1 p$。这种统一性的价值在SLAM后端优化里会体现得淋漓尽致——你维护一个相机位姿，本质上就是在维护一个变换矩阵 T 的估计值。不过实操时我给自己立了一个规矩：只用变换矩阵做坐标变换，不用它做优化变量。原因很简单，T 里包含旋转矩阵 R，而 R 必须满足正交约束。如果你直接对 T 的9个旋转分量做梯度下降，几步迭代下来 R 就不再是旋转矩阵了。这也是后面为什么要引入李代数的一个重要动机。2.3 坐标变换的三个关键名词：世界系、相机系、归一化系读这章的时候，有几个坐标系概念必须分清楚：世界坐标系：固定在地面上的参考系，SLAM里所有地图点都用它描述。相机坐标系：以相机光心为原点、光轴为 Z 轴的坐标系。归一化平面坐标系：把相机坐标投影到 Z=1 平面上得到的虚拟坐标，是连接3D点和2D像素之间的桥梁。这三个坐标系之间的关系，就是 SLAM 前端的日常。一个三维世界点 P_w，先经过变换矩阵 T 到相机坐标 P_c，再通过内参矩阵 K 和归一化处理得到像素坐标。很多初学者在写代码时把内参和外参混在一起，后面第九章标定的时候就会很难受。我自己的经验是：每次写坐标变换的代码，先明确当前变量到底属于哪个坐标系，再动手写矩阵乘法。3. 旋转向量、欧拉角与四元数：绕不开的“表达之争”旋转矩阵作为唯一表达的问题，来自一个基本事实:三维旋转只有3个自由度，而旋转矩阵有9个数字，冗余的6个数字被约束关系锁死。如果你想用一个3维向量直接表示旋转，最自然的想法就是旋转向量：方向代表旋转轴，模长代表旋转角度。这个想法本身很优雅，但从旋转向量到旋转矩阵的转换公式（罗德里格斯公式）第一次看到时还是觉得有点神奇。公式长这样：$$ R = \cos\theta I + (1 - \cos\theta) n n^T + \sin\theta n^\wedge $$其中 $n^\wedge$ 是 n 的反对称矩阵。我一开始对这个公式完全无感，直到自己动手推导了一遍，才发现它其实就是把向量分解成沿轴分量和垂直轴分量，分别做旋转再合起来。推导一遍之后，这个公式就变成了肌肉记忆。3.1 欧拉角：直观的代价是万向锁欧拉角的核心思想是：把一次旋转分解成绕三个坐标轴的依次旋转。比如按 Z-Y-X 顺序旋转，就得到所谓的“偏航-俯仰-横滚”角（yaw-pitch-roll），在无人机和车辆控制领域这个表达非常直觉化。但欧拉角有两个硬伤。一是旋转顺序依赖：按 Z-Y-X 转和按 X-Y-Z 转得到的结果不同，不同库的默认也不一样，极容易搞混。二是万向锁：当中间的旋转角接近 ±90° 时，第一次和第三次旋转的轴会重合，系统丢失一个自由度，出现奇异性。我在某次可视化调试中就遇到过类似的情况：用欧拉角控制一个模拟相机的姿态，当俯仰角接近90度时，画面开始“翻车”，偏航和横滚不再独立作用。从那以后我就很坚定：欧拉角只适合给人看，不适合做计算和存储。3.2 四元数：把旋转藏进四维复数四元数可以理解成复数的推广，由 $q = (s, x, y, z)$ 或 $q = s + x i + y j + z k$ 定义。单位四元数可以用来表示旋转，满足 $s^2 + x^2 + y^2 + z^2 = 1$。我第一次接触四元数时最大的困惑是：为什么一个四维的单位向量就能表示三维的旋转？后来想通了：单位四元数本身落在四维空间里的一个三维球面上（S^3），正好和三维旋转的自由度匹配。这个球面没有欧拉角那样的奇点，也没有旋转矩阵那样的冗余，所以成为SLAM位姿表示的事实标准。四元数旋转一个向量的公式是 $p' = q p q^{-1}$。其中 p 是纯四元数 $(0, v)$，$q^{-1}$ 对单位四元数就是共轭。这里有个细节值得多说一句：四元数乘法不满足交换律，$q p q^{-1}$ 的顺序不能随意换，否则旋转效果就变了。3.3 三种表达方式的互转，实战中最常用的计算在实际工程里，你往往拿到的是某一种表达，却需要换算成另一种。下面是我用代码验证过的一组互转关系，强烈建议自己也在代码里跑一遍：四元数转旋转矩阵：直接套公式，矩阵元素是四元数分量的二次组合。旋转矩阵转四元数：先算迹，根据迹的大小选择不同的计算分支，避免数值不稳定。旋转向量转四元数：$\theta$ 和 $n$ 直接映射到 $(\cos\frac{\theta}{2}, n \sin\frac{\theta}{2})$。特别提醒一点：如果旋转矩阵带有微小的误差（比如来自数值积分），直接转四元数前最好先做正交化处理，否则得到的四元数不在单位球面上，后续运算会累积漂移。4. 李群与李代数：SLAM里的“连续旋转微积分”这一节是第三章的分水岭。前面讲的都是“怎么表示一个旋转状态”，李群李代数则要回答另一个问题：“当你对旋转求导时，该怎么办？”这个需求在SLAM里极其现实。后端优化要求目标函数对位姿求导数，而求导的对象是旋转矩阵这种带约束的量。你在正交矩阵这个流形上做梯度下降，一步迈出去就离开了流形。李代数给出的方案是：把旋转映射成3维向量，在这个无约束的向量空间里做优化，再通过指数映射回到旋转矩阵。4.1 SO(3) 与 so(3)：指数映射和对数映射so(3) 是三维向量 $\phi$ 的集合，每个向量通过反对称矩阵构造出一个矩阵，再指数映射就得到 SO(3)。这个指数映射和常规矩阵指数的关系是：$$ \exp(\phi^\wedge) = R $$实际计算时不需要真的求矩阵指数级数，直接用罗德里格斯公式就能算。反过来，旋转矩阵通过对数映射得到三维向量，本质上就是求旋转轴和旋转角。所以你可以把 SO(3) 和 so(3) 的关系想象成“曲面和平面”的关系：平面上的每一点，通过一个映射贴在曲面上，曲面上的每个点附近，也有一块平面局部坐标系。4.2 BCH公式与扰动模型既然要优化，就必须面对一个问题：当李代数增加一个小量 $\delta \phi$ 时，旋转矩阵变化多少？这里不能直接写成 $R \exp(\delta \phi^\wedge)$ 这样天真的形式，因为矩阵乘法不满足交换律。真正的答案是 BCH 公式，它给出两个矩阵指数的乘积在取对数时的近似。对SLAM来说，通常只需要一阶近似。一阶近似有两种常见形式：左乘模型和右乘模型。左乘模型的意思是，当我们在李代数上加上一个小扰动 $\delta \phi$，在 SO(3) 上相当于左乘一个 $\exp((J_l \delta \phi)^\wedge)$。这里的 $J_l$ 就是左雅可比。很多教材默认用左乘模型，但代码实现里有时用右乘模型更方便，比如更新相机位姿时我习惯用右乘扰动来避免对世界坐标系的额外变换。4.3 SE(3) 与 se(3):位姿的完整表达旋转之外再连上平移，就进入了 SE(3)。SE(3) 的元素是4×4变换矩阵，se(3) 的元素是6维向量：前3维是平移相关，后3维是旋转相关。但这个分组在大扰动时并不严格，实际优化时要注意区分。在优化求解过程中，我常用的思路是：用一个6维向量表示位姿增量。在每次迭代里，把增量通过指数映射变成变换矩阵，左乘或右乘到当前位姿上。因为增量是6维无约束变量，梯度下降、高斯牛顿这些经典算法都能直接套用。这个方法比直接优化变换矩阵的12个参数要稳得多，也是G2O、Ceres等优化库内部的核心思路。学到这里时我终于理解了为什么那么多人说“SLAM的数学核心在旋转”——因为所有优化迭代都绕不开这一步。5. Eigen与Sophus实践：把公式变成可运行的代码概念说得再多，不如跑一遍代码。第三章的实践内容主要围绕 Eigen 库展开，Sophus 库用于李群李代数部分。我自己复现时用的是 C++17 + Eigen 3.4 + Sophus 1.22（模板版本），这里把核心代码和一些容易踩坑的点记录下来。5.1 旋转矩阵与四元数的初始化陷阱Eigen 里四元数的构造函数参数顺序是 (w, x, y, z)，但注意：Quaterniond内部存储顺序却是 (x, y, z, w)。这在直接给数组赋初值时极易出错。另一个常见坑是：旋转矩阵转四元数时，如果矩阵不是严格的单位正交阵，得到的结果可能不在单位球面上，需要做 normalize。#include <Eigen/Core> #include <Eigen/Geometry> Eigen::Matrix3d R; R << 0.866, -0.5, 0, 0.5, 0.866, 0, 0, 0, 1; Eigen::Quaterniond q(R); q.normalize(); std::cout << "Quaternion: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;这个例子演示了绕 Z 轴旋转30度的旋转矩阵。注意q(R)这一步，Eigen 内部会做转换，但不会自动检查矩阵的合法性。如果你传入的矩阵带了微小数值错误，建议先做一次 SVD 分解来投影到 SO(3)：Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d R_ortho = svd.matrixU() * svd.matrixV().transpose();5.2 用 AngleAxis 处理旋转向量Eigen 的AngleAxis是旋转向量的一个很好的封装类型。初始化时指定角度和轴：Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1)); Eigen::Matrix3d R_from_angleaxis = rotation_vector.toRotationMatrix();这里要注意角度单位是弧度，而且轴向量不要求单位化——AngleAxis 内部会自动归一化。但为了代码可读性，我建议始终传单位向量。5.3 Sophus 的基本用法：SO3 和 SE3Sophus 的模板版本用起来很直观。下面是一个 SO3 的小例子：#include <sophus/so3.hpp> #include <sophus/se3.hpp> Sophus::SO3d R1(Sophus::SO3d::exp(Eigen::Vector3d(0.1, 0.2, 0.3))); Eigen::Vector3d so3_vee = R1.log(); Eigen::Matrix3d R_matrix = R1.matrix(); // 增量更新 Eigen::Vector3d update_vec(0.01, 0, 0); Sophus::SO3d R2 = Sophus::SO3d::exp(update_vec) * R1;SE3 的用法类似，但 6 维向量的顺序需要特别注意：前3维对应平移，后3维对应旋转，这是 Sophus 的约定。如果和你的优化库约定不同，做数据对接时一定要先确认顺序，不然位姿会错得莫名其妙。5.4 代码实践中的关键心得这一部分我想把几个影响调试效率的经验集中说一下：第一，坐标变换的代码一定要写好注释。比如T_cw表示从世界坐标到相机坐标的变换，字面含义容易和T_wc混淆。我在代码里统一使用T_wc、T_cw这样的命名习惯，并在每个矩阵乘法的地方标注“此处的点处于哪个坐标系”。第二，不要直接用 float。SLAM 位姿计算对精度要求极高，float 在多次矩阵连乘后误差会迅速放大。我早期用 float 跑过一段小 Demo，轨迹漂移明显比 double 版本大一个量级。第三，四元数插值尽量用slerp而不是lerp。线性插值在旋转角较大时会产生非单位四元数，且中间帧看起来像“加速旋转”。Eigen 自带slerp，但如果你自己实现插值逻辑，请务必做单位化处理。6. 这一章最常见的“看起来懂了，一跑就废”问题许多读者反馈，第三章前半部分概念都懂，但到代码部分，不是打印出来的矩阵跟自己手算的不一致，就是四元数转来转去结果不对。我把几个高频问题整理成一张速查表，你在排查时可以对照着看。现象可能原因解决办法旋转矩阵转四元数后，再用四元数转回矩阵，结果不一致输入的旋转矩阵不严格正交先做 SVD 正交化四元数旋转向量，结果方向不对乘法顺序写错，没有用 $q p q^{-1}$检查是否是共轭乘在前欧拉角转矩阵结果和预期差很多旋转顺序不同（XYZ还是ZYX）统一确认库的默认顺序Sophus 更新位姿后轨迹跳跃se3 更新时6维向量顺序错位确认平移在前还是旋转在前优化迭代发散直接在旋转矩阵分量上做加法改用李代数增量更新6.1 四元数的“双倍覆盖”问题单位四元数 $q$ 和 $-q$ 表示同一个旋转。这个性质在数学上没有问题，但在工程里会导致很隐蔽的 bug。比如你做两个位姿的四元数插值，如果两个四元数点积小于0，说明它们在球面上走的是“长路径”，插值结果会绕远路甚至震荡。解决办法很简单：插值前检查点积符号，如果为负，取反一个四元数。我在之前做某段轨迹平滑时，连续两帧的四元数偶尔出现跳变，查了半天才发现是这个原因——两台设备对同一姿态的四元数输出可能符号不同。从那以后，我把“插值前统一符号”写成了自己的工具函数。6.2 “旋转增量是左乘还是右乘”SLAM 里经常要说“把当前位姿朝某个方向调整一点”。这个调整应该左乘还是右乘，取决于扰动是在世界坐标系还是在相机坐标系。左乘扰动相当于在世界系里沿着某个轴旋转，右乘扰动相当于在相机系里旋转。举个例子，如果你的优化变量是 $T_{wc}$，想对世界系下的位姿做增量，那应该用左乘模型；如果是在相机系下做增量，则用右乘模型。很多工程里，Landmark 的优化用到左乘，位姿之间的相对变换用到右乘。搞混了的话，程序不会报错，但精度会差一个量级，而且难以排查。6.3 我对第三章的整体评价视觉SLAM十四讲这本书前面几章读起来都相对轻松，但第三章像一道分水岭。它不教你任何具体算法，却为后面所有算法铺设了数学地基。第二章的高斯分布、第三章的刚体运动和李群李代数，两章叠加在一起，基本上构建了SLAM的最小数学宇宙。我个人在这章上花的时间比第一章到第二章加起来还多。最开始看李代数部分，每一行公式都认识，连在一起就不知道在干嘛。后来我是用一个笨办法啃下来的：不看证明，先看结论，再用代码验证每个公式，最后回头看证明。这个“先信结论、再验证、最后理解”的三步法，对后来读更难的论文依然管用。如果你正在读这一章，我建议你在四元数上多花点时间，因为后面的视觉里程计、回环检测、图优化，到处都是四元数的身影。把四元数的乘法、共轭、插值写熟练，后面的路会顺畅很多。