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 R=I、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^2+q_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^2+q_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^2+q_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 = R2R1，t = 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 相机位姿就是一组T
SLAM要估计的相机位姿，本质上就是相机坐标系与世界坐标系之间的变换矩阵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，回根溯源几乎都是"旋转表达切换时没注意语义"这种低级但隐蔽的错误，而这些全都能靠第三章的扎实基础提前堵住。
