/ 導(dǎo)讀 /

傳感器標(biāo)定是自動(dòng)駕駛的基本需求，一個(gè)車上裝了多個(gè)/多種傳感器，而它們之間的坐標(biāo)關(guān)系是需要確定的。灣區(qū)自動(dòng)駕駛創(chuàng)業(yè)公司ZooX的co-founder和CTO是Sebastia Thrun的學(xué)生Jesse Levinson，他的博士論文就是傳感器標(biāo)定。
這個(gè)工作可分成兩部分：內(nèi)參標(biāo)定和外參標(biāo)定，內(nèi)參是決定傳感器內(nèi)部的映射關(guān)系，比如攝像頭的焦距，偏心和像素橫縱比（+畸變系數(shù)），而外參是決定傳感器和外部某個(gè)坐標(biāo)系的轉(zhuǎn)換關(guān)系，比如姿態(tài)參數(shù)（旋轉(zhuǎn)和平移6自由度）。
攝像頭的標(biāo)定曾經(jīng)是計(jì)算機(jī)視覺中3-D重建的前提，張正友老師著名的的Zhang氏標(biāo)定法，利用Absolute Conic不變性得到的平面標(biāo)定算法簡化了控制場。
這里重點(diǎn)是，討論不同傳感器之間的外參標(biāo)定，特別是激光雷達(dá)和攝像頭之間的標(biāo)定。
另外在自動(dòng)駕駛研發(fā)中，GPS/IMU和攝像頭或者激光雷達(dá)的標(biāo)定，雷達(dá)和攝像頭之間的標(biāo)定也是常見的。不同傳感器之間標(biāo)定最大的問題是如何衡量最佳，因?yàn)?/span>獲取的數(shù)據(jù)類型不一樣：

1. 攝像頭是RGB圖像的像素陣列；

2. 激光雷達(dá)是3-D點(diǎn)云距離信息（有可能帶反射值的灰度值）；

3. GPS-IMU給的是車身位置姿態(tài)信息；

4. 雷達(dá)是2-D反射圖。

這樣的話，實(shí)現(xiàn)標(biāo)定誤差最小化的目標(biāo)函數(shù)會(huì)因?yàn)椴煌瑐鞲衅髋鋵?duì)而不同。
另外，標(biāo)定方法分targetless和target兩種，前者在自然環(huán)境中進(jìn)行，約束條件少，不需要用專門的target；后者則需要專門的控制場，有g(shù)round truth的target，比如典型的棋盤格平面板。
這里僅限于targetless方法的討論，依次給出標(biāo)定的若干算法。

首先是手-眼標(biāo)定

這是一個(gè)被標(biāo)定方法普遍研究的，一定約束條件下的問題：可以廣義的理解，一個(gè)“手”（比如GPS/IMU）和一個(gè)“眼”（激光雷達(dá)/攝像頭）都固定在一個(gè)機(jī)器上，那么當(dāng)機(jī)器運(yùn)動(dòng)之后，“手”和“眼”發(fā)生的姿態(tài)變化一定滿足一定的約束關(guān)系，這樣求解一個(gè)方程就可以得到“手”-“眼”之間的坐標(biāo)轉(zhuǎn)換關(guān)系，一般是AX=XB形式的方程。

手眼系統(tǒng)分兩種：eye in hand和eye to hand，我們這里顯然是前者，即手-眼都在動(dòng)的情況。
手眼標(biāo)定分兩步法和單步法，后者最有名的論文是“hand eye calibration using dual quaternion"。一般認(rèn)為，單步法精度高于兩步法，前者估計(jì)旋轉(zhuǎn)之后再估計(jì)平移。
這里通過東京大學(xué)的論文“LiDAR and Camera Calibration using Motion Estimated by Sensor Fusion Odometry”來看看激光雷達(dá)和攝像頭的標(biāo)定算法。
顯然它是求解一個(gè)手-眼標(biāo)定的擴(kuò)展問題-，即2D-3D標(biāo)定，如圖所示：

