4、刚体变换基础：旋转矩阵、平移向量、齐次坐标、欧拉角与四元数
各位同学，欢迎来到第四讲。
前面我们聊了传感器各自的坐标系，也聊了内参。现在，我们要解决一个核心问题：怎么把激光雷达看到的一个点，准确地映射到相机的像素坐标上？
说白了，就是两个坐标系之间的“刚体变换”。刚体，顾名思义，就是不会发生形变的物体。我们假设激光雷达和摄像头都是刚性的，它们之间的相对位置和姿态是固定的。那么，这个变换就由两部分组成：旋转和平移。
4.1 旋转矩阵：最直观的旋转表达
先说说旋转。想象一下，你手里拿着一个手机，从正面对着屏幕，到把手机横过来。这个动作就是旋转。
在数学上，我们用旋转矩阵来描述它。它是一个3x3的矩阵，记作 R。它的每一列，其实代表了旋转后的新坐标系的基向量在原坐标系下的投影。
核心性质：
- 正交矩阵：RTR = I，且 det(R) = 1。
- 逆矩阵等于转置：R-1 = RT。这意味着求逆运算非常快。
举个例子，绕Z轴旋转θ角度的旋转矩阵长这样：
R_z(θ) = | cosθ -sinθ 0 |
| sinθ cosθ 0 |
| 0 0 1 |我个人习惯把旋转矩阵看作是“坐标系对齐器”。你想想看，一个点P在A坐标系下的坐标是PA，那么它在B坐标系下的坐标PB = RBA * PA。这里的RBA，就是把A坐标系旋转到B坐标系的矩阵。
实战小贴士： 我在项目中遇到过，有人把旋转矩阵的转置和逆搞混了。记住，对于旋转矩阵，转置就是逆。如果你发现点云投影到图像上位置完全不对，先检查一下是不是把R用反了。
4.2 平移向量：把坐标系挪过去
旋转完了，两个坐标系的原点还不在一起。这时候就需要平移向量 t 了。它是一个3x1的向量，表示从A坐标系原点到B坐标系原点的偏移量。
完整的刚体变换公式就是：
PB = R * PA + t
这里要注意，t 是在B坐标系下表示的。什么意思呢？就是你先用R把PA旋转到和B坐标系同向，然后再沿着B坐标系的轴移动t的距离。
我曾经踩过的坑： 有一次标定完，发现投影误差总是很大。查了半天，原来是平移向量的单位搞错了。激光雷达给的是毫米，相机标定用的棋盘格是米。嗯，这种低级错误，希望大家不要犯。
4.3 齐次坐标：把旋转和平移写在一起
每次都要写 PB = R * PA + t，是不是有点麻烦？而且，连续变换的时候，公式会变得很冗长。
这时候，齐次坐标就派上用场了。我们把一个三维点 (x, y, z) 扩展成四维 (x, y, z, 1)。同时，把旋转矩阵和平移向量组合成一个4x4的变换矩阵 T：
T = | R t |
| 0 1 |那么，变换就变成了一个简单的矩阵乘法：
PB = T * PA
你看，是不是简洁多了？而且，连续变换 TCB * TBA 就可以直接乘起来，非常方便。
为什么叫“齐次”？ 因为最后一位是1，代表这是一个“点”。如果最后一位是0，代表这是一个“方向向量”，平移对它无效。这个细节在计算法向量时很有用。
4.4 欧拉角：直观但容易“万向锁”
旋转矩阵有9个参数，但自由度只有3个。有没有更直观的表达？有，欧拉角。
欧拉角用三个角度来描述旋转：绕X轴旋转（roll，翻滚角）、绕Y轴旋转（pitch，俯仰角）、绕Z轴旋转（yaw，偏航角）。
听起来很直观，对吧？但这里有个大坑——万向锁。
为什么会这样？当pitch角达到±90度时，roll和yaw的旋转轴会重合，导致丢失一个自由度。这时候，你无法唯一地表示某些姿态。
我的建议： 在标定过程中，我们通常用欧拉角来初始化外参，或者做可视化调试。但在内部计算和优化时，我强烈建议使用旋转矩阵或四元数。我曾经在优化代码里用了欧拉角，结果迭代到某个点直接发散，查了半天才发现是万向锁的问题。
4.5 四元数：SLAM和标定的首选
四元数，听起来有点玄乎，但它其实是解决旋转问题的最优雅工具之一。
一个四元数 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。它满足 i² = j² = k² = ijk = -1。
用四元数表示旋转，有几个巨大的优势：
- 无万向锁：这是最核心的优势。
- 插值平滑：在两个姿态之间做插值，用四元数的球面线性插值（Slerp）非常平滑自然。
- 紧凑：只需要4个参数，比旋转矩阵的9个少。
一个点P用四元数旋转的公式是：
P' = q * P * q-1其中，P 被看作一个纯虚四元数 (0, x, y, z)。q-1 是 q 的逆。
实战中的选择： 在自动驾驶的标定和SLAM系统中，我几乎只用四元数。比如在优化外参时，我通常把旋转部分用四元数表示，平移部分用三维向量表示。这样优化变量只有7个，而且没有奇异性问题。
4.6 它们之间的转换
在实际工程中，我们经常需要在不同表达方式之间切换。下面这个表格总结了它们之间的转换关系：
| 从/到 | 旋转矩阵 | 欧拉角 | 四元数 |
|---|---|---|---|
| 旋转矩阵 | - | 从矩阵元素反解角度（注意多解） | 从矩阵迹和元素计算四元数分量 |
| 欧拉角 | 按顺序乘三个基本旋转矩阵 | - | 用半角公式组合三个轴的旋转 |
| 四元数 | 用四元数元素构造矩阵 | 从四元数解出欧拉角 | - |
一个小技巧： 如果你用Eigen库，这些转换都有现成的函数。比如 Eigen::Quaterniond q(R); 就可以从旋转矩阵构造四元数。但理解背后的原理，能帮你更快地定位问题。
4.7 总结与避坑
好了，这一讲的内容就到这里。我们来回顾一下重点：
- 旋转矩阵：9参数，正交，适合计算。
- 平移向量：3参数，注意坐标系。
- 齐次坐标：把旋转和平移统一成4x4矩阵，方便连续变换。
- 欧拉角：直观，但有万向锁，适合人看，不适合机器算。
- 四元数：4参数，无奇异性，SLAM和标定的首选。
最后，我再强调一点： 无论你用哪种表达方式，一定要明确旋转的顺序和坐标系定义。比如，是绕固定轴旋转还是绕动轴旋转？是先旋转再平移，还是先平移再旋转？这些细节，差之毫厘，谬以千里。我在项目里见过太多因为坐标系定义不清导致的bug了。
下一讲，我们会把这些刚体变换的知识，真正用到激光雷达和相机的联合标定中去。到时候，你会看到这些数学工具是如何在代码里发挥作用的。