在高级驾驶辅助系统（ADAS）领域，存在多种常用的坐标系：LiDAR坐标系、车辆坐标系、相机坐标系、图像坐标系等。在笔者最近的实习过程中，和这些坐标系频繁打交道。作为第一次在CSDN发文，本文将详细总结坐标变换矩阵。

目录

1. 何为坐标变换矩阵 （Transformation Matrix）

2. 旋转变换矩阵（Rotation Matrix）

3. 缩放变换矩阵（Scale Matrix）

4. 平移变换矩阵（Translation Matrix）

5. 综合变换

6. 小结

1. 何为坐标变换矩阵 （Transformation Matrix）

首先要回答一个问题，何为坐标变换矩阵呢？

“横看成岭侧成峰，远近高低各不同”，这说明了参照系的选取对我们观察事物的重要性。在以上所举例的坐标系变换的语境下，点是客观存在的，而坐标系则是根据不同的应用场景人为选择的。处理pcd点云数据时，需要三维的LiDAR坐标系；查看图像时，需要二维的图像坐标系（通常为1920*1080等尺寸）。即：

“点不变，坐标系进行变换。”

而坐标变换矩阵，就是在这种变换坐标系的前后，点的数值的变换映射关系矩阵。注意，点不动，坐标系动。下文讨论三种变换矩阵：旋转变换矩阵、缩放变换矩阵和平移变换矩阵。

2. 旋转变换矩阵（Rotation Matrix）

2.1 二维情形

如图1所示，在二维平面xoy上，由绿色坐标系逆时针旋转θ°到蓝色坐标系。可以看到，点A是没有移动的，变化的是点A分别在前后两个坐标系中的坐标，即从变换到了。

如图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）求原矩阵的逆矩阵：先逆时针，再顺时针，相当于回到原系，也就是坐标和单位矩阵相乘。

可以得到，坐标系顺时针旋转θ°的旋转变换矩阵为：

3. 缩放变换矩阵（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。

4. 平移变换矩阵（Translation Matrix）

坐标系的旋转和缩放可以通过3*3的变换矩阵完成，但是平移就需要将3*3扩展到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楼就是不动的一个点。

5. 综合变换

将变换矩阵和点向量全部齐次化：

那么此时，对于不动点，如果按照“旋转-缩放-平移”的顺序变换坐标系后，此点的值产生如下变换：

6. 小结

ADAS中涉及多种坐标系的变换，点的坐标会随着坐标系的变换而变化。坐标变换矩阵就是在坐标系变换前后，点的数值的映射关系矩阵。主要有旋转变换矩阵、缩放变换矩阵和平移变换矩阵等。极其重要和容易混淆的是，在具体的应用场景中，是什么在变，什么不变，是系还是点。矩阵的具体实现可以采用python中的numpy模块。

1 仿射变换与齐次坐标

《Unity3D C#数学系列之矩阵基础》中我们说到一个4×4的矩阵可以用来对三维空间中的点或向量进行各种变换，包括平移、旋转、缩放（当然还有错切、镜像等，由于我工作中这几种从来没用过，所以这里就不说这两种变换了）以及这几种变化的组合。这些变换有一个统一的名字，叫做仿射变换。三维空间中，只要是仿射变换，就可以用一个4×4的变换矩阵来表示。
但是为什么变换矩阵一定是4×4的呢，3×3行不行？答案是不行。如果不考虑点的平移，只考虑向量或点的旋转、缩放，3×3的矩阵就可以了。但如果考虑点的平移，3×3的矩阵就不满足要求了，必须扩充到4×4。至于为什么3×3无法表示平移，下面我们推导平移矩阵的时候就知道答案了。
我们知道三维空间中，一个点或者向量用3个变量x，y，z来表示就行了，但是在进行矩阵变换的时候，需要增加一维变量w。用(x, y, z, w)表示的向量或点，就叫齐次坐标。向量的w分量为0，点的w分量为1（为0时可以忽略掉平移的效果，如向量平移后还是它本身，为1时会保留平移的效果）。

2 变换矩阵的推导

2.1 变换矩阵推导一般过程

设4×4的变换矩阵为$\text{M}$_4×4。

