AVM環(huán)視系統(tǒng)在自標(biāo)定中的應(yīng)用及原理分析

前言

AVM環(huán)視系統(tǒng)中相機(jī)參數(shù)通常是汽車出廠前在標(biāo)定車間中進(jìn)行的離線階段標(biāo)定。很多供應(yīng)商還提供了不依賴于標(biāo)定車間的汽車自標(biāo)定方法。自標(biāo)定指的是：汽車在馬路上慢速行駛一段路，利用車道線等先驗(yàn)信息標(biāo)定出相機(jī)的外參。

筆者在算法實(shí)現(xiàn)的開始階段沒有從頭推導(dǎo)公式，而是直接使用論文中的結(jié)論公式，導(dǎo)致自標(biāo)定出的外參矩陣與車間標(biāo)定的結(jié)果（車間標(biāo)定結(jié)果當(dāng)作真值）總是存在一個(gè)正負(fù)號(hào)的差別。最終的拼接結(jié)果表現(xiàn)為圖像與預(yù)想的結(jié)果存在反轉(zhuǎn)、旋轉(zhuǎn)等問題。筆者從頭梳理了一下左右手坐標(biāo)系、坐標(biāo)系表征、坐標(biāo)轉(zhuǎn)換、歐拉角等基本原理，發(fā)現(xiàn)網(wǎng)上很多作者對(duì)這部?jī)?nèi)容都是抄來抄去文檔很混亂。在實(shí)現(xiàn)自標(biāo)定算法前，需要把這些都搞清楚。

在本帖中，筆者1.2.3節(jié)從基礎(chǔ)原理講起，最后第4節(jié)講這些原理在自標(biāo)定中的應(yīng)用。

自標(biāo)定相機(jī)外參生成鳥瞰圖

1. 左右手坐標(biāo)系

1.1 左右手坐標(biāo)系判定方式

右、左手坐標(biāo)系

右手坐標(biāo)系：右手大拇指指向Z軸，其余四指握向x->y（90°而不是270°）

左手坐標(biāo)系：左手大拇指指向Z軸，其余四指握向x->y（90°而不是270°）

1.2 旋轉(zhuǎn)角正、負(fù)判定方式

方法一：

右手坐標(biāo)系：從旋轉(zhuǎn)軸上方看下去，逆時(shí)針方向?yàn)樾D(zhuǎn)正方向

左手坐標(biāo)系：從旋轉(zhuǎn)軸上方看下去，順時(shí)針方向?yàn)樾D(zhuǎn)正方向

方法二：

右手坐標(biāo)系：右手大拇指指向旋轉(zhuǎn)軸正方向，四指握向的方向?yàn)檎较颍?0°而不是270°）

左手坐標(biāo)系：左手大拇指指向旋轉(zhuǎn)軸正方向，四指握向的方向?yàn)檎较颍?0°而不是270°）

方法二中四指握向的方向始終是x->y->z->x，即角度為90°，而不是270°。

本節(jié)的內(nèi)容先當(dāng)作一個(gè)先驗(yàn)知識(shí)，在后面自標(biāo)定的實(shí)際應(yīng)用中我們可以一起體會(huì)一下。

2. 坐標(biāo)系表征與坐標(biāo)轉(zhuǎn)換

2.1 坐標(biāo)系表征

什么是坐標(biāo)系的表征

“坐標(biāo)系表征”表達(dá)的物理意義用通俗的話解釋就是：在A坐標(biāo)系下觀察B坐標(biāo)系時(shí)，所看到的B坐標(biāo)系的X、Y軸的方向。再通俗一點(diǎn)舉個(gè)簡(jiǎn)單的例子：我們觀察別人頭部的時(shí)候，可以很容易知道這個(gè)人在看哪里，這就是因?yàn)槲覀儷@取到了對(duì)方人頭坐標(biāo)系的x、y、z軸在我們自己坐標(biāo)系下的朝向。

?

可以理解為在XOY坐標(biāo)系下觀察xoy坐標(biāo)系的坐標(biāo)軸方向。

