前言

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

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

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

自標定相機外參生成鳥瞰圖

1. 左右手坐標系

1.1 左右手坐標系判定方式

右、左手坐標系

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

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

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

方法一：

右手坐標系：從旋轉(zhuǎn)軸上方看下去，逆時針方向為旋轉(zhuǎn)正方向

左手坐標系：從旋轉(zhuǎn)軸上方看下去，順時針方向為旋轉(zhuǎn)正方向

方法二：

右手坐標系：右手大拇指指向旋轉(zhuǎn)軸正方向，四指握向的方向為正方向（90°而不是270°）

左手坐標系：左手大拇指指向旋轉(zhuǎn)軸正方向，四指握向的方向為正方向（90°而不是270°）

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

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

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

2.1 坐標系表征

什么是坐標系的表征

“坐標系表征”表達的物理意義用通俗的話解釋就是：在A坐標系下觀察B坐標系時，所看到的B坐標系的X、Y軸的方向。再通俗一點舉個簡單的例子：我們觀察別人頭部的時候，可以很容易知道這個人在看哪里，這就是因為我們獲取到了對方人頭坐標系的x、y、z軸在我們自己坐標系下的朝向。

?

可以理解為在XOY坐標系下觀察xoy坐標系的坐標軸方向。

舉個簡單的視覺任務(wù)中的頭姿例子：

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

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

2.2坐標轉(zhuǎn)換

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

使用2.1中的坐標系表征矩陣

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

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

使用2.1中的坐標系表征矩陣的逆

這種情況對應(yīng)的物理意義為：XOY坐標系下的點（1，0）在xoy坐標系下的坐標：

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

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

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

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

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

3.1 什么是歐拉角

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

相機坐標系

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

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

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

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

飛機坐標系

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

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

3.2 歐拉角與坐標系表征

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

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

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

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

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

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

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

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

3.3 歐拉角與坐標變換

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

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

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

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

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

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

這回懂了吧，網(wǎng)上csdn、某戶、github上經(jīng)?？匆娺@個矩陣里面的元素有正有負，大家寫的都不一樣。因為這兩種矩陣根本上代表的就不是一個東西，一個是坐標系表征，一個是坐標變換。

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

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

4. 相機自標定

4.1 消失點原理

消失點原理

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

4.2 車道線檢測

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

4.3 消失點的計算

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

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

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

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

消失點求解代碼如下：

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可以進行特征分解，特征值大小從大到小排列，特征向量是一行一行寫入opencv的mat中的，即mat矩陣的第三行為解，拿到第三行向量做歸一化即可。

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

4.4 基于消失點標定外參

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

相機模型

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

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

?

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

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

實際上我們在基于消失點進行相機外參標定時用的時如下組合：

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

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

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

yaw pitch的計算：

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 相機姿態(tài)計算

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

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

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

道路坐標系為：原點在汽車坐標系y軸延長線的地面上某一點，z軸為汽車行駛方向，y軸為垂直于地面朝下，x軸為右手方向。其實就是正朝向汽車行駛方向的虛擬相機坐標系。

相機坐標系：坐標系原點在汽車車頭某位置，與道路坐標系相比存在繞x軸的俯仰角pitch。相比之下繞y軸的yaw角和繞z軸的roll角較小。

下圖為放大后的坐標系示意圖：

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

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

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

示意代碼如下：

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

	來直接看下基于消失點自標定與在標定車間通過solvePnP標定的差異：

	

	從標定的外參結(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)?<

	自標定生成鳥瞰圖

	總結(jié)

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

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

	ToDo List：

	車道線檢測策略

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

	全局優(yōu)化

	編輯：黃飛

	?