在高级驾驶辅助系统（ADAS）领域，存在多种常用的坐标系：LiDAR 坐标系、车辆坐标系、相机坐标系、图像坐标系等。
在高级驾驶辅助系统（ADAS）领域，存在多种常用的坐标系：LiDAR 坐标系、车辆坐标系、相机坐标系、图像坐标系等。在笔者最近的实习过程中，和这些坐标系频繁打交道。作为第一次在 CSDN 发文，本文将详细总结坐标变换矩阵。
目录
何为坐标变换矩阵 （Transformation Matrix）
旋转变换矩阵（Rotation Matrix）
缩放变换矩阵（Scale Matrix）
平移变换矩阵（Translation Matrix）
综合变换
小结
何为坐标变换矩阵 （Transformation Matrix）
首先要回答一个问题，何为坐标变换矩阵呢？
“横看成岭侧成峰，远近高低各不同”，这说明了参照系的选取对我们观察事物的重要性。在以上所举例的坐标系变换的语境下，点是客观存在的，而坐标系则是根据不同的应用场景人为选择的。处理 pcd 点云数据时，需要三维的 LiDAR 坐标系；查看图像时，需要二维的图像坐标系（通常为 1920*1080 等尺寸）。即：
“点不变，坐标系进行变换。”
而坐标变换矩阵，就是在这种变换坐标系的前后，点的数值的变换映射关系矩阵。注意，点不动，坐标系动。下文讨论三种变换矩阵：旋转变换矩阵、缩放变换矩阵和平移变换矩阵。
旋转变换矩阵（Rotation Matrix）
2.1 二维情形
如图 1 所示，在二维平面 xoy 上，由绿色坐标系逆时针旋转θ° 到蓝色坐标系。可以看到，点 A 是没有移动的，变化的是点 A 分别在前后两个坐标系中的坐标，即从变换到了。
图 1 二维平面旋转矩阵
如图 1 中黑色虚线的分解方式所示，通过矢量分解（类似于物理中力、速度等矢量的分解），将绿色坐标系中的分别分解到蓝色坐标系的 x 轴和 y 轴上，可以得到：
用矩阵表示为：
其中 R 则为二维情形下的旋转变换矩阵，它表示了 A 点在前后坐标系中的值的映射关系。
2.2 三维情形
有了上述在二维平面旋转的基础，三维空间的旋转矩阵也就不难得出了。
即绕 x 轴，y 轴，z 轴分别进行旋转。最后将这三个旋转变换矩阵相乘，就能得到在三维空间任意角度的旋转变换矩阵了。（xyz 轴满足右手系关系）
在绕 x 轴旋转的时候，可以看作在 yoz 二维平面上的旋转，此时 x 的值不变。
在绕 y 轴旋转的时候，可以看作在 zox 二维平面上的旋转，此时 y 的值不变。
在绕 z 轴旋转的时候，可以看作在 xoy 二维平面上的旋转，此时 z 的值不变。
最终的三维旋转变换矩阵就是上面三个矩阵相乘，意为三维坐标系分别绕 x 轴、y 轴和 z 轴旋转相应的角度。
2.3 顺时针？逆时针？
在笔者初次接触旋转概念之时，常常对何时顺时针，何时逆时针十分头疼。
高中就接触到点的旋转矩阵 R（rotation matrix of a point），在这种情况下是坐标系不变，点绕坐标原点顺时针旋转 θ°。
而在本文的情形下，旋转变换矩阵的形式完全一致，但是方向却相反了——变成了逆时针！究其原因，还是上文老生常谈的那点，高中的矩阵是点动系不动，而本文是系动点不动。因此方向正好相反了。
回到本文的系动点不动，此时再从两种角度推导出顺时针旋转的公式。
1）θ变为 -θ：此时就是选择了相反的旋转角度，变成顺时针。
2）求原矩阵的逆矩阵：先逆时针，再顺时针，相当于回到原系，也就是坐标和单位矩阵相乘。
可以得到，坐标系顺时针旋转θ° 的旋转变换矩阵为：
缩放变换矩阵（Scale Matrix）
除了旋转变换，还有坐标数值的纯粹放大缩小变换，即缩放变换。下面直接给出缩放变换的公式：
上式中点的 x, y, z 坐标值分别扩大（缩小）了 Scale.x, Scale.y, Scale.z 倍。
缩放矩阵同样存在 “系动点不动” 还是 “点动系不动” 的问题。如果是“点动系不动”，那么 S 矩阵中的 Scale.x, Scale.y, Scale.z 就是单纯的点 x, y, z 的扩大（缩小）倍数。
如果是本文重点探讨的 “系动点不动”，那么 S 矩阵中的 Scale.x, Scale.y, Scale.z 就是坐标系的 x, y, z 轴的单位扩大（缩小）倍数的倒数。换言之，如果坐标轴单位放大 Scale 倍，那么点 x, y, z 的值就要缩小 Scale 倍。可以用千米和米的转换来思考这个问题。如果单位是 m，一个物体长 1000m。当单位变为 km 后，这个物体就长 1km 了。单位扩大的同时，数值上从 1000 缩小为了 1。
平移变换矩阵（Translation Matrix）
坐标系的旋转和缩放可以通过 33 的变换矩阵完成，但是平移就需要将 33 扩展到 4*4，引入齐次变换矩阵。下面直接给出平移变换矩阵的公式：
具体过程即：
平移矩阵同样存在 “系动点不动” 还是 “点动系不动” 的问题。
如果是本文重点探讨的 “系动点不动”。如果系向左（x 轴负方向）/ 后（y 轴负方向）/ 下（z 轴负方向）平移，那么 T 矩阵中的 Translation.x, Translation.y, Translation.z 为正；如果系向右（x 轴正方向）/ 前（y 轴正方向）/ 上（z 轴正方向）平移，那么 T 矩阵中的 Translation.x, Translation.y, Translation.z 为负。可以借助爬楼梯来理解，小明在地面抬头看着 5 楼，五楼相当于 + 5。当小明爬到 5 楼的时候，此时五楼相当于 0 了。小明爬到 10 楼的时候，此时五楼就相当于 - 5 了。小明就是坐标系的原点，5 楼就是不动的一个点。
综合变换
将变换矩阵和点向量全部齐次化：
那么此时，对于不动点，如果按照 “旋转 - 缩放 - 平移” 的顺序变换坐标系后，此点的值产生如下变换：
小结
ADAS 中涉及多种坐标系的变换，点的坐标会随着坐标系的变换而变化。坐标变换矩阵就是在坐标系变换前后，点的数值的映射关系矩阵。主要有旋转变换矩阵、缩放变换矩阵和平移变换矩阵等。极其重要和容易混淆的是，在具体的应用场景中，是什么在变，什么不变，是系还是点。矩阵的具体实现可以采用 python 中的 numpy 模块。
