超详细！从单应矩阵推导到自动驾驶环视投影应用
本文内容
本文的主要目的在于介绍自动驾驶中环视投影，也就是所谓的俯瞰图/鸟瞰图/BEV图，其背后的计算机视觉原理。环视投影的背后理论基础就是单应矩阵（Homography Matrix）。为了建立从直观到一般形式，本文从以下章节内容展开
在第一节中先规定相机坐标系和图像坐标系，以及介绍常用的车辆坐标系的规定。同时，对相机投影中常使用的齐次坐标做了简要介绍，顺带回顾相机投影方程。
第二节先对单应矩阵做介绍。对于一个空间中一个平面，以及两个不同姿态的相机，两个相机的对该平面的成像之间的联系是单应矩阵。但是如何从相机间的关系推导单应矩阵的具体形式？该节从特殊化形式和一般化形式两个角度对单应矩阵的表达进行推导。
环视投影是多个方向的地面成像结果到一个位于车辆上方，平行于地面往下成像的虚拟相机的成像结果的变换。第三节详细推导这个应用中的投影过程以及相机内外参与单应矩阵的关系。
第四节介绍如何在不知道相机内外参的情况下通过点对匹配的方法求解单应矩阵。
基础知识及相关定义
相机坐标系与图像坐标系
在计算机视觉的一般任务中，我们规定相机坐标系为光轴（经过相机原点）往前为\(Z\)轴正方向，右为\(X\)，下为\(Y\) 对空间中的点放缩到归一化平面，归一化平面的坐标系定义也类似相机坐标系。 但是在图像中，我们一般令图片的左上角为坐标原点，横轴往右为\(u/X\)正方向，竖轴往下为\(v/Y\)正方向，又叫像素坐标系
如下图所示
车辆坐标系
我们一般规定车的后轮横轴中心在地面的点为坐标系原点，\(X\)正方向指向车头，\(Y\)轴正方向指向车左
齐次坐标（Homogeneous Coordinate）
当我们对三维空间中一个点\(P_1=[X_1,Y_1,Z_1]^T\)做旋转平移变换到另一个点\(P_2\)，可以用下面的公式
\[ P_2 = RP_1+\boldsymbol{t} \]
其中\(R\in \mathbb{R}^{3\times3}\)为旋转矩阵，\(\boldsymbol{t}\in \mathbb{R}^{3\times 1}\)为平移向量
上式也可以将点坐标写为\(P_1=[X_1,Y_1,Z_1,1]^T\)的形式，然后用变换矩阵对点做坐标变换
\[ P_2 = T_{21}P_1 \]
\[ T_{21} = \begin{bmatrix} R&\boldsymbol{t}\\ \boldsymbol{0}&1 \end{bmatrix}\in \mathbb{R}^{4\times 4} \]
\(T_{21}\)称为变换矩阵;\(P_1=[X_1,Y_1,Z_1,1]^T\)这种形式也叫齐次坐标（Homogeneous Coordinate）
注意：齐次坐标其实有严格的定义，具体形式可以参考Wiki:Homogeneous Coordinate
同理，在做像素坐标的逆变换，也通常将像素坐标\([u,v]^T\)后面添加一个1变为齐次坐标\(p=[u,v,1]^T\)
\[ p_n=K^{-1}p \]
投影方程
空间中的一个点\(P_W\)投影到图像上的点\(\boldsymbol{p}\)，我们直接记这个过程为
\[ \begin{split} \lambda \boldsymbol{p} &=\ KT_{CW}P_W\\ &=\ \begin{bmatrix} u&v&1 \end{bmatrix}^T \end{split} \]
这里面包含了一次齐次坐标的转换（\(T_{CW}P_W \in \mathbb{R}^{4\times 1}\),\(K\in \mathbb{R}^{3\times 1}\)），但是不会对理解有歧义
单应矩阵（Homography Matrix）
在计算机视觉中，两个相机对于空间中的同一个平面的成像结果可以通过单应矩阵进行映射（相差一个常量系数）
另\([x,y,1]^T\)为图片1的像素齐次坐标，\([x^{\prime},y^{\prime},1]^T\)为同一个点在图片2的像素齐次坐标，则上面那句话可以等价为如下关系：
\[ \lambda \begin{bmatrix} x^{\prime}\\ y^{\prime}\\ 1 \end{bmatrix}= \boldsymbol{H} \begin{bmatrix} x\\ y\\ 1 \end{bmatrix}= \begin{bmatrix} h_{00} & h_{10} & h_{20}\\ h_{01} & h_{11} & h_{21}\\ h_{02} & h_{12} & h_{22} \end{bmatrix} \begin{bmatrix} x\\ y\\ 1 \end{bmatrix}\tag{2-1} \]
公式（2-1）中的系数\(\lambda\)用来将变换后的坐标归一化（将第三个维度的值放缩为1） 同时要注意，单应矩阵虽然有9个元素，但自由度只有8，可以除以\(h_{22}\)进行归一化
下面我们分别从用形象化的解释和一般化的推导来建立单应矩阵与相机内参、外参的关系
特殊形式推导
我们假设空间中有一平面，两个相机\(C_1,C_2\)在两个不同的姿态下对这个平面进行成像。同时，我们令世界坐标系（\(O_G\))\(的\)Z\(轴垂直于该平面，并且令该平面刚好位于世界坐标系的\)XY\(平面，这样这个平面上的点\)z$坐标等于0。可以用下图帮忙理解
同时，我们记世界坐标系到相机\(C_1\)的变换矩阵为\(T_{C_1W}\)，到相机\(C_2\)的变换矩阵为\(T_{C_2W}\);\(K_1,K_2\)为两个相机的内参。
我们现在对平面上的一个点\(P_W=[X_w,Y_w,Z_w=0,1]^T\)进行投影，得到在两个相机成像的图片上对应的像素点\(\boldsymbol{p}_1,\boldsymbol{p}_2\)。投影过程具有如下形式
\[ \begin{split} \lambda_1\boldsymbol{p}_1 &=\ \begin{bmatrix} u_1\\ v_1\\ 1 \end{bmatrix} =\ K_1T_{C_1W}P_W\\ &=\ K_1 \begin{bmatrix} r_{00} & r_{01} & r_{02} & t_0\\ r_{10} & r_{11} & r_{12} & t_1\\ r_{20} & r_{21} & r_{22} & t_2\\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_W\\ Y_W\\ 0\\ 1 \end{bmatrix}\\ &=\ K_1 \begin{bmatrix} r_{00} & r_{01} & t_0\\ r_{10} & r_{11} & t_1\\ r_{20} & r_{21} & t_2 \end{bmatrix} \begin{bmatrix} X_W\\ Y_W\\ 1 \end{bmatrix}\\ &=\ K_1T_{C_1W}^{0:3;0:2,3}P_W \end{split} \]
上式中\(T^{0:3;0:2,3}\)表示变换矩阵前3行的0,1,3列。为了方便记号，我们将\(T_{C_1W}^{0:3;0:2,3}\)记为\(J_{C_1W}\)
同理，我们可以得到图像2的类似投影方程，我们整理如下
\[ \begin{split} \lambda_1\boldsymbol{p}_1 &= K_1J_{C_1W}P_W\\ \\ \lambda_2\boldsymbol{p}_2 &= K_2J_{C_2W}P_W\\ \end{split}\tag{2-2} \]
注意上式中\(P_W=[X_W,Y_W,1]\)，且上式中矩阵相乘的行列数量都是对应的
我们将上式（2-2）重新整理一下，将\(P_W\)写为\(p_1\)的函数，有
\[ P_W=\lambda_1J_{C_1W}^{-1}K_1^{-1}\boldsymbol{p}_1\tag{2-3} \]
将（2-3）带入（2-2）中的相机2方程
\[ \lambda_2\boldsymbol{p}_2=\lambda_1K_2J_{C_2W}J_{C_1W}^{-1}K_1^{-1}\boldsymbol{p}_1 \]
其中\(\lambda_1,\lambda_2\)只是为了将投影后的值进行放缩，使得第三个元素的值为1,所以两者可以写成一个，最后我们可以得到如下形式
\[ \begin{split} &\lambda\boldsymbol{p}_2=\boldsymbol{H}_{21}\boldsymbol{p}_1\\ & \boldsymbol{H}_{21} = K_2J_{C_2W}J_{C_1W}^{-1}K_1^{-1}\\ &J=T^{0:3,0:2,3} \end{split}\tag{2-4} \]
可以看到，公式（2-4）的形式和公式（2-1）是一样的，同时将单应矩阵与相机的内参外参联系起来
单应矩阵与相机内外参关系的一般形式
上一个小节我们对单应矩阵与相机的内外参之间的关系建立了公式化的联系和直观上的理解。上一小节的推导可以看出，对于空间中的一快平面，存在一个\(3 \times 3\)的矩阵（自由度为8）将两个相机对这块平面的成像结果联系起来。
在上一小节中，我们假设世界坐标系与成像平面对齐。我们在这一小节中继续推导更一般的表达形式，这一小节的内容主要参考Wiki:Homography (computer vision)
由于一般情况下，我们都是知道相机的外参（也就是世界坐标系到相机坐标系的变换关系），所以我们这一节不假设平面的坐标系与世界坐标系对齐，我们假设平面在相机\(C_1\)下的法向量为\(\boldsymbol{n}\)，平面到相机\(C_1\)的距离为\(d\)，且对于法向量的方向，我们做如下规定：对于平面上的点\(P_{C_1}\)，满足\(\boldsymbol{n}^TP_{C_1}+d=0\)，其中\(\boldsymbol{n}^TP_{C_1}\)为点到法向量的投影
假设相机\(C_1\)的外参为\(T_{C_1W}=[R_1|\boldsymbol{t}_1]\)，相机\(C_2\)的外参为\(T_{C_2W}=[R_2|\boldsymbol{t}_2]\)，由变换矩阵的性质，我们可以得到
\[ \begin{split} T_{WC_1} &= T_{C_1W}^{-1}\\ &= \begin{bmatrix} R_1&\boldsymbol{t}_1\\ \boldsymbol{0}&1 \end{bmatrix}^{-1}\\ &=\ \begin{bmatrix} R_1^{-1}&-R_1^{-1}\boldsymbol{t}_1\\ \boldsymbol{0}&1 \end{bmatrix} \end{split}\tag{2-5} \]
所以相机1到相机2的变换矩阵为
\[ \begin{split} T_{C_2C_1} &= T_{C_2W}T_{WC_1}\\ &= \begin{bmatrix} R_2&\boldsymbol{t}_2\\ \boldsymbol{0}&1 \end{bmatrix} \begin{bmatrix} R_1^{-1}&-R_1^{-1}\boldsymbol{t}_1\\ \boldsymbol{0}&1 \end{bmatrix}\\ &=\ \begin{bmatrix} R_2R_1^{-1}&-R_2R_1^{-1}\boldsymbol{t}_1+\boldsymbol{t}_2\\ \boldsymbol{0}&1 \end{bmatrix}\\ &=\ \begin{bmatrix} R_{21}&\boldsymbol{t}_{21}\\ \boldsymbol{0}&1 \end{bmatrix} \end{split}\tag{2-6} \]
根据相机投影关系，我们有
\[ \begin{split} &\boldsymbol{p}_1=\frac{1}{Z_{C_1}}K_1P_1\\ &\boldsymbol{p}_2=\frac{1}{Z_{C_2}}K_2P_2\\ \end{split}\tag{2-7} \]
\[ P_1 = Z_{C_1}K_1^{-1}\boldsymbol{p}_1\tag{2-8} \]
又\(P_1,P_2\)为点\(P_W\)在两个相机坐标系下的坐标，这两个点又可以通过下面的变换联系
\[ P_2 = R_{21}P_1 + \boldsymbol{t}_{21}\tag{2-9} \]
联合（2-7）到（2-9），我们可以得出
\[ \boldsymbol{p}_2=\frac{1}{Z_{C_2}}K_2(R_{21}Z_{C_1}K_1^{-1}\boldsymbol{p}_1+\boldsymbol{t}_{21})\tag{2-10} \]
对于平面，我们已经做了如下规定： 在相机\(C_1\)坐标系下，对于平面上的点\(P_{1}\)，满足\(\boldsymbol{n}^TP_{1}+d=0\)，其中\(\boldsymbol{n}^TP_{1}\)为点到法向量的投影。即
\[ \frac{\boldsymbol{n}^TP_1}{-d}=1\tag{2-11} \]
将（2-11）带入公式（2-10）中的\(\boldsymbol{t}_{21}\)可以得到
\[ \begin{split} \boldsymbol{t}_{21} &=\ \boldsymbol{t}_{21}\frac{\boldsymbol{n}^TP_1}{-d}\\ &=\ \frac{\boldsymbol{t}_{21}\boldsymbol{n}^T}{-d}P_1\\ &\overset{2-8}{=}\ -\frac{\boldsymbol{t}_{21}\boldsymbol{n}^T}{d}Z_{C_1}K_1^{-1}\boldsymbol{p}_1 \end{split}\tag{2-12} \]
将（2-12）代入公式（2-10）中，我们可以得到
\[ \begin{split} \boldsymbol{p}_2&=\frac{1}{Z_{C_2}}K_2\left(R_{21}-\frac{\boldsymbol{t}_{21}\boldsymbol{n}^T}{d}\right)Z_{C_1}K_1^{-1}\boldsymbol{p}_1\\ &=\ \frac{Z_{C_1}}{Z_{C_2}}K_2H_{21}K_1^{-1}\boldsymbol{p_1} \end{split}\tag{2-13} \]
于是，我们可以得到单应矩阵的表达形式
\[ H_{21} = R_{21}-\frac{\boldsymbol{t}_{21}\boldsymbol{n}^T}{d}\tag{2-14} \]
如果我们将\(R_{21}\)与\(t_{21}\)的展开形式，即公式（2-6），则我们可以得到
\[ H_{21} = R_2R_1^{-1} - \frac{\left(-R_2R_1^{-1}\boldsymbol{t}_1+\boldsymbol{t}_2\right)\boldsymbol{n}^T}{d}\tag{2-15} \]
这篇论文Online Camera Pose Optimization for the Surround-viewSystem，中关于环视图的地平面投影一章的公式推导很详细，作为基础理论学习很不错。
环视投影/BEV
基础概念
从前文中我们可以得出，对于空间中的一个平面，不同位姿的相机成像的图片可以通过单应矩阵在不同的图片中进行变换。对于自动驾驶车辆来说，环绕车周的多个摄像头(一般是4个及以上)会同时拍摄到地面。同时，我们可以假设有一个虚拟相机位于车辆正上方往下拍摄。这样，车周的摄像头可以根据地平面对应的单应矩阵投影到虚拟相机，得到所谓的俯瞰图/鸟瞰图，也就是 BEV(Bird Eye View)
图示说明
上图中\(O_G\)是地面坐标系，这里与第一节中说明的车辆坐标系有一点不同，主要是为了方便后文进行公式推导。实际中只需要把图中的地面坐标系与车辆坐标系根据车身参数简单计算出变换矩阵即可。
上图中\(O_I\)是图像坐标系，以左上角为坐标原点，\(u,v\)为像素的横轴、纵轴坐标值。\(H,W\)为我们希望得到的地平面投影图像的投影范围，单位是\(米\)。同时，我们令\(d_W,d_H\)为图像每个像素对应的实际中方格的大小，单位也是\(米\)
地平面点到相机投影
假设车周有4个相机\(C_1,C_2,C_3,C_4\)，4个相机的与地面坐标系的位姿关系为\(T_{C_1G}, T_{C_2G}, T_{C_3G}, T_{C_4G}\)
对于一个地面坐标系下的点\(P_{G}=[X_G,Y_G,Z_G,1]^T\)，在通过投影方程投影在相机\(C_i\)上的图像像素坐标\(\boldsymbol{p}_i\)有如下关系：
\[ \begin{split} P_{C_i}&=T_{C_iG}P_G\\ &= \begin{bmatrix} X_{C_i} & Y_{C_i} & Z_{C_i} & 1 \end{bmatrix}^T \end{split} \]
则\(P_G\)在相机\(C_i\)中对应的像素坐标为
\[ \begin{split} \lambda_{C_i} \boldsymbol{p}_{C_i} &=\ K_{C_i}P_{C_{i}}\\ &=\ K_{C_i}T_{C_iG}P_G \end{split}\tag{3-1} \]
\(K_{C_i}\)是相机\(C_i\)的内参，\(\lambda_{C_i}\)是放缩系数，确保最后\(\boldsymbol{p}_{C_i}\)的第三个维度值为1,也就是具有如下形式：\(\boldsymbol{p}_{C_i}=[u,v,1]^T\)。另外，要注意（3-1）中包含一个齐次坐标的变换。
现在假设点\(P_G\)的\(Z\)轴坐标是0,也就是点位于地面，则 \(P_{G}=[X_G,Y_G,0,1]^T\)，上面\(P_{C_i}=T_{C_iG}P_G\)展开会变成
\[ \begin{split} P_{C_i}&=T_{C_iG}P_G\\ &= \begin{bmatrix} r_{00} & r_{01} & r_{02} & t_0\\ r_{10} & r_{11} & r_{12} & t_1\\ r_{20} & r_{21} & r_{22} & t_2\\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_{G_i}\\ Y_{G_i}\\ 0\\ 1 \end{bmatrix}\\ &= \begin{bmatrix} r_{00} & r_{01} & t_0\\ r_{10} & r_{11} & t_1\\ r_{20} & r_{21} & t_2\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_{G_i}\\ Y_{G_i}\\ 1 \end{bmatrix} \end{split} \]
我们可以重新记公式（3-1）为
\[ \begin{split} \lambda_{C_i}\boldsymbol{p}_{C_i}=K_{C_i}T_{C_iG}^{0:3;0:2,3}P_G \end{split}\tag{3-3} \]
其中
\[ T_{C_iG}^{0:3;0:2,3} = \begin{bmatrix} r_{00} & r_{01} & t_0\\ r_{10} & r_{11} & t_1\\ r_{20} & r_{21} & t_2 \end{bmatrix} \]
表示变换矩阵的前三行，第0,1,3列
公式（3-3）实际就是第二节中推导出的公式（2-2）
地平面点虚拟相机投影
根据上图，假设我们想要投影以地面坐标系原点为中心，\(H\times W\)的地面区域。同时，我们希望投影后的像素分辨率为一个像素对应横轴、长轴为\(d_W,d_H\)长度的地面方格。（PS:实际上相机的焦距的定义就是每个像素对应的实际尺寸，焦距的单位为\(pixel/meter\)）。对于地平面投影，我们可以假设有一个虚拟相机位于地面上1米，这样\(u=f_xX_G+c_x\)，\(f_x=\frac{1}{d_W}\)，\(c_x=\frac{W}{2d_W}\)。我们可以写成如下的形式
\[ \begin{bmatrix} u_G\\ v_G\\ 1 \end{bmatrix}= \begin{bmatrix} \frac{1}{d_W} & 0 & \frac{W}{2d_W}\\ 0 & \frac{1}{d_H} & \frac{H}{2d_H}\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_G\\ Y_G\\ 1 \end{bmatrix}\tag{3-4} \]
上式可以写成矩阵形式，我们记\(K_G\)为上式中的矩阵（虚拟相机的内参）
\[ \boldsymbol{p}_G = K_GP_G\tag{3-5} \]
联立公式（3-3）和公式（3-5）可以写为如下形式
\[ \lambda \boldsymbol{p}_G=K_G\left(T_{C_iG}^{0:3;0:2,3}\right)^{-1}K_{C_i}^{-1}\boldsymbol{p}_{C_i}\tag{3-6} \]
其中，\(\lambda\)系数只是为了将\(\boldsymbol{p}_G\)的最后一个维度归一化，不需要具体形式。
直接估计环视投影的单应矩阵
除了根据第三节的推导，从相机内外参直接推导出车周相机到地平面的投影矩阵，还可以直接人工选点估计单应矩阵。这一点跟透视矫正（Perspective Correction）任务基本类似。所谓的透视矫正，就是对于同一个平面的不同成像结果（两张图片），我们选择两张图片上的对应点构建对应点集，根据单应矩阵的定义
\[ \boldsymbol{p}_2 = H_{21}\boldsymbol{p}_1 \]
我们可以从对应的点集中求解出单应矩阵。由于前面第二节我们知道单应矩阵的实际自由度为8，而一个对应点对可以构建两个约束方程（\(x,y\)），所以我们一共最少需要4个点对来求解单应矩阵。
示例图
还是以上文的简笔图来说明
上图中两个相机对同一个平面成像，得到两张图片。两张图片中\(A,B,C,D\)四个点是一一对应的关系，记这些点对为\(\boldsymbol{p}_i,\boldsymbol{p}_i^{\prime}\)，则我们可以建立方程
\[ \begin{split} \lambda \boldsymbol{p}_i^{\prime}&=H\boldsymbol{p}_i \end{split} \]
即 \[ \lambda \begin{bmatrix} u_i^{\prime}\\ v_i^{\prime}\\ 1 \end{bmatrix} =\ \begin{bmatrix} h_{00}&h_{01}&h_{02}\\ h_{10}&h_{11}&h_{21}\\ h_{20}&h_{21}&h_{22}\\ \end{bmatrix} \begin{bmatrix} u_i\\ v_i\\ 1 \end{bmatrix}\tag{4-1} \]
单应矩阵求解
公式（4-1）的求解可以用最小二乘法，或者最小二乘法+RANSAC等算法，优化目标为：
\[ J = \sum_{i}\left(u_i^{\prime}-\frac{h_{00}u_i+h_{01}v_i+h_{02}}{h_{20}u_i+h_{21}v_i+h_{22}}\right)^2+\left(v_i^{\prime}-\frac{h_{10}u_i+h_{11}v_i+h_{21}}{h_{20}u_i+h_{21}v_i+h_{22}}\right)^2 \]
这部分的求解可以使用OpenCV的函数OpenCV:findHomography
环视投影单应矩阵求解
在环视投影中，我们需要一些额外的标定物要求解单应矩阵。主要的原理就是我们将地面上的坐标和图像中的坐标联立写成如公式（4-1）的形式，然后调用接口计算单应矩阵。图像中的坐标可以通过手工选点，也可以通过一些角点检测程序。
上图引用自：https://www.guyuehome.com/39649
上图中0,1,2,3四个点是在图片上选择的点，因此知道像素值。同时，我们又可以通过标定布的几何属性以及我们提前设定的投影图的分辨率，得出这四个点在BEV上的等效像素值，于是我们就可以求解方程（4-1）得出单应矩阵
单应矩阵的其他应用
单应矩阵还可以用在求解平面标定板的位姿、全景图拼接等应用，OpenCV的这一篇教程写得十分详细，这里就不再赘述。