舉個(gè)簡(jiǎn)單的視覺任務(wù)中的頭姿例子：

如圖所示，紅色的XYZ坐標(biāo)系為標(biāo)準(zhǔn)人頭坐標(biāo)系，即人頭沒有任何角度正朝前方的姿態(tài)。黃色的xyz為人頭有一些旋轉(zhuǎn)時(shí)當(dāng)前姿態(tài)的坐標(biāo)系。“頭姿”指的就是在XYZ坐標(biāo)系下xyz坐標(biāo)系的表征，即在標(biāo)準(zhǔn)XYZ坐標(biāo)系下觀察當(dāng)前人頭的姿態(tài)：

其中?,,?分別表示XYZ坐標(biāo)系下x軸、z軸、y軸方向的單位向量。這就叫做在XYZ坐標(biāo)系下xyz坐標(biāo)系的表征。

2.2坐標(biāo)轉(zhuǎn)換

對(duì)XOY坐標(biāo)系下的點(diǎn)（1，0）進(jìn)行坐標(biāo)轉(zhuǎn)換也分為兩種情況，對(duì)應(yīng)兩種不同的物理意義

使用2.1中的坐標(biāo)系表征矩陣

這種情況對(duì)應(yīng)的物理意義為：XOY坐標(biāo)系下的點(diǎn)（1，0）跟隨坐標(biāo)軸做了??旋轉(zhuǎn)后，在原XOY坐標(biāo)系下的坐標(biāo)：

顯然XOY坐標(biāo)系下的點(diǎn)（1，0）跟隨坐標(biāo)軸做了??旋轉(zhuǎn)后，在原XOY坐標(biāo)系下的坐標(biāo)為 (cos,sin)

使用2.1中的坐標(biāo)系表征矩陣的逆

這種情況對(duì)應(yīng)的物理意義為：XOY坐標(biāo)系下的點(diǎn)（1，0）在xoy坐標(biāo)系下的坐標(biāo)：

在我們通常說的相機(jī)坐標(biāo)轉(zhuǎn)換中，更多的用到的是這第二種，下式中??表示kinect相機(jī)到ir相機(jī)的坐標(biāo)轉(zhuǎn)換。

因此我們可以得出一個(gè)結(jié)論：我們通常說的相機(jī)坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換矩陣 R ，實(shí)際上是相機(jī)坐標(biāo)系表征矩陣的逆。即對(duì)于如圖AB這兩個(gè)相機(jī)，將某點(diǎn) P 的坐標(biāo)從A坐標(biāo)系轉(zhuǎn)換到B坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣，實(shí)際上等于A相機(jī)坐標(biāo)系下B相機(jī)坐標(biāo)系的表征矩陣的逆。這個(gè)結(jié)論對(duì)于各種視覺任務(wù)的理解非常重要。后面我們?cè)僬f“坐標(biāo)轉(zhuǎn)換”說的就是這第二種情況。

3. 歐拉角與旋轉(zhuǎn)矩陣

筆者在根據(jù)github項(xiàng)目[1]實(shí)現(xiàn)基于消失點(diǎn)的自標(biāo)定算法中計(jì)算坐標(biāo)轉(zhuǎn)換矩陣的時(shí)候，總是差一個(gè)正負(fù)號(hào)。當(dāng)時(shí)就覺得大概率是因?yàn)闅W拉角與旋轉(zhuǎn)矩陣的關(guān)系沒搞懂，而且筆者發(fā)現(xiàn)csdn、某乎甚至github上的老哥都是抄來抄去，給出的公式經(jīng)常會(huì)相差一個(gè)正負(fù)號(hào)，估計(jì)大部分人都沒搞懂最基礎(chǔ)的原理是怎么回事。于是筆者從頭推導(dǎo)了一遍，終于搞清楚這個(gè)東西是怎么用的。

下面我們一起來推導(dǎo)一遍。

3.1 什么是歐拉角

歐拉角是坐標(biāo)系旋轉(zhuǎn)的表示，針對(duì)于相機(jī)坐標(biāo)系可定義為（相機(jī)坐標(biāo)系如下）：