求解激光雷達(dá)的姿態(tài)變化采用ICP，而攝像頭的運(yùn)動(dòng)采用特征匹配。后者有一個(gè)單目SFM的scale問題，論文提出了一個(gè)基于傳感器融合的解法：初始估計(jì)來自于無尺度的攝像頭運(yùn)動(dòng)和有尺度的激光雷達(dá)運(yùn)動(dòng)；之后有scale的攝像頭運(yùn)動(dòng)會(huì)在加上激光雷達(dá)點(diǎn)云數(shù)據(jù)被重新估計(jì)。最后二者的外參數(shù)就能通過手-眼標(biāo)定得到。下圖是算法流程圖：

手眼標(biāo)定的典型解法是兩步法：先求解旋轉(zhuǎn)矩陣，然后再估計(jì)平移向量，公式在下面給出：                       

現(xiàn)在因?yàn)閟cale問題，上述解法不穩(wěn)定，所以要利用激光雷達(dá)的數(shù)據(jù)做文章，見下圖：

3-D點(diǎn)云的點(diǎn)在圖像中被跟蹤，其2D-3D對(duì)應(yīng)關(guān)系可以描述為如下公式：

而求解的問題變成了：

上面優(yōu)化問題的初始解是通過經(jīng)典的P3P得到的。
得到攝像頭的運(yùn)動(dòng)參數(shù)之后可以在兩步手眼標(biāo)定法中得到旋轉(zhuǎn)和平移6參數(shù)，其中平移估計(jì)如下：

注：這里估計(jì)攝像頭運(yùn)動(dòng)和估計(jì)手眼標(biāo)定是交替進(jìn)行的，以改進(jìn)估計(jì)精度。除此之外，作者也發(fā)現(xiàn)一些攝像頭運(yùn)動(dòng)影響標(biāo)定精度的策略，看下圖分析：

可以總結(jié)出：1）攝像頭實(shí)際運(yùn)動(dòng)a 越小，投影誤差越??；2） ( )越小，投影誤差越小。第一點(diǎn)說明標(biāo)定時(shí)候攝像頭運(yùn)動(dòng)要小，第二點(diǎn)說明，標(biāo)定的周圍環(huán)境深度要變化小，比如墻壁。
另外還發(fā)現(xiàn)，增加攝像頭運(yùn)動(dòng)的旋轉(zhuǎn)角，攝像頭運(yùn)動(dòng)估計(jì)到手眼標(biāo)定的誤差傳播會(huì)小。
這個(gè)方法無法在室外自然環(huán)境中使用，因?yàn)辄c(diǎn)云投影的圖像點(diǎn)很難確定。
有三篇關(guān)于如何優(yōu)化激光雷達(dá)-攝像頭標(biāo)定的論文，不是通過3-D點(diǎn)云和圖像點(diǎn)的匹配誤差來估計(jì)標(biāo)定參數(shù)，而是直接計(jì)算點(diǎn)云在圖像平面形成的深度圖，其和攝像頭獲取的圖像存在全局匹配的測度。
不過這些方法，需要大量迭代，最好的做法是根據(jù)手眼標(biāo)定產(chǎn)生初始值為好。
另外，密西根大學(xué)是采用了激光雷達(dá)反射值，悉尼大學(xué)在此基礎(chǔ)上改進(jìn)，兩個(gè)都不如斯坦福大學(xué)方法方便，直接用點(diǎn)云和圖像匹配實(shí)現(xiàn)標(biāo)定。
斯坦福論文“Automatic online Calibration of Cameras and Lasers”。
斯坦福的方法是在線修正標(biāo)定的“漂移”，如下圖所示：精確的標(biāo)定應(yīng)該使圖中綠色點(diǎn)（深度不連續(xù)）和紅色邊緣（通過逆距離變換 IDT，即inverse distance transform）匹配。

標(biāo)定的目標(biāo)函數(shù)是這樣定義的：

其中w 是視頻窗大小，f 是幀#，(i, j) 是圖像中的像素位置，而p是點(diǎn)云的3-D點(diǎn)。X表示激光雷達(dá)點(diǎn)云數(shù)據(jù)，D是圖像做過IDT的結(jié)果。
下圖是實(shí)時(shí)在線標(biāo)定的結(jié)果例子：

