3、刚体变换与位姿表示：旋转矩阵、平移向量、齐次坐标、欧拉角、四元数

各位同学，今天我们来聊聊外参标定里最核心的数学基础——刚体变换。说白了，就是研究一个物体在三维空间里怎么转、怎么挪。你想想看，相机装在车上，车在动，相机跟着动，这个「动」就是刚体变换。

我刚开始做标定的时候，觉得这东西不就是个旋转加平移嘛，有啥难的？结果第一次实际标定，数据怎么都对不上，折腾了两天才发现是旋转矩阵的符号搞反了。嗯，从那以后我再也不敢小看这块了。

旋转矩阵，说白了就是一个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的朝向关系。我习惯把它理解成「三个轴的方向余弦」——你想想看，新坐标系的x轴在原坐标系里的投影，就是矩阵的第一列。

旋转矩阵有个重要性质：它是正交矩阵。什么意思？就是它的逆等于它的转置。这个性质在实际计算中特别有用，比如你要求反向旋转，直接转置就行，不用求逆。

核心性质：

我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，由于浮点误差，矩阵会慢慢偏离正交性。这时候就需要做正交化处理。我一般用SVD分解来修正，效果还不错。

平移向量就简单多了，就是一个3x1的列向量，表示两个坐标系原点之间的偏移。比如相机坐标系原点相对于世界坐标系原点的位置，就是平移向量t。

这里有个容易踩的坑：平移向量的方向。我记得有一次标定，数据怎么都对不上，后来发现是平移向量的方向搞反了。你想想看，从世界坐标系到相机坐标系的平移，和从相机到世界的平移，正好是相反数。

避坑指南：

我曾经在项目中把平移向量和旋转矩阵的变换顺序搞混了。记住：先旋转，后平移。顺序错了，结果完全不对。

为什么要引入齐次坐标？说白了就是为了把旋转和平移统一成一个矩阵乘法。你想想看，如果不用齐次坐标，你要先做旋转（矩阵乘法），再做平移（向量加法），两步操作。用齐次坐标，一步矩阵乘法搞定。

齐次坐标就是把三维点(x, y, z)变成四维(x, y, z, 1)。对应的变换矩阵是4x4的：

| R   t |
| 0   1 |

这个4x4矩阵就是刚体变换矩阵，也叫SE(3)矩阵。我习惯叫它「外参矩阵」，因为相机外参标定标的就是这个矩阵。

个人经验：

我在做多相机标定时，经常用齐次坐标来串联多个变换。比如从相机1到世界，再从世界到相机2，直接矩阵相乘就行，非常方便。

欧拉角用三个角度来表示旋转：绕x轴转（roll）、绕y轴转（pitch）、绕z轴转（yaw）。听起来很直观对吧？但实际用起来坑不少。

最大的问题是万向锁。当pitch接近90度时，roll和yaw会变得无法区分。我记得第一次遇到这个问题是在做无人机姿态估计时，飞机垂直朝下时，航向角突然跳变，吓了我一跳。

另一个问题是旋转顺序。不同的应用场景用不同的顺序：航空航天用ZYX，机器人常用ZYZ。我建议你在项目里统一用一种顺序，并在文档里明确标注。

四元数是我个人最喜欢的旋转表示方式。它用四个数表示旋转：一个实部和三个虚部。形式上写成q = w + xi + yj + zk。

为什么用四元数？三个原因：

我建议你在做视觉SLAM或者IMU融合时，优先用四元数。旋转矩阵虽然直观，但做优化时约束太多，四元数只需要归一化约束就行。

四元数转旋转矩阵公式：

R = | 1-2(y²+z²)  2(xy-wz)    2(xz+wy)   |
    | 2(xy+wz)    1-2(x²+z²)  2(yz-wx)   |
    | 2(xz-wy)    2(yz+wx)    1-2(x²+y²) |

下面这张图是我整理的刚体变换知识体系，你可以看到各种表示方式之间的关系和转换路径。

看到这张图，你应该能理解：旋转矩阵、平移向量、齐次坐标、欧拉角、四元数，它们都是描述同一个东西的不同方式。就像描述一个人的位置，你可以说经纬度，也可以说地址，还可以说GPS坐标——本质一样，只是表达方式不同。

我个人建议：做标定计算时用旋转矩阵（方便求导），做姿态估计时用四元数（避免万向锁），做可视化时用欧拉角（直观）。至于齐次坐标，那是把旋转和平移统一起来的工具，写代码时用起来特别顺手。

实用建议：

我在写标定代码时，会封装一个Transform类，内部用四元数+平移向量存储，对外提供转旋转矩阵、转欧拉角、转齐次矩阵的接口。这样既保证了内部计算的稳定性，又方便外部调用。

好了，这一章的内容就到这里。刚体变换是外参标定的数学基础，理解透了，后面的标定算法学起来就轻松多了。