相機(jī)坐標(biāo)系

繞相機(jī)x軸旋轉(zhuǎn)，得到俯仰角pitch

繞相機(jī)y軸旋轉(zhuǎn)，得到航偏角yaw

繞相機(jī)Z軸旋轉(zhuǎn)，得到滾轉(zhuǎn)角roll

注意：這樣定義的pitch、yaw、roll（x軸pitch，y軸yaw，z軸roll）只能適用于這種相機(jī)坐標(biāo)系，對(duì)于其他坐標(biāo)系，例如飛機(jī)坐標(biāo)系，那pitch、yaw和roll表示的含義就不同了，例如下圖（引用自一文詳解四元數(shù)、歐拉角、旋轉(zhuǎn)矩陣、軸角如何相互轉(zhuǎn)換 (qq.com)）：

飛機(jī)坐標(biāo)系

當(dāng)然，我們只討論相機(jī)坐標(biāo)系的情況，別的不考慮。相機(jī)坐標(biāo)系也是一種右手系。其中z軸為相機(jī)光軸，朝向相機(jī)正前方；x軸朝向相機(jī)右側(cè)；y軸朝向正下方。因此繞x軸旋轉(zhuǎn)為俯仰角，繞y軸旋轉(zhuǎn)為航偏角，繞z軸旋轉(zhuǎn)為滾轉(zhuǎn)角。

我們已經(jīng)了解歐拉角在相機(jī)坐標(biāo)系中的物理含義，知道了roll、yaw、pitch分別表示什么含義，那么如何通過歐拉角來表示兩個(gè)不同的坐標(biāo)系之間的關(guān)系呢？繼續(xù)往下看。

3.2 歐拉角與坐標(biāo)系表征

我們先思考一個(gè)問題：如果相機(jī)繞著某一個(gè)軸轉(zhuǎn)動(dòng)了一個(gè)角度，那么我們?nèi)绾问褂脭?shù)學(xué)來表示這個(gè)變換呢？本節(jié)的示意圖，先看xyz，XYZ兩個(gè)三維的相機(jī)坐標(biāo)系，然后再觀察相機(jī)坐標(biāo)系繞某個(gè)軸做旋轉(zhuǎn)。每個(gè)三維坐標(biāo)系都表示相機(jī)坐標(biāo)系，也是右手系。

相機(jī)繞z軸旋轉(zhuǎn)（roll）

相機(jī)繞Z軸旋轉(zhuǎn)與2.1節(jié)很像，為了更好地推導(dǎo)數(shù)學(xué)公式，我們先從上圖這個(gè)角度來看相機(jī)坐標(biāo)系。圖中Z軸方向?yàn)榇怪庇赬Y平面朝外，先腦補(bǔ)一下這個(gè)坐標(biāo)系是不是和前面3.1中的坐標(biāo)系是一模一樣的（肯定是一樣的啊，只不過從哪個(gè)角度來觀察相機(jī)坐標(biāo)系是不同的），XYZ相機(jī)坐標(biāo)系繞找Z軸旋轉(zhuǎn)了 roll 角度轉(zhuǎn)換到了xyz坐標(biāo)系，根據(jù)第1章的內(nèi)容不難知道相機(jī)坐標(biāo)系是一種右手系，且根據(jù)1.2中的結(jié)論可知上圖中的旋轉(zhuǎn)角 roll 為正。

進(jìn)一步地，根據(jù)2.1節(jié)中坐標(biāo)系表征的結(jié)論，在XYZ坐標(biāo)系下xoy坐標(biāo)系的表征如下，式中 r 表示roll角

相機(jī)繞y軸旋轉(zhuǎn)(yaw)

上圖依然是從某個(gè)角度看的相機(jī)坐標(biāo)系，其中Y軸為垂直于XOZ平面朝里。根據(jù)右手坐標(biāo)系定理可知圖中相機(jī)坐標(biāo)系XYZ繞著Y軸旋轉(zhuǎn)到xyz坐標(biāo)系，這個(gè)方向的旋轉(zhuǎn)角 yaw 為正。進(jìn)一步地，在原始XYZ相機(jī)坐標(biāo)系下觀察旋轉(zhuǎn)后的xyz坐標(biāo)系的表征如下，其中 y 表示yaw角

