智能车逆透视算法（IPM）原理及实现

嵌入式
12k words

之前校赛就有在用逆透视了，不过都是硬套学长写好的公式乱写。寒假重新来梳理一下这个算法。参考的是opencv库的函数，例如getPerspectiveTransform（opencv/modules/imgproc/src/imgwarp.cpp Line 3432）

写得有点乱，可能一些地方会有错误，不过大致意思应该没问题，代码跑出来效果还可以()。

和网上其他一些佬的做法应该有所不同，目前比较广为流传的标定方法是四点标定法，通过原图和逆透视后的图，用四个点(x,y)，总计8个参数，调用opencv库进行标定，不方便在下位机中直接调参。因此我改成只用3个参数（起始行yuy_u、平行赛道斜率kk、赛道起始点列数yly_l）进行标定，并且3个参数和物理世界有比较直接的对应关系，调整摄像头角度时可以直接调整kk即可；如果想增加前瞻，就调整yuy_u；如果更改了摄像头高度，调整yly_l。

一. 基本数学模型

透视变换可以用以下的公式表示。设(x,y)(x,y)是原图像中的点，(x′,y′)(x',y')是对应的俯视图中的点（我们先假设这个是存在对应的，实际上有的时候并不存在这个对应）。

再设MM是（正）透视矩阵，有：

M⋅(xyz)=(x′y′z′)M\cdot \begin{pmatrix} x \\ y \\ z \end{pmatrix} =\begin{pmatrix} x' \\ y' \\ z' \end{pmatrix}

一般来说，z=1z=1，这使得三维齐次坐标转为二维平面上的位置，也就是说，有

M⋅(xy1)=(x′′y′′w′)=w′(x′y′1)M\cdot \begin{pmatrix} x \\ y \\ 1 \end{pmatrix} =\begin{pmatrix} x'' \\ y'' \\ w' \end{pmatrix} =w'\begin{pmatrix} x' \\ y' \\ 1 \end{pmatrix}

这个MM是一个3×33\times 3的矩阵：

