视觉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 轴的坐标系。归一化平面坐标系把相机坐标投影到 Z1 平面上得到的虚拟坐标是连接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 库用于李群李代数部分。我自己复现时用的是 C17 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::JacobiSVDEigen::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的最小数学宇宙。我个人在这章上花的时间比第一章到第二章加起来还多。最开始看李代数部分每一行公式都认识连在一起就不知道在干嘛。后来我是用一个笨办法啃下来的不看证明先看结论再用代码验证每个公式最后回头看证明。这个“先信结论、再验证、最后理解”的三步法对后来读更难的论文依然管用。如果你正在读这一章我建议你在四元数上多花点时间因为后面的视觉里程计、回环检测、图优化到处都是四元数的身影。把四元数的乘法、共轭、插值写熟练后面的路会顺畅很多。