设一个点A变换前的坐标为(x，y，z，w)，变换后的点A’ 坐标为(x’，y’， z’，w’)，则有
（还记得为什么点或向量的坐标要写成列矩阵的的形式吗，不记得了去看看上一篇文章）

矩阵乘法展开后就有方程组

我们上面说过，点的齐次坐标的w分量为1，所以又有

从上面方程组中的最后一个方程我们可以看出（无论x、y、z为何值都满足这个方程），$\text{m}$₄₁、$\text{m}$₄₂、$\text{m}$₄₃全部等于0，$\text{m}$₄₄等于1。

然后，我们令A(x，y，z，1)分别为（1， 0， 0， 1），（0，1，0， 1），（0，0，1， 1）同时我们需要分析出对应变换后的坐标A’(x’，y’， z’，1)，然后代入上面的方程组的前3个，一般就能推导出平移、旋转、缩放的矩阵了。

2.2 平移矩阵

设点A(x，y，z，1)平移（a，b，c，0）（即在x轴方向平移a个单位，y方向平移b个单位，z方向平移c个单位），则A’点的坐标为(x + a，y + b，z + c，1)，代入上面的方程组中有

无论x、y、z为何值都满足上面方程组中的各个方程，必然有

所以平移的变换矩阵为

平移矩阵的逆矩阵如下：

如果我们用一个3×3的矩阵来进行平移变换，将方程组展开，会发现根本没有解。所以3×3的矩阵无法表示平移，但4×4的矩阵可以。其根本原因是因为平移不是线性变换，必须要扩展一维才可以。

2.3 缩放矩阵

2.3.1 沿坐标轴的缩放

设向量(x，y，z，0)沿坐标轴缩放（k_x，k_y，k_z，0），则缩放后的坐标为(k_xx，k_yb，k_zz，0)，代入2.1中的方程组中有

显然有

所以沿坐标轴缩放（k_x，k_y，k_z）的缩放矩阵为

其逆矩阵为如下：

2.3.2 沿任意轴的缩放

设向量$\vec{v}$(x，y，z，0)沿单位向量$\vec{n}$(n_x，n_y，n_z，0)缩放k个单位，假设缩放后的坐标为$\overrightarrow{v^{\prime }}$(n_x，n_y，n_z，0)，现在我们要求缩放矩阵，必须先把$\overrightarrow{v^{\prime }}$给求出来。
我们将$\vec{v}$分为两个部分，一个是$\vec{v}$_⊥，其垂直于$\vec{n}$，沿着$\vec{n}$缩放将对其不产生任何影响；另一个是$\vec{v}$_//，其平行于$\vec{n}$，沿着$\vec{n}$缩放将对其产生影响。


由于$\vec{v}$_//平行于$\vec{n}$，则有$\vec{v}$_// = ($\vec{v}$·$\vec{n}$)$\vec{n}$。
所以$\vec{v}$_//沿着$\vec{n}$缩放k个单位后的值为k($\vec{v}$·$\vec{n}$)$\vec{n}$。
又由于$\vec{v}$_⊥ ⃗垂直于$\vec{n}$，所以$\vec{v}$_⊥ ⃗沿着$\vec{n}$缩放k个单位后的值仍为$\vec{v}$_⊥ = $\vec{v}$-$\vec{v}$_// = $\vec{v}$-($\vec{v}$·$\vec{n}$)$\vec{n}$。
于是

将$\vec{v}$=(x,y,z)，$\vec{n}$=(n_x,n_y,n_z )代入有，

代入2.1中的方程组中有

于是

所以沿单位向量$\vec{n}$（n_x，n_y，n_z）缩放k个单位的缩放矩阵为

2.4 坐标系之间的变换

在推导旋转矩阵之前，我们先看看不同坐标系之间的变换矩阵是怎么推导的，这是推导旋转矩阵的基础。
我们在说一个点或者一个向量的坐标的时候，一定要知道这个坐标是在什么坐标系下的。
我们在说一个点或者一个向量的坐标的时候，一定要知道这个坐标是在什么坐标系下的。
我们在说一个点或者一个向量的坐标的时候，一定要知道这个坐标是在什么坐标系下的。
只要说到坐标，一定要立马想到这个坐标所在的坐标系。
同一个点或者同一个向量，在不同坐标系下，它们的坐标值是不一样的。所以我们这里才会来求不同坐标系之间的变换矩阵。
这里先直接给出结论：

