视觉SLAM刚体运动详解：旋转矩阵、四元数与齐次坐标实战

1. 为什么要从刚体运动开始啃SLAM坦白说第一次翻开《视觉SLAM十四讲》第三讲的时候我一度以为自己在看一本数学教材而不是机器人书。满页的矩阵乘法、向量叉乘、坐标变换甚至还有四元数这种听起来就很玄的概念。但是等我真的跑起视觉里程计、开始处理相机位姿之后才意识到这一章的分量整个SLAM系统的状态变量说白了就是相机在哪里、朝哪看而这两个问题的数学表达全部落在第三讲。这一章的核心任务是把刚体运动用数学语言讲清楚。所谓刚体就是形状和大小都不变化的物体相机就是一个典型的刚体。它在空间里运动时只有两种变化位置变了平移朝向变了旋转。而SLAM要做的就是不断估计这两个量。所以第三讲的每一个公式、每一个矩阵后面都会在PnP求解、BA优化、回环检测里反复出现。我这篇笔记不会照着书把每个证明都抄一遍而是把自己当时理解得最费劲、后来发现也最关键的几条线拎出来坐标系如何描述点、旋转矩阵为什么必须是正交阵、旋转向量和欧拉角到底各有什么坑、四元数为什么是SLAM里的宠儿以及齐次坐标如何把平移旋转统一成一次矩阵乘法。每一条后面我都会补上自己的理解方式和踩过的坑希望对正在啃这一章的人有点帮助。2. 坐标系、点与向量SLAM里所有数学的地基2.1 从一个点到某个坐标系下的点在SLAM里我们谈论的不是抽象的点而是在某个坐标系下的点。最简单的例子我手边有个茶杯它在相机坐标系下的坐标是(0.5, 0.3, 1.0)意思是茶杯中心在相机前方1米、右侧0.5米、上方0.3米。但如果换到世界坐标系这个数字就完全变了因为坐标系原点不同、各轴朝向也不同。这个道理看似简单却是我最初经常搞混的地方。在代码里看到一个三维向量就默认它是世界坐标结果几何计算一团糟。后来养成一个习惯任何坐标向量都要问一句这是哪个坐标系下的。这也正是第三讲反复强调的点SLAM从一开始就要面对多个坐标系——世界坐标系、相机坐标系、像素坐标系它们之间的桥梁就是旋转矩阵和平移向量。2.2 向量的内积、外积到底用来干嘛第三讲一开始复习了向量的内积和外积。内积a·b给出一个标量衡量两个向量方向的相似程度比如计算投影长度时会用到。外积a×b的结果是一个向量方向垂直于a和b所在的平面模长等于|a||b|sinθ也就是两个向量张成的平行四边形的面积。外积在SLAM里有个非常重要的用途它被改写成矩阵形式用一个反对称矩阵去表示叉乘运算。记作a×b a^ b其中a^是一个由a的分量构造的3x3反对称矩阵$$a^{\wedge} \begin{bmatrix} 0 -a_3 a_2 \ a_3 0 -a_1 \ -a_2 a_1 0 \end{bmatrix}$$这个符号看着奇怪但后面会贯穿始终。在估计姿态误差、推导雅可比矩阵时反对称矩阵会频繁出现。所以第三讲看似在复习线性代数其实是在给后面所有推导埋线头。我当时没重视这一点等看到对极几何里本质矩阵E t^R的时候才回头补课白白绕了一段路。2.3 坐标系变换的本质同一个点换个坐标系描述刚体运动的核心可以用一句话概括同一个点在不同的坐标系里有不同的坐标变换规则由两个坐标系之间的旋转和平移决定。设某个点在世界坐标系下的坐标为pw在相机坐标系下的坐标为pc那么它们的关系是$$p_c R p_w t$$这里的R是3x3旋转矩阵t是3x1平移向量。这个公式我在第三讲翻来覆去看了很多遍才真正理解R描述的是两个坐标系之间的朝向关系t描述的是原点之间的位置偏移。需要注意的是这里t不是点的平移而是坐标系原点的平移——我当时总把t当成点移动的量导致在分析相机运动时越绕越晕。区分方法是盯住变换的对象如果模型固定、相机动了那是一个刚体变换如果相机固定、模型动了那是另一个刚体变换。两者的R和t方向正好相反。第三讲里特意比较了这两种情况我建议你也自己动手把坐标画出来推一遍这一步通了后面的变换矩阵、四元数都会顺很多。3. 旋转矩阵的性质与约束正交矩阵和行列式3.1 为什么旋转矩阵是正交矩阵旋转矩阵R有三个重要性质R是正交矩阵R^T R I、行列式为1、对R取逆就等于转置R^{-1} R^T。这意味着求逆旋转变得极其简单想从相机坐标系回到世界坐标系直接用R^T乘过去就行。但正交性不是白来的。旋转的本质是保持长度和夹角不变的线性变换。一个刚体上任意两点之间的距离在旋转之后不能变这个条件推导下来就等价于R^T R I。第三讲用旋转前后向量长度不变这个约束条件推出R是正交阵这个推导虽然只有几行却是我理解旋转矩阵的门钥匙。它让我明白不是旋转矩阵恰好是正交阵而是要表示旋转就必须满足这个代数约束。3.2 det(R)1的意义与刚体运动的空间含义正交矩阵的行列式可以是1也可以是-1而旋转矩阵要求det(R)1。这个细节很多人一笔带过但它对应一个直观的几何区别行列式为-1的正交矩阵包含反射成分比如照镜子那种左右互换。反射会改变物体的手性一个右手坐标系经过反射会变成左手坐标系这在物理世界里是不可能的。SLAM中我们处理的是真实相机在真实空间的运动不可能出现反射所以必须把det(R)-1的情况排除掉。第三讲在这里顺带提到了特殊正交群SO(3)这个名词——它就是所有满足R^T RI、det(R)1的3x3矩阵的集合。这个集合里任意两个矩阵相乘仍在该集合内而且每个矩阵都有逆所以它构成一个群。这个抽象概念初看没用但到第四讲李群李代数时SO(3)就是主角之一所以现在混个脸熟很值。3.3 旋转矩阵的自由度9个数却只有3个自由度旋转矩阵有9个元素但真正独立的参数只有3个。正交性约束给出6个独立方程R^T R I是对称阵只有6个独立分量9减6等于3。这个多余是理解后续旋转向量、四元数表达方式的钥匙。为什么SLAM里不能只用一个旋转矩阵表示姿态因为9个数字浪费存储空间不说优化的时候还要时刻保证它满足正交约束这非常麻烦。如果你把R的9个元素当成普通变量去优化很快会发现优化出来的矩阵根本不是一个合法旋转。所以后面的工作里我们通常会改用3自由度的旋转向量或四元数来参数化旋转这也是第三讲接下来要解决的痛点。4. 旋转向量与欧拉角两个直观但各有暗坑的表达方式4.1 旋转向量任何旋转都可以看成绕某一轴转某一角欧拉证明了任意旋转都可以用绕某个轴旋转某个角度来描述。把这个轴记为单位向量n角度记为θ那么旋转向量就是θn。它只有3个分量和旋转矩阵的自由度一致非常紧凑。旋转向量和旋转矩阵之间的转换由罗德里格斯公式完成$$R \cos\theta I (1-\cos\theta)nn^T \sin\theta n^{\wedge}$$反过来已知R求旋转向量时角度可以直接从矩阵迹得到$$\theta \arccos\left(\frac{\text{tr}(R)-1}{2}\right)$$这个公式我在代码里用过很多次比如手眼标定、外参标定里经常要把旋转矩阵转成轴角去查看旋转量大小。需要注意的反倒是特殊情况的处理θ接近0的时候旋转矩阵接近单位阵此时轴n的分量会变得很不稳定数值上需要设定阈值做保护。4.2 欧拉角最友好但万向锁是绕不过去的坎欧拉角把旋转分解成绕三个坐标轴的依次转动比如常见的rpy角roll-roll、pitch、yaw就是绕x、y、z轴的旋转在航空、汽车领域特别直观。第三讲拿它举例子是因为偏航-俯仰-横滚的说法对我们接受度最高。但欧拉角有一个著名缺陷万向锁。当中间轴的旋转角达到±90°时第一次和第三次旋转会绕着同一根轴系统丢失一个自由度。此时无论怎么调整角度都没有办法独立控制某些方向。更麻烦的是在万向锁附近很小的姿态变化会导致欧拉角的剧烈跳变数字上看起来像角度突变了几十度实际上姿态只动了不到一度。这个特性让欧拉角在SLAM的位姿优化和插值中几乎没法用。我们要优化的里程计、图优化动辄需要处理接近万向锁的姿态一不留神优化器就发散。所以第三讲的结论很明确欧拉角适合人看不适合机器算。4.3 两者各自的使用场景在实际工程中我把旋转向量用在前面提到的可视化、初始化估计以及需要直观给出绕哪个轴转了多少的调试场景。欧拉角则主要用在遥控指令和展示界面比如把当前相机姿态显示成pitch角度给人看。真要放进SLAM的状态向量里参与优化第四讲的李代数才是正解而这也解释了为什么第三章要花这么大篇幅铺垫这么多种旋转表达方式。5. 四元数看着吓人其实是被逼出来的最优解5.1 为什么要引入四元数紧凑又无奇异旋转向量、欧拉角都有一个致命的通病存在奇异位形也就是某些姿态下表达会退化。四元数则完全没有这个问题。它用4个参数表达旋转比旋转矩阵的9个参数紧凑又没有欧拉角的万向锁而且对插值、平滑过渡非常友好。四元数的形式是q q0 q1i q2j q3k其中q0是实部后面的虚部满足i²j²k²ijk-1这种运算规则。单位四元数模长为1才能表示旋转这是使用四元数时时刻要记住的铁律。第三讲特意强调任意旋转都可以用两个互为相反数的单位四元数表示q和-q是同一个旋转。这在做姿态平均、插值的时候如果不小心会带来正负号相关的麻烦。5.2 四元数怎么表示一个点被旋转四元数和旋转矩阵的转换公式我建议直接背下来用。设旋转对应的四元数为q q0 q1i q2j q3k则旋转矩阵R的元素可以用如下方式构造$$R \begin{bmatrix} 1-2(q_2^2q_3^2) 2(q_1q_2 - q_0q_3) 2(q_1q_3 q_0q_2) \ 2(q_1q_2 q_0q_3) 1-2(q_1^2q_3^2) 2(q_2q_3 - q_0q_1) \ 2(q_1q_3 - q_0q_2) 2(q_2q_3 q_0q_1) 1-2(q_1^2q_2^2) \end{bmatrix}$$反过来由R求四元数时可以用迹最大分量的方法避免数值不稳定。实际写代码时Eigen库的Quaterniond提供了fromRotationMatrix和toRotationMatrix大多数情况下不需要手写公式但我仍然建议至少推导一遍因为很多BA优化里的雅可比是和四元数切切实实相关的只靠库函数会学得很空。5.3 四元数的乘法与旋转复合真的不难两个旋转依次执行对应的四元数乘法是q_total q2 ⊗ q1注意顺序和矩阵乘法一样先q1后q2。四元数乘法的展开公式在书上有一整页看起来复杂本质上就是普通多项式乘法再套用i、j、k的规则。我用一个小技巧记忆把它看成向量形式标量矢量然后套用标量乘法和叉积公式。在代码里Eigen的Quaterniond重载了运算符表示四元数乘法也支持用直接对向量做旋转比如p_rotated q * p这里p被当成纯虚四元数处理。第一次看到这种写法的人可能觉得魔幻但习惯之后确实比矩阵乘法香——它省空间、没有奇异值、组合也顺畅。6. 齐次坐标与变换矩阵把平移旋转装进同一个框架6.1 为什么不能只用R和t复合变换的擦屁股问题假设一个点先经过变换T1旋转R1、平移t1再经过变换T2旋转R2、平移t2那么总的变换是R R2R1t R2t1 t2。如果连续嵌套几十次变换手写这个公式很快会出错。更关键的是旋转和平移分开处理时变换的逆计算也会变得很别扭。齐次坐标解决了这个问题。把三维点p扩充成四维向量(p, 1)同时把R和t拼成一个4x4矩阵T$$T \begin{bmatrix} R t \ 0^T 1 \end{bmatrix}$$那么变换就可以统一写成Tp的形式。连续变换的复合直接变成矩阵乘法T_total T2 * T1求逆也变成对T直接求逆。这个统一正是工程上最喜欢的东西——一套矩阵运算搞定所有几何变换。6.2 特殊欧氏群SE(3)带上平移的SO(3)4x4变换矩阵的集合被称为特殊欧氏群SE(3)。它包含旋转部分属于SO(3)和平移部分任意三维向量。SE(3)同样构成群任意两个变换矩阵相乘仍是变换矩阵它们的逆也仍是变换矩阵。这一章引入的SO(3)和SE(3)到第四章都要对应成李代数所以现在的群概念理解得越扎实后面越省力。6.3 齐次坐标的最后一层深意把投影也统一进来第三讲在末尾提到了一个很重要的视角齐次坐标不只用来拼接R和t它还能统一表达透视投影。一个三维点经过相机内参K的作用投影到像素平面在齐次坐标里就是一次线性变换。SLAM里我们经常要在世界坐标—相机坐标—像素坐标之间来回切换每一层切换都可以用齐次坐标写成一个矩阵。学完整本十四讲再回头看会发现第三讲其实是在为整个多视图几何的代数体系打地基。7. 莽一点我用代码验证这一章的每个变换公式7.1 用Eigen验证旋转矩阵、旋转向量、四元数互转只看推导不动手很容易看一遍觉得自己懂了闭卷就露馅。我的建议是用Eigen把第三章的每个公式都跑一遍。下面这段C代码展示了旋转向量转旋转矩阵、矩阵转四元数、以及用四元数旋转一个点的完整流程#include Eigen/Core #include Eigen/Geometry #include iostream int main() { // 旋转轴沿 z 轴旋转 90 度 Eigen::Vector3d axis(0, 0, 1); axis.normalize(); double angle M_PI / 2; // 方法1直接从角轴构造旋转矩阵 Eigen::Matrix3d R Eigen::AngleAxisd(angle, axis).toRotationMatrix(); std::cout R \n R std::endl; // 方法2罗德里格斯公式手工构造验证用 Eigen::Matrix3d axis_hat; axis_hat 0, -axis(2), axis(1), axis(2), 0, -axis(0), -axis(1), axis(0), 0; Eigen::Matrix3d R2 cos(angle) * Eigen::Matrix3d::Identity() (1 - cos(angle)) * axis * axis.transpose() sin(angle) * axis_hat; std::cout R2 \n R2 std::endl; // 旋转矩阵 - 四元数 Eigen::Quaterniond q(R); std::cout q q.coeffs().transpose() std::endl; // 四元数旋转一个点 Eigen::Vector3d p(1, 0, 0); Eigen::Vector3d p_rotated q * p; std::cout p_rotated p_rotated.transpose() std::endl; return 0; }运行结果里p_rotated应该是(0, 1, 0)也就是绕z轴转了90度。你别小看这个验证我身边好几个同学在这一步才发现自己会用手推、不会用库或者反过来只会调库说不清公式两条都通了才算是真懂。7.2 一个最容易翻车的细节四元数构造函数参数顺序Eigen的Quaterniond明明接收(实部, 虚部x, 虚部y, 虚部z)但很多人传参时容易传成(x, y, z, w)。我当时就吃过这个亏构造出来的四元数旋转结果完全不对排查了半天才发现是参数顺序问题。Eigen的构造函数是Quaterniond(w, x, y, z)coeffs()返回的顺序却是(x, y, z, w)。这种不一致非常坑。正因如此我强烈建议在代码里写一个带注释的包装函数比如Eigen::Quaterniond makeQuaternion(double w, double x, double y, double z) { return Eigen::Quaterniond(w, x, y, z); }这样每个调用的意图都能看得明明白白省下无数排查时间。7.3 变换矩阵的坑t不是随便用的用4x4变换矩阵T做事时新手容易把t当成点在目标坐标系下的坐标。实际t的含义是目标坐标系原点在当前坐标系下的坐标。举个例子世界系到相机系的变换矩阵T_cw中平移部分t是相机在世界系下的坐标位置不是某个点在世界系下的坐标。把这一点想明白很多坐标变换的谜之错误就都通了。我的实操习惯是在代码里命名时直接体现这个语义比如把变换矩阵命名为T_camera_from_world而不是光秃秃写个T。Eigen里也完全可以用T_cw * p_w这种命名代码可读性会好很多。8. 从公式到SLAM工程这一章到底在哪里用8.1 相机位姿就是一组TSLAM要估计的相机位姿本质上就是相机坐标系与世界坐标系之间的变换矩阵T_cw。有了它任何一个世界系下的地图点都能转到相机系下做投影反过来相机系下的观测也能投影回世界。整个SLAM的图优化里顶点就是这些T边就是它们之间的相对约束。第三讲里的变换矩阵公式初看只是机械操作到后面就会贯穿整个工程关键帧之间的相对位姿用T计算、闭环约束的累积误差用T连乘、后端优化里用T把误差投影回局部坐标系。可以说这一章的每一个符号都会再次出现。8.2 为什么要同时掌握四种旋转表达既然四元数这么好为什么还要学旋转矩阵、旋转向量、欧拉角因为不同环节有不同需求。旋转矩阵适合数值计算一次变换就是一次矩阵乘法旋转向量适合直观理解绕轴转多少度欧拉角适合人机交互四元数适合状态表示和插值。在实际代码里我通常会在接口层用四元数传递姿态在计算层用旋转矩阵在调试显示层用欧拉角。每次切换都明确调转换函数而不是让变量带着隐含语义到处飘。这里面的功夫就是第三讲反复练习得到的转换敏感度。8.3 手写一遍还是直接用库我的建议《视觉SLAM十四讲》的第三讲课后题里有一道是用Eigen实现旋转矩阵、旋转向量、四元数、欧拉角之间的相互转换我建议你一定亲手写一遍哪怕最终代码里全都是Eigen现成接口推导的肌肉记忆也能在遇到bug时给你第六感。另外建议把isometry的坑提前了解Eigen的Isometry3d存储4x4矩阵但它内部不保证左上角是合法旋转矩阵要保证使用前确实有合法旋转分量。我在写手眼标定的时候吃过这个亏——标定结果怎么都不对后来打印出Isometry3d的矩阵才发现旋转部分已经退化了。9. 一个帮助记忆的类比把姿态想象成移动并转向的机器人我一直觉得第三讲之所以看得费劲是因为符号太多但内容本身其实很直观。你可以想象一个机器人站在三维空间里旋转矩阵是它转了多少平移向量是它走了多远变换矩阵是这次转向加移动的总账本。欧拉角是先低头再抬头再左右看的描述方式四元数是直接绕某个方向转某个角度的更聪明的描述方式。我在做视觉SLAM调试时脑子里一直保持这个画面相机在世界系里挪动、转动它看到的所有特征点都要经过T_cw转到相机系下再投影到像素平面。这个画面一旦建立第三章的所有数学符号都变得有实际意义了后面进入到对极几何、PnP求解也不会觉得数学和代码是两张皮。如果让我给这条笔记做个经验总结只有一句话不要急着往后翻先把第三章的每一个矩阵转换在Eigen里验证一遍验证过的公式才是你自己的工具。我后来做过好几个不同传感器融合的项目凡是姿态处理出bug回根溯源几乎都是旋转表达切换时没注意语义这种低级但隐蔽的错误而这些全都能靠第三章的扎实基础提前堵住。