相機(jī)繞x軸旋轉(zhuǎn)（pitch）

上圖中相機(jī)坐標(biāo)系XYZ繞X軸旋轉(zhuǎn)pitch角，其中X軸為垂直與YOZ平面朝外，按照右手原則上圖中的pitch為正。進(jìn)一步地，在原始XYZ坐標(biāo)系下觀察xyz坐標(biāo)系的表征如下，其中 p 表示pitch角。

3.3 歐拉角與坐標(biāo)變換

上一節(jié)中討論的是歐拉角與坐標(biāo)系表征之間關(guān)系的問題，更多地是用在頭姿任務(wù)中。在很多視覺任務(wù)中，我們需要知道的是：當(dāng)坐標(biāo)系做了某種旋轉(zhuǎn)之后，同一個(gè)點(diǎn)在前后兩個(gè)坐標(biāo)系下的映射關(guān)系。

第2章中我們得到了結(jié)論：我們通常說的相機(jī)坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換矩陣 R ，實(shí)際上是相機(jī)坐標(biāo)系表征矩陣的逆

相機(jī)繞z軸旋轉(zhuǎn)（roll）

圖中點(diǎn)P在XYZ與xyz坐標(biāo)系下的坐標(biāo)之間的轉(zhuǎn)換關(guān)系可以表示如下：

相機(jī)繞y軸旋轉(zhuǎn)(yaw)

相機(jī)繞x軸旋轉(zhuǎn)(pitch)

這回懂了吧，網(wǎng)上csdn、某戶、github上經(jīng)常看見這個(gè)矩陣?yán)锩娴脑赜姓胸?fù)，大家寫的都不一樣。因?yàn)?strong>這兩種矩陣根本上代表的就不是一個(gè)東西，一個(gè)是坐標(biāo)系表征，一個(gè)是坐標(biāo)變換。

第3章結(jié)論如下，如果不想推導(dǎo)可以直接記住，經(jīng)過筆者驗(yàn)證肯定是對(duì)的：

相機(jī)坐標(biāo)系問題中的“歐拉角”“坐標(biāo)系表征”“坐標(biāo)系轉(zhuǎn)換”

4. 相機(jī)自標(biāo)定

4.1 消失點(diǎn)原理

消失點(diǎn)原理

上圖中 O 為相機(jī)光心，Image plane為相機(jī)成像平面。地上的兩條平行線為汽車行駛在路上時(shí)的平行車道線，根據(jù)相機(jī)透視投影模型不難發(fā)現(xiàn)：三維相機(jī)坐標(biāo)系下的平行車道線投影到相機(jī)的二維成像平面上會(huì)交于一點(diǎn)，這一點(diǎn)我們稱之為消失點(diǎn)，它對(duì)應(yīng)的是三維相機(jī)坐標(biāo)系下平行車道線的無窮遠(yuǎn)點(diǎn)。

4.2 車道線檢測(cè)

本篇不想講關(guān)于車道線檢測(cè)的內(nèi)容，直接看結(jié)果吧（帖子寫的太辛苦了，后面補(bǔ)吧。。。）：

4.3 消失點(diǎn)的計(jì)算

檢測(cè)到車道線后，需要計(jì)算消失點(diǎn)。實(shí)際上我們會(huì)檢測(cè)到很多的直線，但是由于噪聲原因，它們不會(huì)交于一點(diǎn)。因此這是一個(gè)求解超定方程的問題，見筆者另一篇文章[2]中的1.3和1.4。

此時(shí)，需要我們用數(shù)學(xué)方法構(gòu)造一個(gè)argmin|||| ?的問題。

對(duì)于圖中的每一條車道線，我們都計(jì)算出了它對(duì)應(yīng)的方程：x+y+=0 ?，其中 (x,y)為消失點(diǎn)，?,,為車道線的直線方程參數(shù)，我們需要計(jì)算出滿足所有直線方程的消失點(diǎn) (x,y) 。因此問題轉(zhuǎn)換為：