具体推导过程如下。

我们在上一篇文章《Unity3D C#数学系列之矩阵基础》中说过逆矩阵可以用来还原某种变换或者说反变换，且正交矩阵的逆矩阵就是其转置矩阵。并且，由标准正交基(长度为1的坐标系的x、y、z轴向量)构成的矩阵其实就是正交矩阵。
而对于向量，无论怎么平移，向量的值都是不变的，所以对于向量其变换矩阵使用3×3就足够了。那么对于向量，坐标系O’x’y’z’到Oxyz的变换矩阵也可求到了，如下。

2.5 旋转矩阵

2.5.1 二维平面中的旋转

我们先来看二维平面上的旋转。在二维平面上，我们说的旋转是指绕着某一个点旋转。
说一条线绕着某一点(假设名字为旋转中心)旋转，是指该线上的所有点绕着旋转中心旋转。
说一个多边形绕着某一点(假设名字为旋转中心)旋转，是指这个多边形上的每一个点绕着旋转中心旋转。

2.5.1.1 绕原点的旋转

我们先来推导下二维平面Oxyz坐标系中，绕原点O旋转的旋转矩阵。

设坐标系Oxyz是左手坐标系(因为Unity中世界坐标系和局部坐标系都是左手坐标系，所以我们这里举例子也按照Unity的来)，A点绕着原点O旋转了θ度到达新点A‘，求旋转矩阵。
推导过程如下。


旋转矩阵是正交矩阵。
（还记得正交矩阵满足什么条件吗？忘记了去复习一下《Unity3D C#数学系列之矩阵基础》第4.4节）
所以其逆矩阵就是其转置矩阵。

2.5.1.2 绕任意点的旋转

如图，左手坐标系Oxyz中，B点在Oxyz坐标系的坐标为(a, b)，求A点绕点B旋转θ角的旋转矩阵。

其推导过程如下：

2.5.2 三维空间中的旋转

三维空间说的旋转是指一个点或者一个向量绕着某一个方向或者说某一个轴旋转（二维是绕着某一个点）。

2.5.2.1 绕x轴的旋转

如图，A点绕X轴旋转θ度后到达A‘点(我们这里还是假设Oxyz坐标系是左手坐标系的)。

绕着X轴旋转其实可以理解在Oyz这个二维平面内绕着原点O进行旋转。然后将A’点和A点的x轴坐标设置为一样的。
上面这句话可能没表达清楚，看下面这张图应该就能理解了，其中灰色平面与Oyz平面平行。

根据上一节我们讲的二维平面中绕原点的旋转矩阵，咱们可以直接写出三维空间中绕x轴的旋转矩阵。

2.5.2.2 绕y轴的旋转

同绕x轴的旋转，绕着y轴的旋转可以理解为在Oxz这个二维平面内绕着原点O进行旋转。然后将A’点和A点的y轴坐标设置为一样的。

绕y轴旋转θ角的旋转矩阵为

2.5.2.3 绕z轴的旋转

同绕x轴的旋转，绕着z轴的旋转可以理解为在Oxy这个二维平面内绕着原点O进行旋转。然后将A’点和A点的z轴坐标设置为一样的。

绕z轴旋转θ角的旋转矩阵为

2.5.2.4 绕任意轴的旋转

如图，Oxyz坐标系中，A点绕向量$\vec{r}$ =(a,b,c)旋转θ度到达A’点，求旋转矩阵。

绕任意轴的旋转与二维平面的旋转推导方法类似的，关键在建立一个辅助坐标系。
其推导过程如下：

后面我实在是不想算了，要用的时候直接查资料吧。。。

博主本文博客链接。

仿射变换，是一种二维坐标到二维坐标之间的线性变换；它保持了二维图形的“平直性”（直线经过变换后依然是直线）和“平行性”（二维图形之间相对位置保持不变，平行线依然是平行线，且直线上点的位置关系不变）。仿射变换可以写为如下形式。