M=(a11a12a13a21a22a23a31a32a33)M=\begin{pmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{pmatrix}

变换后的坐标(x′,y′)(x',y')需要通过w′w'进行归一化，也就是说要：

x′=a11x+a12y+a13a31x+a32y+a33x'=\frac{a_{11}x+a_{12}y+a_{13}}{a_{31}x+a_{32}y+a_{33}}

y′=a21x+a22y+a23a31x+a32y+a33y'=\frac{a_{21}x+a_{22}y+a_{23}}{a_{31}x+a_{32}y+a_{33}}

二. 道路模型的特殊化

特殊化1：a33=1a_{33}=1

在道路模型中，我们通常直接设置a33=1a_{33}=1，也就是

M=(a11a12a13a21a22a23a31a321)M=\begin{pmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & 1 \end{pmatrix}

这样做是为了数学上便于处理。这样设置后，我们可以直接得到

x′=a11x+a12y+a13a31x+a32y+1x'=\frac{a_{11}x+a_{12}y+a_{13}}{a_{31}x+a_{32}y+1}

y′=a21x+a22y+a23a31x+a32y+1y'=\frac{a_{21}x+a_{22}y+a_{23}}{a_{31}x+a_{32}y+1}

特殊化2：行坐标固定映射

基本的数学模型已经确立了，下面我们要做的是求解这个M矩阵及其逆矩阵。不过在此之前还可以继续进行一些特殊化假设，例如，在畸变较小的相机中，对于同一行坐标xx的像素值，我们可以认为，它们映射过去后，行坐标x′x'也是同样的。这个假设其实是为了简化后面的图像处理（例如，我们可以通过俯视图的行坐标来估计点的距离，不过这个前提是摄像头得摆正）。

在前面我们已经得到了公式

x′=a11x+a12y+a13a31x+a32y+1x'=\frac{a_{11}x+a_{12}y+a_{13}}{a_{31}x+a_{32}y+1}

基于这个假设，我们可以得到x′x'值与yy无关，我们直接可以得到：

{a12=0a32=0\left\{ \begin{array}{l} a_{12}=0 \\ a_{32}=0 \\ \end{array} \right.

（注意，我们这里是以x作为行坐标，如果在一些文献中，以y作为行坐标，那就是a21a_{21}和a31a_{31}等于0了）

带入后得到：

x′=a11x+a13a31x+1x'=\frac{a_{11}x+a_{13}}{a_{31}x+1}

y′=a21x+a22y+a23a31x+1y'=\frac{a_{21}x+a_{22}y+a_{23}}{a_{31}x+1}

特殊化3：梯形斜率对称性

在求解矩阵MM及其逆矩阵前，我们要先进行一个标定。逆透视实际上就是研究正视图中的梯形转为俯视图中的矩形，因此我们首先要得到这个梯形和矩形的分别的四个点坐标及其对应点（起码opencv中是这样做的）。

但是考虑到调车的时候，如果每次调整摄像头位置都需要用坐标点去标定，不太方便，我们可以进一步考虑赛道的特殊情况。

如果认为行坐标是xx，纵坐标是yy，逆透视实际上就是研究正视图中的梯形转为俯视图中的矩形，因此我们首先要得到这个梯形和矩形的分别的四个点坐标及其对应点，但是这个原图中的梯形的两个侧边如果其中一条斜率是kk(dy/dxdy/dx)，那么另一条就可以表示为−k-k，同时两个侧边延长后必交于图形的纵向中线，也就是y=midy=mid这一列。y=midy=mid和斜向线的交点的行坐标记为xmx_m。对应的，逆透视后的结果中两条侧边应该成为k′=0k'=0（是纵向直线了，dy=0dy=0了），相对应的xm′x_m'应该成为无穷。

很容易从公式x′=a11x+a13a31x+1x'=\frac{a_{11}x+a_{13}}{a_{31}x+1}看出，分母为0的时候，xm′x_m'为无穷

xm=−1a31x_m=-\frac{1}{a_{31}}

按照这个思路输入，那么标定时需要的参数就相对直观且易于调整了，我们只需要如下参数：

kk 两条赛道（梯形侧边线）的斜率，这里假设是大于0的。 自标定
yly_l 左边线与原图片下边界的交点的列坐标。 （左边线有可能与下边界没有交点，为了方便我们假设这个交点存在。如果这个交点不存在，那说明摄像头稍微有点太低了，可以把摄像头位置调高一点，或者把假设的下边界调高一点） 自标定
HH 原图下边界的行坐标，也就是原图的高度。 120
CC 原图的宽度，也就是列坐标最大值+1。要引入这个值，是因为y_r（右边线与图片下边界的交点的列坐标），满足y_r=C-y_l-1。 188
WW 目标结果图的高度和宽度。为了方便我一般假设目标结果图是正方形，这样高度和宽度就一致了。如果不是正方形就稍微按照这个思路加一个参数就行。一般我设置成120px。 120
WRW_R 目标结果图中的赛道宽度，一般来讲我设成40px或者45px，因为赛道宽度是45cm，减去两侧黑胶带是40cm，这样方便换算。 40
yuy_u 目标结果图中最远处在原图中的横坐标，这个点不能高于灭点。（一般来讲灭点是高于水平中线的，也就是说小于最大行坐标的一半，一般来说我会象征性地设成灭点+15px） 自标定

可以看出，在调整摄像头位置和角度的时候，我们只需要调整kk、yly_l和yuy_u这3个参数即可，其余都是硬件和算法固有参数，设好了可以不动。比起普通的用四个坐标xy映射前后对应（总共16个参数）来标定，这样标定在调试的时候更快。

（注意，yly_l是列坐标，yuy_u是行坐标，我写的有点乱，不过意思应该没错。）

三. 逆透视矩阵参数标定

如下图，通过这样写，直接写好程序，调试时就可以比较方便地不通过上位机、在下位机上更改逆透视参数了。

pkIiKyt.png

下位机程序中，只需要调整这三个参数即可修改逆透视参数。

1
2
3
4
/* 设置的参数 */
float IPM_ROAD_K; // 标定的赛道斜率 j=ki+b 【这个值一般越大，摄像头角度越低，越接近于完全正对地面】
int IPM_ROAD_L_START; // 赛道开始坐标(IMG_ROWS-1, START) 【这个和摄像头高度有关，没有调高度，这个应该不用调的】
int IPM_FAREST_ROW; // 逆透视最远行在原图的行坐标

通过这两个参数和算法预先设置的参数，可以算出矩阵MM的所有参数。下面是我的一种计算方法，opencv库getPerspectiveTransform函数的写法则是另一种适应更多情况的解法，我的方法是由智能车中特殊化的条件，简化了一些部分。

首先从前面（特殊化1和特殊化2）我们已经推导得到，a12=a32=0a_{12}=a_{32}=0，a33=1a_{33}=1，只剩下：

M=(a110a13a21a22a23a3101)M=\begin{pmatrix} a_{11} & 0 & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & 0 & 1 \end{pmatrix}

然后前面从梯形斜率对称性（特殊化3）我们推导得到：

a31=−1xma_{31}=-\frac{1}{x_m}

其中xmx_m是斜线与纵向中线的交点，可以通过几何关系由kk和yly_l表达出来。需要注意的是因为xmx_m计算出来的可能小于0，是正常的，但是代码里不能用uint8存这个变量，可以用int或者float。

\begin{align} a_{31} &= -\frac{1}{x_m} \\ &= -\frac{1}{\frac{C-2\cdot y_l}{2k}+H-1} \end{align}

现在来看进一步的标定。有一个问题就是，一般来讲，我们希望俯视图中横纵的每个像素表示的距离是一样的，这样比较方便。我前面已经设定了目标结果图中的赛道宽度是WR=40pxW_R=40px，也就是横向上1px约等于1cm，那我也希望在纵向上也实现这样的比例。实现这个可以直接在地上测量标定，但是我想试一下，能不能不通过二次标定实现这个结果？

（这个问题我还没有解决，目前还在用物理标定来解决，汗流浃背了。不过我感觉是可以根据灭点距离水平中线的比例尺结合相机焦距来算的，因为相机平放正对前方的时候，灭点一般处于水平中线上。）

先不谈这个问题，直接来用预估的目标结果图中最远处在原图中的横坐标yuy_u计算。在前面已经写了，原图和结果图的行坐标是一一对应的，因为我们这里用的相机畸变很小。也就是说：

原图 -> 结果图
yuy_u -> 0
(H-1) -> (W-1)

由于我们前面已经求解得到了a31a_{31}，那么方程x′=a11x+a13a31x+1x'=\frac{a_{11}x+a_{13}}{a_{31}x+1}中就剩下两个未知数a11a_{11}和a13a_{13}了，整理一下，求解这个线性方程组即可：

[yu1H−11][a11a13]=[0(W−1)(a31(H−1)+1)]\begin{bmatrix} y_u & 1 \\ H-1 & 1 \end{bmatrix} \begin{bmatrix} a_{11} \\ a_{13} \end{bmatrix} =\begin{bmatrix} 0 \\ (W-1)(a_{31}(H-1)+1) \end{bmatrix}

对应这种n元方程组，可以跑个高斯消元算法求解，不过我们已经将维度降到2维了，完全没必要。我们将(W−1)(a31(H−1)+1)(W-1)(a_{31}(H-1)+1)设成常数TT，直接给出a11a_{11}和a13a_{13}公式：

{a11=−Tyu−(H−1)a13=yu⋅Tyu−(H−1)\left\{ \begin{array}{l} a_{11}=\frac{-T}{y_u-(H-1)} \\ a_{13}=\frac{y_u\cdot T}{y_u-(H-1)} \\ \end{array} \right.

接下来来看a21a_{21}、a22a_{22}、a23a_{23}的求解，可以列个三元方程解，不过也可以利用一些性质。假设有两个处于同一行、且关于纵向中线对称的两个像素点(x,y1)(x,y_1)、(x,y2)(x,y_2)，它们在原图中和结果图中满足

{y1+y2=C−1y1′+y2′=W−1\left\{ \begin{array}{l} y_1+y_2=C-1 \\ y_1'+y_2'=W-1 \\ \end{array} \right.

带入到公式y′=a21x+a22y+a23a31x+1y'=\frac{a_{21}x+a_{22}y+a_{23}}{a_{31}x+1}中，得到：

W−1=2a21x+a22(C−1)+2a23a31x+1W-1=\frac{2a_{21}x+a_{22}(C-1)+2a_{23}}{a_{31}x+1}

需要注意，因为我们每一行都满足这个公式，也就是说xx为任意值时都成立，进一步得到：

a21=12a31(W−1)a_{21}=\frac{1}{2} a_{31}(W-1)

同时，有

a22(C−1)+2a23=W−1a_{22}(C-1)+2a_{23}=W-1

a22a_{22}和a23a_{23}还需要再加一个方程求解，直接带入原图(H−1,yl)(H-1,y_l)->结果图(W−1,W/2−WR/2)(W-1, W/2-W_R/2)的映射，得到二元方程组：

{(C−1)a22+2a23=W−1yl⋅a22+a23=12(W−WR)(a31(W−1)+1)−a21(W−1)\left\{ \begin{array}{l} (C-1)a_{22}+2a_{23}=W-1 \\ y_l\cdot a_{22}+a_{23}=\frac{1}{2}(W-W_R)(a_{31}(W-1)+1)-a_{21}(W-1) \end{array} \right.

同样和上面一样，设T2=12(W−WR)(a31(W−1)+1)−a21(W−1)T_2=\frac{1}{2}(W-W_R)(a_{31}(W-1)+1)-a_{21}(W-1)，解得

{a22=(W−1)−2⋅T2(C−1)−2⋅yla23=(C−1)⋅T2−(W−1)⋅yl(C−1)−2⋅yl\left\{ \begin{array}{l} a_{22}=\frac{(W-1)-2\cdot T_2}{(C-1)-2\cdot y_l} \\ a_{23}=\frac{(C-1)\cdot T_2-(W-1)\cdot y_l}{(C-1)-2\cdot y_l} \end{array} \right.

这样，我们就完全实现了矩阵的求解。

四. 逆透视结果

下面是一些逆透视后的效果图。

结果图：弯道

pkIi3TS.png

结果图：十字

pkIi1w8.png

五. 代码实现

作为全局变量供上位机设置的参数主要有三个：

1
2
3
4
/* 设置的参数 */
float IPM_ROAD_K;		// 标定的赛道斜率 j=ki+b	【这个值一般越大，摄像头越低】
int IPM_ROAD_L_START;	// 赛道开始坐标(IMG_ROWS-1, START) 【这个和摄像头高度有关，没有调高度，这个应该不用调的】
int IPM_FAREST_ROW;		// 逆透视最远行在原图的行坐标

IPM.h提供给其他文件的接口只需要这几个：

1
2
3
4
5
6
7
void IPM_init(void);		// 逆透视初始化（全局只需要预处理一次，如果重新设置了参数，需要重新调用）
#define ARL_ROW(x,y) aerial_row[x]		// 原图中的(x,y)对应的俯视图行数
#define ARL_COL(x,y) aerial_col[x][y]	// 原图中的(x,y)对应的俯视图列数
#define FNT_ROW(x,y) front_row[x]		// 俯视图中的(x,y)对应的原图行数
#define FNT_COL(x,y) front_col[x][y]	// 俯视图中的(x,y)对应的原图列数
#define isView(x,y)	(0<=x&&x<IPM_WIDTH&&View_lim_L[x]<=y&&y<=View_lim_R[x])	// 俯视图中(x,y)点是否可见
#define View(x,y)	ImageGray[front_row[x]][front_col[x][y]]				// 俯视图中(x,y)点灰度值

其余的固定参数和用到的值：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
/* 固定参数 */
#define IPM_ROAD_WIDTH (40)	// 结果图中的赛道宽度
/* 逆透视矩阵 */
float IPM_MAT[4][4];	// 逆透视矩阵M，其实是3*3，0位不用
float IPM_INV[4][4];	// M的逆矩阵
#define a IPM_MAT
#define b IPM_INV
/* 预处理好的值 */
uint8 aerial_row[IMG_ROWS];	// 原图中第i行对应的俯视图行数
uint8 front_row[IPM_WIDTH];	// 俯视图中第i行对应的原图行数
uint8 aerial_col[IMG_ROWS][IMG_COLS];	// 原图中(i,j)对应的俯视图(u,v)的列数v
uint8 front_col[IPM_WIDTH][IPM_WIDTH];	// 俯视图中(u,v)对应的原图(i,j)的列数j
uint8 View_lim_L[IPM_WIDTH];			// 俯视图中第i行的列数左限制（可以等于）
uint8 View_lim_R[IPM_WIDTH];			// 俯视图中第i行的列数右限制（可以等于）
uint8 ImageIPM[IPM_WIDTH][IPM_WIDTH];	// 逆透视灰度数组

剩下的主要函数的实现如下。

逆透视矩阵计算：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
/**
  * 函数功能: 逆透视矩阵计算
  * 传入参数: 无
  * 传出参数: 无
  * 备注:	 根据k和start，计算矩阵IPM_MAT
  */
void IPM_matrix_calculate(void) {
    static float x_m;
    static float T;
    static float T2;
    /* IPM_MAT 计算 */
    a[1][2] = a[3][2] = 0.0f;
    a[3][3] = 1.0f;
    x_m = IMG_ROWS - 1 + ((IMG_COLS >> 1) - IPM_ROAD_L_START) / (-IPM_ROAD_K);
    a[3][1] = -1.0f / x_m;
    // 下面两个不一定对
    T = (IPM_WIDTH - 1) * (a[3][1] * (IMG_ROWS - 1) + 1);
    a[1][1] = -T / (IPM_FAREST_ROW - IMG_ROWS + 1);
    a[1][3] = IPM_FAREST_ROW * T / (IPM_FAREST_ROW - IMG_ROWS + 1);
    a[2][1] = 0.5f * a[3][1] * (IPM_WIDTH - 1);
    T2 = (IPM_WIDTH - IPM_ROAD_WIDTH) * (a[3][1] * (IPM_WIDTH - 1) + 1.0f) * 0.5f - a[2][1] * (IPM_WIDTH - 1);
    a[2][2] = ((IPM_WIDTH - 1) - 2.0f * T2) / (IMG_COLS - 1 - (IPM_ROAD_L_START << 1));
    a[2][3] = ((IMG_COLS - 1) * T2 - (IPM_WIDTH - 1) * IPM_ROAD_L_START) / (IMG_COLS - 1 - (IPM_ROAD_L_START << 1));
#if ENVIRONMENT_PC
    CString string = "";
    string.Format("MAT \n"); 
    PrintDebug(string);
    for (int i = 1; i <= 3; i++) {
        string.Format("%.3f\t %.3f\t %.3f\t \r \n", a[i][1], a[i][2], a[i][3]);
        PrintDebug(string);
    }
#endif
    /* IPM_INV 计算：是IPM_MAT的逆矩阵 */
    // 计算行列式
    float D = a[1][1] * (a[2][2] * a[3][3] - a[2][3] * a[3][2])
        - a[1][2] * (a[2][1] * a[3][3] - a[2][3] * a[3][1])
        + a[1][3] * (a[2][1] * a[3][2] - a[2][2] * a[3][1]);
    if (D != 0) {
        float invDet = 1.0f / D;
        // 计算逆矩阵
        IPM_INV[1][1] = invDet * (a[2][2] * a[3][3] - a[2][3] * a[3][2]);
        IPM_INV[1][2] = invDet * (a[1][3] * a[3][2] - a[1][2] * a[3][3]);
        IPM_INV[1][3] = invDet * (a[1][2] * a[2][3] - a[1][3] * a[2][2]);
        IPM_INV[2][1] = invDet * (a[2][3] * a[3][1] - a[2][1] * a[3][3]);
        IPM_INV[2][2] = invDet * (a[1][1] * a[3][3] - a[1][3] * a[3][1]);
        IPM_INV[2][3] = invDet * (a[1][3] * a[2][1] - a[1][1] * a[2][3]);
        IPM_INV[3][1] = invDet * (a[2][1] * a[3][2] - a[2][2] * a[3][1]);
        IPM_INV[3][2] = invDet * (a[1][2] * a[3][1] - a[1][1] * a[3][2]);
        IPM_INV[3][3] = invDet * (a[1][1] * a[2][2] - a[1][2] * a[2][1]);
        // IPM_INV[3][3] = 1.0f;
#if ENVIRONMENT_PC
        CString string = "";
        string.Format("INV \n");
        PrintDebug(string);
        for (int i = 1; i <= 3; i++) {
            string.Format("%.3f\t %.3f\t %.3f\t \r \n", IPM_INV[i][1], IPM_INV[i][2], IPM_INV[i][3]);
            PrintDebug(string);
        }
#endif
    }
    else {
#if ENVIRONMENT_PC
        PrintDebug("The matrix is singular and cannot be inverted.\n");
#else
#endif
    }
}

逆透视初始化，即调用上面的矩阵计算，并计算出一些全局调用变量：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
/**
  * 函数功能: 逆透视初始化
  * 传入参数: 无
  * 传出参数: 无
  * 备注:	 全局只需要预处理一次，如果修改了设置参数，需要重新调用
  */
void IPM_init(void) {
#if ENVIRONMENT_PC
#if 1  // 上位机中一些我设置好的参数
    IPM_ROAD_K = 1.043f;
    IPM_ROAD_L_START = 2;
    IPM_FAREST_ROW = 45;
#else
    IPM_ROAD_K = 0.4f;
    IPM_ROAD_L_START = 40;
    IPM_FAREST_ROW = 20;
#endif
#else
    // 下位机环境，读取Flash中存储的上次标定数据，略
#endif
    IPM_matrix_calculate();	// 计算逆透视矩阵
    static float clip = 0.5f;	// 感觉好像加上去后因为误差不对应的情况稍微好一点
    // 预处理参数
    for (int i = 0; i < IMG_ROWS; i++)
        aerial_row[i] = (a[1][1] * i + a[1][3]) / (a[3][1] * i + 1.0f) + clip;	
    for (int i = 0; i < IMG_ROWS; i++) {
        for (int j = 0; j < IMG_COLS; j++) {
            aerial_col[i][j] = (a[2][1] * i + a[2][2] * j + a[2][3]) / (a[3][1] * i + 1.0f) + clip;
        }
    }
    for (int i = 0; i < IPM_WIDTH; i++) {
        front_row[i] = (b[1][1] * i + b[1][3]) / (b[3][1] * i + b[3][3]) + clip;
    }
    static int temp;
    for (int i = 0; i < IPM_WIDTH; i++) {
        View_lim_L[i] = 255;
        View_lim_R[i] = 0;
        for (int j = 0; j < IPM_WIDTH; j++) {
            temp = (b[2][1] * i + b[2][2] * j + b[2][3]) / (b[3][1] * i + b[3][2] * j + b[3][3]) + clip;
            if (0 <= temp && temp < IMG_COLS) {
                front_col[i][j] = temp;
                if (View_lim_L[i] > j)
                    View_lim_L[i] = j;
                if (View_lim_R[i] < j)
                    View_lim_R[i] = j;
            }
            else 
                front_col[i][j] = 255;
        }
    }
}