由于噪聲的存在，上述理想化等式不可能存在，因此進(jìn)一步地轉(zhuǎn)換為：，可以通過對(duì)矩陣??A進(jìn)行特征分解，最小特征值對(duì)應(yīng)的特征向量就是這個(gè)問題的解，也就是消失點(diǎn)坐標(biāo)。

消失點(diǎn)求解代碼如下：

void?RoadCalibrate::VanishPointsFitting(const?std::vector&?line_params,
????????????????????????????????cv::Point2d&?pts)?{
??if?(int(line_params.size())?(i,?0)?=?line_params[i].x;
????A.at(i,?1)?=?line_params[i].y;
????A.at(i,?2)?=?line_params[i].z;
??}
??cv::Mat?A_T;
??cv::transpose(A,?A_T);
??cv::Mat?At_A?=?A_T?*?(A);

??cv::Mat?eigenvalues;
??cv::Mat?eigenvectors;

??cv::eigen(At_A,?eigenvalues,?eigenvectors);
??pts.x?=?eigenvectors.at(2,?0)?/?eigenvectors.at(2,?2);
??pts.y?=?eigenvectors.at(2,?1)?/?eigenvectors.at(2,?2);
}

opencv中調(diào)用eigen可以進(jìn)行特征分解，特征值大小從大到小排列，特征向量是一行一行寫入opencv的mat中的，即mat矩陣的第三行為解，拿到第三行向量做歸一化即可。

至于為什么要做歸一化，這個(gè)尺度是怎么來的，可以參見筆者另一篇知乎文章：?jiǎn)螒?yīng)矩陣+相機(jī)標(biāo)定+ICP—計(jì)算機(jī)視覺中的數(shù)學(xué)方法 - 知乎 (zhihu.com)。截取其中一段話，簡(jiǎn)單來講就是我們計(jì)算出來的解 (x,y,)是包含一個(gè)尺度在里面的，而在二維平面中這個(gè)??肯定是1，因此不管算出來的解是啥，最后做歸一化就可以得到消失點(diǎn)的二維像素坐標(biāo)了。

4.4 基于消失點(diǎn)標(biāo)定外參

基于消失點(diǎn)標(biāo)定外參的方法是通過相機(jī)模型推導(dǎo)的，式中 (X,Y,Z,1)為道路坐標(biāo)系下某一點(diǎn)的齊次坐標(biāo)，通過外參 (R,t)轉(zhuǎn)換到相機(jī)坐標(biāo)系，然后通過相機(jī)內(nèi)參 K 轉(zhuǎn)移到圖像坐標(biāo)系下。我們要做的就是計(jì)算外參 R 。

相機(jī)模型

在我們AVM自標(biāo)定的應(yīng)用中，要標(biāo)定的相機(jī)外參主要是俯仰角pitch，一般情況下yaw和roll很小。道路坐標(biāo)系可以理解為朝向正前方，而我們的相機(jī)坐標(biāo)系有一個(gè)俯仰角pitch，以一定角度向下傾斜。

我們已經(jīng)知道消失點(diǎn)對(duì)應(yīng)的三維坐標(biāo)點(diǎn)在無窮遠(yuǎn)處，即 Z 為正無窮，且根據(jù)4.2中消失點(diǎn)計(jì)算的方法可以求解出消失點(diǎn)的二維坐標(biāo)。因此我們可以通過上述相機(jī)模型+消失點(diǎn)二維圖像坐標(biāo)+消失點(diǎn)三維坐標(biāo)的Z為無窮大這些先驗(yàn)構(gòu)建起約束關(guān)系，推導(dǎo)如下：

?