第一行標(biāo)定好的，第二行出現(xiàn)漂移，第三行重新標(biāo)定。
密西根大學(xué)的論文“Automatic Targetless Extrinsic Calibration of a 3D Lidar and Camera by Maximizing Mutual Information”。
這里定義了標(biāo)定的任務(wù)就是求解兩個(gè)傳感器之間的轉(zhuǎn)換關(guān)系，如圖：求解R，T。

定義的Mutual Information (MI) 目標(biāo)函數(shù)是一個(gè)熵值：

求解的算法是梯度法：

下圖是一個(gè)標(biāo)定的例子：RGB像素和點(diǎn)云校準(zhǔn)。

澳大利亞悉尼大學(xué)的論文“Automatic Calibration of Lidar and Camera Images using Normalized Mutual Information”。
本文是對(duì)上面方法的改進(jìn)。傳感器配置如圖：

標(biāo)定的流程在下圖給出：

其中定義了一個(gè)新測度Gradient Orientation Measure (GOM)如下：

實(shí)際上是圖像和激光雷達(dá)點(diǎn)云的梯度相關(guān)測度。
點(diǎn)云數(shù)據(jù)和圖像數(shù)據(jù)匹配時(shí)候需要將點(diǎn)云投影到柱面圖像上，如圖所示：

投影公式如下：

而點(diǎn)云的梯度計(jì)算之前需要將點(diǎn)云投影到球面上，公式如下：

最后，點(diǎn)云的梯度計(jì)算方法如下：

標(biāo)定的任務(wù)就是求解GOM最大，而文中采用了蒙特卡洛方法，類似particle filter。下圖是一個(gè)結(jié)果做例子：

IMU-攝像頭標(biāo)定

德國Fraunhofer論文“INS-Camera Calibration without Ground Control Points“。
本文雖然是給無人機(jī)的標(biāo)定，對(duì)車輛也適合。
這是IMU定義的East, North, Up (ENU) 坐標(biāo)系：

而實(shí)際上IMU-攝像頭標(biāo)定和激光雷達(dá)-攝像頭標(biāo)定都是類似的，先解決一個(gè)手眼標(biāo)定，然后優(yōu)化結(jié)果。只是IMU沒有反饋信息可用，只有姿態(tài)數(shù)據(jù)，所以就做pose graph optimization。下圖是流程圖：其中攝像頭還是用SFM估計(jì)姿態(tài)。

這是使用的圖像標(biāo)定板：

激光雷達(dá)系統(tǒng)標(biāo)定

牛津大學(xué)論文“Automatic self-calibration of a full field-of-view 3D n-laser scanner".
本文定義點(diǎn)云的“crispness” 作為質(zhì)量測度，通過一個(gè)熵函數(shù)Re?nyi Quadratic Entropy (RQE）最小化作為在線標(biāo)定激光雷達(dá)的優(yōu)化目標(biāo)。（注：其中作者還討論了激光雷達(dá)的時(shí)鐘偏差問題解決方案）
“crisp“其實(shí)是描述點(diǎn)云分布作為一個(gè)GMM（Gaussian Mixture Model）形式下的致密度。根據(jù)信息熵的定義，RQE被選擇為測度：

下圖是一個(gè)標(biāo)定后采集的點(diǎn)云結(jié)果：

標(biāo)定算法如下：

雷達(dá)-攝像頭標(biāo)定

西安交大論文“Integrating Millimeter Wave Radar with a Monocular Vision Sensor for On-Road Obstacle Detection Applications”。
在講傳感器融合的時(shí)候提過這部分工作，這里重點(diǎn)介紹標(biāo)定部分。首先坐標(biāo)系關(guān)系如下：

傳感器配置如下：

標(biāo)定環(huán)境如下：

標(biāo)定其實(shí)是計(jì)算圖像平面和雷達(dá)反射面之間的homography矩陣參數(shù)，如下圖：