变换形式如下，a0, a1, a2, b0, b1,b2是对应2*3变换矩阵的几个值。

针对图像而言，变换矩阵和图像之间关系为：dst_img=M*src_img

仿射变换有如下几种变换形式：平移、旋转、放缩、剪切、翻转。

        为了涵盖平移，将变换矩阵引入齐次坐标，在原有二维基础上增加一个维度，如下：

不同的基础变换，a,b,c,d,e,f约束不同，可以有如下几种，a,b,d,e主要控制x,y的旋转，缩放指标，c,f 则是控制是否平移的指标。比如当c,f不为0时候，表示原图左下角坐标(h1,w1)平移变成了（h1+y, w1+x）。当a,b,d,e 分别是1,0,0,1时候，表示原图尺寸不变，如果变成了 w2, 0, 0, h2，则表示原图的height和width分别re_scale了H，W倍。

借助opencv， 一张图像的仿射变换求解流程：

1，给出图像上三个点的坐标，以及仿射变换后对应的这三个点的坐标

2，利用M=cv2.getAffineTransform(src_point,dst_point)得到转换矩阵M

3，利用dst= cv2.warpAffine(img,M,(new_width,new_height))得到转换后的图像。其中img是原图，(new_width,new_height)是转换后的图像尺寸。

另外一种方法，根据如上图的自定义的仿射变换结果，直接给出M矩阵，求解仿射后图像的方法。

比如，想通过仿射变换实现将一张图resize成小点的尺寸，使用两种方法的对比代码如下：

# -*- coding: utf-8 -*-

import cv2
import numpy as np

img = cv2.imread('test.jpg')
# img=cv2.resize(img,(250,300))
height, width = img.shape[:2]

new_height=220
new_width=180
print("ori:%d,%d, new: %d, %d"%(height,width,new_height,new_width))
scale=min(new_height/height,new_width/width)
M1=np.array([[scale, 0, 0],
             [0,scale, 0]])
iM1=cv2.invertAffineTransform(M1)#逆矩阵，可反向由dst*iM1求得src

dst1=cv2.warpAffine(img,M1,(new_width,new_height))
new_dst=np.zeros([height,width,3]).astype(np.uint8)
new_dst[:new_height,:new_width,:]=dst1
img1 = np.hstack([img,new_dst])

src_point=np.float32([[0,0],[0,height],[width,0]])
dst_point=np.float32([[0,0],[0,new_height],[new_width,0]])
M2=cv2.getAffineTransform(src_point,dst_point)
dst2=cv2.warpAffine(img,M2,(new_width,new_height))
new_dst1=np.zeros([height,width,3]).astype(np.uint8)
new_dst1[:new_height,:new_width,:]=dst2
img2 = np.hstack([img,new_dst1])
img3=np.hstack([img1,img2])
cv2.imshow("M2",img3)
cv2.waitKey(0)

'''
# 在原图像和目标图像上各选择三个点
mat_src = np.float32([[0, 0],[0, height-1],[width-1, 0]])
mat_dst = np.float32([[0, 0],[100, height-100],[width-100, 100]])
# 得到变换矩阵
mat_trans = cv2.getAffineTransform(mat_src, mat_dst)
# 进行仿射变换
dst = cv2.warpAffine(img, mat_trans, (width,height))

# 显示
imgs = np.hstack([img,dst])
cv2.namedWindow('imgs', cv2.WINDOW_NORMAL)
cv2.imshow("imgs",imgs)
cv2.waitKey(0)
'''

两种方法对比如下，左边是直接定义M，右边是通过两组图的三个对应点求M的方式，所有的左边是原图，右边是仿射处理后的图。

透射变换

透视变换的API和仿射变换的API很像，原理也相同，不同的只是由之前的三点变为四点法求透视变换矩阵，变换矩阵也由2*3变为3*3

M = cv2.getPerspectiveTransform(src_points, dst_points)
im_dst= cv2.warpPerspective(im_src, M, dsize = (w,h), flags = cv2.INTER_LINEAR)

参考：仿射变换及其变换矩阵的理解 - shine-lee - 博客园

OpenCV: Geometric Image Transformations