那么問題來了，我們得到了旋轉(zhuǎn)矩陣的最后一列 r 有什么用呢？這就又要使用到第3章中的歐拉角知識(shí)。首先我們要確定，當(dāng)前的問題是坐標(biāo)轉(zhuǎn)換（求解將某點(diǎn)的坐標(biāo)從道路坐標(biāo)系轉(zhuǎn)換到相機(jī)坐標(biāo)系的 R ），而不是坐標(biāo)系表征，因此用到的是第三章表格中的第二行矩陣（這一點(diǎn)必須要明確，否則在算法實(shí)現(xiàn)的時(shí)候總是差一個(gè)正負(fù)號(hào)，或者你看到最終投影的結(jié)果反轉(zhuǎn)之類的，這也是本貼為什么花了前三個(gè)章節(jié)分別講了左右手坐標(biāo)系、坐標(biāo)系轉(zhuǎn)換、坐標(biāo)系表征、歐拉角這些東西）。

相機(jī)坐標(biāo)系與道路坐標(biāo)系的轉(zhuǎn)換關(guān)系可以理解成相機(jī)坐標(biāo)系先繞x軸旋轉(zhuǎn)某個(gè)pitch角度，再繞y軸旋轉(zhuǎn)某個(gè)yaw角度，最后繞z軸旋轉(zhuǎn)某個(gè)roll角度；也可以理解成先繞x軸旋轉(zhuǎn)某個(gè)pitch角，再繞z軸旋轉(zhuǎn)某個(gè)roll角，最后繞y軸旋轉(zhuǎn)......。一共有?=321=6 中不同的順序組合。假設(shè)我們確定了坐標(biāo)轉(zhuǎn)換矩陣 R ，那么可以求解出6中不同的歐拉角，就是因?yàn)樾D(zhuǎn)軸的組合順序不同。

實(shí)際上我們?cè)诨谙c(diǎn)進(jìn)行相機(jī)外參標(biāo)定時(shí)用的時(shí)如下組合：

上式的物理意義是：相機(jī)先繞Z軸旋轉(zhuǎn)roll翻滾角，然后繞X旋轉(zhuǎn)pitch俯仰角，最后繞Y旋轉(zhuǎn)航偏角，需要注意的是坐標(biāo)轉(zhuǎn)換矩陣的連乘順序?yàn)?strong>左乘。即假設(shè)??將點(diǎn) p 的坐標(biāo)從a坐標(biāo)系轉(zhuǎn)換到b坐標(biāo)系得到??（即?=p ），??將點(diǎn)??的坐標(biāo)從b坐標(biāo)系轉(zhuǎn)換到c坐標(biāo)系（即?=），那么點(diǎn) p 從a坐標(biāo)系轉(zhuǎn)換到c坐標(biāo)系可表示為：

進(jìn)一步地，可以得到道路坐標(biāo)系與相機(jī)坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換關(guān)系，從下式中可以看到第三列向量非常簡(jiǎn)潔，而由于我們前面已經(jīng)算出了第三列??，因此可以通過反三角函數(shù)直接計(jì)算出pitch角和yaw角。

實(shí)際上roll、yaw、pitch的六種組合式中，每一種組合式的結(jié)果 R 必然有一列是非常簡(jiǎn)潔的，而在4.3節(jié)中我們根據(jù)消失點(diǎn)深度Z是無窮的+消失點(diǎn)圖像坐標(biāo)+相機(jī)內(nèi)參這三個(gè)先驗(yàn)信息可以計(jì)算出 R 矩陣中第三列向量，因此我們?cè)诨谙c(diǎn)計(jì)算外參的算法中構(gòu)造這個(gè) R 的時(shí)候，選取的是如上式的這種組合方式。

yaw pitch的計(jì)算：

void?RoadCalibrate::ExtrinsicFitting(const?cv::Point2d&?pts,?float&?yaw,
?????????????????????????????????????float&?pitch)?{
??//?vanish
??cv::Mat?vanish_point?=?(cv::Mat_(3,?1)?<(1,?0));????????????????????
??yaw?=?atan(-r3.at(0,?0)?/?r3.at(2,?0));
}

4.5 相機(jī)姿態(tài)計(jì)算

以前置攝像頭為例，小車坐標(biāo)系、道路坐標(biāo)系、相機(jī)坐標(biāo)系之間的關(guān)系如下：

這個(gè)碗模型的中心就是汽車中心，中間的長(zhǎng)方形區(qū)域就是汽車位置。

