前言

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

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

在本帖中，筆者1.2.3節從基礎原理講起，最后第4節講這些原理在自標定中的應用。

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

1. 左右手坐標系

1.1 左右手坐標系判定方式

右、左手坐標系

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

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

1.2 旋轉角正、負判定方式

方法一：

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

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

方法二：

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

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

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

本節的內容先當作一個先驗知識，在后面自標定的實際應用中我們可以一起體會一下。

2. 坐標系表征與坐標轉換

2.1 坐標系表征

什么是坐標系的表征

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

?

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

舉個簡單的視覺任務中的頭姿例子：

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

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

2.2坐標轉換

對XOY坐標系下的點（1，0）進行坐標轉換也分為兩種情況，對應兩種不同的物理意義

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

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

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

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

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

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

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

3. 歐拉角與旋轉矩陣

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

下面我們一起來推導一遍。

3.1 什么是歐拉角

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

相機坐標系

繞相機x軸旋轉，得到俯仰角pitch

繞相機y軸旋轉，得到航偏角yaw

繞相機Z軸旋轉，得到滾轉角roll

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

飛機坐標系

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

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

3.2 歐拉角與坐標系表征

我們先思考一個問題：如果相機繞著某一個軸轉動了一個角度，那么我們如何使用數學來表示這個變換呢？本節的示意圖，先看xyz，XYZ兩個三維的相機坐標系，然后再觀察相機坐標系繞某個軸做旋轉。每個三維坐標系都表示相機坐標系，也是右手系。

相機繞z軸旋轉（roll）

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

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

相機繞y軸旋轉(yaw)

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

相機繞x軸旋轉（pitch）

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

3.3 歐拉角與坐標變換

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

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

相機繞z軸旋轉（roll）

圖中點P在XYZ與xyz坐標系下的坐標之間的轉換關系可以表示如下：

相機繞y軸旋轉(yaw)

相機繞x軸旋轉(pitch)

這回懂了吧，網上csdn、某戶、github上經常看見這個矩陣里面的元素有正有負，大家寫的都不一樣。因為這兩種矩陣根本上代表的就不是一個東西，一個是坐標系表征，一個是坐標變換。

第3章結論如下，如果不想推導可以直接記住，經過筆者驗證肯定是對的：

相機坐標系問題中的“歐拉角”“坐標系表征”“坐標系轉換”

4. 相機自標定

4.1 消失點原理

消失點原理

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

4.2 車道線檢測

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

4.3 消失點的計算

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

此時，需要我們用數學方法構造一個argmin|||| ?的問題。

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

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

消失點求解代碼如下：

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

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

4.4 基于消失點標定外參

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

相機模型

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

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

?

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

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

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

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

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

實際上roll、yaw、pitch的六種組合式中，每一種組合式的結果 R 必然有一列是非常簡潔的，而在4.3節中我們根據消失點深度Z是無窮的+消失點圖像坐標+相機內參這三個先驗信息可以計算出 R 矩陣中第三列向量，因此我們在基于消失點計算外參的算法中構造這個 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 相機姿態計算

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

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

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

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

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

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

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

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

則坐標轉換矩陣為：

示意代碼如下：

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

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

	

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

	

	?

	

	代碼如下：
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)?<

	自標定生成鳥瞰圖

	總結

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

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

	ToDo List：

	車道線檢測策略

	嘗試用網絡學習車道線

	全局優化

	編輯：黃飛

	?