汽車坐標(biāo)系：坐標(biāo)原點(diǎn)在汽車中心（地面上），z軸垂直地面向上，y軸為汽車正前方，x軸為右手方向。這個(gè)是AVM 3D算法中設(shè)置的，見引用自動(dòng)駕駛——自動(dòng)泊車之AVM環(huán)視系統(tǒng)算法2 - 知乎 (zhihu.com)。

道路坐標(biāo)系為：原點(diǎn)在汽車坐標(biāo)系y軸延長(zhǎng)線的地面上某一點(diǎn)，z軸為汽車行駛方向，y軸為垂直于地面朝下，x軸為右手方向。其實(shí)就是正朝向汽車行駛方向的虛擬相機(jī)坐標(biāo)系。

相機(jī)坐標(biāo)系：坐標(biāo)系原點(diǎn)在汽車車頭某位置，與道路坐標(biāo)系相比存在繞x軸的俯仰角pitch。相比之下繞y軸的yaw角和繞z軸的roll角較小。

下圖為放大后的坐標(biāo)系示意圖：

如圖上圖所示，道路坐標(biāo)系與汽車標(biāo)系之間的關(guān)系非常簡(jiǎn)單，它們都是有兩個(gè)軸平行于地面，一個(gè)軸垂直于地面，因此很容易用第2章中的坐標(biāo)系表征方法來相互表示。

我們想得到的是car坐標(biāo)系->road坐標(biāo)系的坐標(biāo)轉(zhuǎn)換關(guān)系，因此我們只需要先計(jì)算car坐標(biāo)系下road坐標(biāo)系的表征，然后對(duì)矩陣求逆即可。顯然，在car坐標(biāo)系下road坐標(biāo)系的表征為：

則坐標(biāo)轉(zhuǎn)換矩陣為：

示意代碼如下：

/*******************************************************************************************************/
void?RoadCalibrate::Mat?&coor_car2cam?)?{
????cv::Mat?car_look_road?=?(cv::Mat_(3,?3)?<

	來直接看下基于消失點(diǎn)自標(biāo)定與在標(biāo)定車間通過solvePnP標(biāo)定的差異：

	

	從標(biāo)定的外參結(jié)果上來看大差不差，接下來我們可視化看下生成的鳥瞰圖如何。

	

	?

	

	代碼如下：
cv::Mat?RoadCalibrate::GenerateBev(float?yaw,?float?pitch,?const?cv::Mat?&gray)?{

??//coordinate:?road->camera
??float?roll?=?0;
??cv::Mat?rotation_yaw?=?(cv::Mat_(3,?3)?<(3,?3)?<(3,?3)?<road
??cv::Mat?coorR_cam2road;
??cv::invert(coorR_road2cam,?coorR_cam2road,?cv::DECOMP_LU);

??//road->bev
??cv::Mat?coorR_road2bev?=?(cv::Mat_(3,?3)?<

	自標(biāo)定生成鳥瞰圖

	總結(jié)

	最近的工作中越來越意識(shí)到算法底層原理的重要性，就算是調(diào)參也要弄清楚算法的邏輯，更不用說像自標(biāo)定這種沒有現(xiàn)成的API需要自己一點(diǎn)一點(diǎn)堆砌的東西。

	本文中基于消失點(diǎn)的自標(biāo)定只是一個(gè)糙版demo，目前還只能標(biāo)定出yaw和pitch默認(rèn)roll是0，外參矩陣中的t也是使用標(biāo)定車間的標(biāo)定結(jié)果，還有很多東西需要完善。比如車道線檢測(cè)中的保護(hù)邏輯，標(biāo)定出前后相機(jī)外參后需要在全局做優(yōu)化等等。最后推薦一篇論文：《Automatic Calibration of an Around View Monitor System Exploiting Lane Markings》有些思路可以借鑒一下。

	ToDo List：

	車道線檢測(cè)策略

	嘗試用網(wǎng)絡(luò)學(xué)習(xí)車道線

	全局優(yōu)化

	編輯：黃飛

	?