車端激光和雙目相機(jī)的自動(dòng)標(biāo)定算法

摘要

在車端配置一個(gè)雙目相機(jī)再加上一個(gè)激光雷達(dá)已經(jīng)成為一種比較常用的配置。但是為了融合相機(jī)和激光數(shù)據(jù)我們需要一個(gè)復(fù)雜的標(biāo)定過(guò)程。本文提供了一種不需要人干預(yù)的自動(dòng)化的雙目和激光雷達(dá)的外參估計(jì)方法。本文的標(biāo)定方法是克服在自動(dòng)駕駛車輛中的傳感器的常見(jiàn)的限制，如低分辨率和特殊的傳感器的位置（如你在車端在沒(méi)有升降臺(tái)的情況下不能讓車上下動(dòng)，roll,pitch旋轉(zhuǎn)）。為了證明算法的可行性，作者分別在仿真和真實(shí)環(huán)境中做的實(shí)驗(yàn)都表現(xiàn)出了比較好的結(jié)果。

代碼地址：在公眾號(hào)「計(jì)算機(jī)視覺(jué)工坊」，后臺(tái)回復(fù)「車端激光」，即可直接下載。

一、介紹

高級(jí)駕駛輔助系統(tǒng)（ASAS, Advanced Driver Assistance Systems）或者是自動(dòng)駕駛的系統(tǒng)需要準(zhǔn)確的環(huán)境感知來(lái)正確的進(jìn)行路徑規(guī)劃，在可能影響駕駛方法的情況下（例如光照的變化），這些系統(tǒng)必需要有一定程度的魯棒性。因此車端配置的傳感器需要可以在不同的情況下互補(bǔ)。視覺(jué)系統(tǒng)可以提供場(chǎng)景的外觀信息，所以在車端系統(tǒng)中很常見(jiàn)。雙目視覺(jué)能提供一定范圍內(nèi)的場(chǎng)景的深度信息，所以作為一種經(jīng)濟(jì)高效的傳感器，可以提供車周圍一定范圍內(nèi)稠密的點(diǎn)云信息。而激光雷達(dá)的特點(diǎn)是測(cè)量精度高（測(cè)量飛行時(shí)間，根據(jù)點(diǎn)的線束和角度信息投影到空間坐標(biāo)系），而且可以提供360度范圍內(nèi)的點(diǎn)云信息。(但是相機(jī)的深度信息畢竟不準(zhǔn)確，但是分辨率高；激光的測(cè)量雖然準(zhǔn)，但是分辨率很低，特別是z軸上，造成純激光的系統(tǒng)都會(huì)有很大的z軸漂移）。

由于激光和視覺(jué)都有他們的缺陷和優(yōu)勢(shì)，所以兩個(gè)傳感器可以融合起來(lái)。但是我們?nèi)诤系臅r(shí)候，一個(gè)精準(zhǔn)的外參就很重要了。（比如我們要利用得到像素點(diǎn)的深度，需要利用外參把單幀或者多幀激光點(diǎn)云利用外參投影到圖像上獲得像素點(diǎn)的深度，如果外參不準(zhǔn)，我們深度恢復(fù)的結(jié)果也很難準(zhǔn)。還有一些語(yǔ)義信息的投影諸如此類的融合都需要一個(gè)比較準(zhǔn)的外參）。

當(dāng)前外參估計(jì)的方法可能需要復(fù)雜的過(guò)程或者缺乏泛化能力，標(biāo)定的精度完全取決于標(biāo)定的環(huán)境或者初值的準(zhǔn)確程度。本文提出了一種針對(duì)車端雙目相機(jī)和激光雷達(dá)的標(biāo)定的方法，和現(xiàn)有的方法不同，本文沒(méi)有對(duì)系統(tǒng)提出任何強(qiáng)有力的假設(shè)，可以標(biāo)定16線這種低分辨率的激光雷達(dá)，也可以允許傳感器之間的位姿很不同。本文的方法只是假設(shè)兩個(gè)融合的數(shù)據(jù)沒(méi)有很大的時(shí)間間隔，這是很容易做到的（激光一般是10HZ，相機(jī)是30HZ）。

二、相關(guān)工作

在過(guò)去幾年，關(guān)于不同的傳感器的外參標(biāo)定的工作很多，主要是自動(dòng)駕駛和多傳感器融合的機(jī)器人。這其中最多的研究就是關(guān)于相機(jī)和激光雷達(dá)的，由于移動(dòng)平臺(tái)的固有限制，比如傳感器智能采集到一個(gè)或者四個(gè)面（東西南北，上下就看不到）。標(biāo)定的過(guò)程一般是在傳感器運(yùn)行正常的人工布置的系統(tǒng)中執(zhí)行的。傳統(tǒng)的方法一般會(huì)需要人工標(biāo)注：https://github.com/MegviiRobot/CamLaserCalibraTool（這是2007年IROS的方法，Extrinsic Self Calibration of a Camera and a 3D Laser Range Finder from Natural Scenes），然而由于機(jī)器人平臺(tái)經(jīng)常出現(xiàn)標(biāo)定的錯(cuò)誤，所以這個(gè)過(guò)程自動(dòng)進(jìn)行最好。

標(biāo)定的過(guò)程是需要找圖像平面和激光點(diǎn)云在不同視角下的對(duì)應(yīng)點(diǎn)，所以一般都會(huì)用一些特殊的標(biāo)定板作為目標(biāo)，例如三角板，多邊形或者球體。現(xiàn)有的方法中基于物體形狀的多樣性來(lái)找到他們?cè)诓煌瑐鞲衅鞯奈恢?，但是盡管如此，場(chǎng)景中的平面很普遍，因?yàn)榭梢岳蒙疃刃畔⒑吞峁┑奶囟ǖ呐d中基于幾何的計(jì)算很容易檢測(cè)出來(lái)。另外，Scott等人基于運(yùn)動(dòng)跟蹤的方法也解決了沒(méi)有共視關(guān)系的傳感器之間的標(biāo)定的問(wèn)題（Exploiting Known Unknowns: Scene Induced Cross-Calibration of Lidar-Stereo Systems， 2015年IROS）。

近些年來(lái)稠密的3D傳感器發(fā)展很快（例如掃描儀，非重復(fù)掃描特性的固態(tài)激光雷達(dá)）。所以有一些研究這種稠密3D傳感器的標(biāo)定的工作。Automatic Cameraand Range Sensor Calibration using a single Shot（2012. ICRA）把幾塊標(biāo)定板當(dāng)做目標(biāo)位置，提出了one-shot的方法。Calibration of RGB Camera With Velodyne LiDAR（2014）提出了單一視角下基于標(biāo)定區(qū)域圓形特征的標(biāo)定方法。

第二種方法是不急于人工設(shè)計(jì)的標(biāo)定目標(biāo)，而是利用環(huán)境中的特征。Line-based Extrinsic Calibration of Range and Image Sensors（2013,ICRA）這篇論文利用場(chǎng)景中的線段特征來(lái)計(jì)算傳感器的外參。這種方法適應(yīng)于室內(nèi)有大量線特征的場(chǎng)景。在室外的道路場(chǎng)景，利用地面和一些障礙物可以相機(jī)和激光的外參。

另外，如何評(píng)價(jià)校準(zhǔn)方法的準(zhǔn)確程度也是一個(gè)沒(méi)有被解決的問(wèn)題。因?yàn)樵趯?shí)際標(biāo)定的時(shí)候我們很難得到傳感器之間的外參的真值。Automatic Online Calibration of Cameras and Lasers中提出了一種方法，基于場(chǎng)景的不連續(xù)性來(lái)計(jì)算目標(biāo)函數(shù)的變化得到外參的誤差。

三、標(biāo)定算法

本文提出的標(biāo)定方法標(biāo)定的是雙目相機(jī)之間的外參和相機(jī)到激光雷達(dá)的外參。如下圖所示：

相機(jī)和激光雷達(dá)之間的剛體變換可以表示為：

其中前三個(gè)表示兩個(gè)坐標(biāo)系之間的平移，后三個(gè)表示的就是兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)（RPY）。有了這個(gè)外參之后我們可以很容易得到相機(jī)到激光的外參的變換矩陣形式，利用這個(gè)矩陣我們可以很容易把相機(jī)系下的點(diǎn)轉(zhuǎn)換到激光坐標(biāo)系下。為了得到外參，作者采集了很多雙目圖像（時(shí)間同步后的）和激光幀的掃描，系統(tǒng)中假設(shè)相機(jī)的內(nèi)參是已知的，圖片是經(jīng)過(guò)畸變矯正的。

本文只用了一個(gè)單個(gè)的校準(zhǔn)目標(biāo)，而且他只能在特定的視角下才可以觀測(cè)到。這樣可以避免在標(biāo)定過(guò)程中的需要處理多個(gè)標(biāo)定目標(biāo)或者場(chǎng)景的變化。作者利用Calibration of RGB Camera With Velodyne LiDAR論文中的思路，在一個(gè)平面上挖了四個(gè)對(duì)稱的圓孔，這些孔作為相機(jī)和激光雷達(dá)可見(jiàn)的明顯特征。因此在我們放置激光和視覺(jué)的時(shí)候兩個(gè)傳感器的視野內(nèi)要有一定的重疊，以便圓形孔與至少兩個(gè)激光雷達(dá)光束相交，并且從相機(jī)完全可見(jiàn)。

整個(gè)系統(tǒng)除了上述假設(shè)外沒(méi)有任何其他假設(shè)，沒(méi)有對(duì)兩個(gè)傳感器的旋轉(zhuǎn)和平移有什么要求。標(biāo)定的過(guò)程分為兩個(gè)階段：兩個(gè)點(diǎn)云（圖像三角化出來(lái)的和激光掃描的）的參考點(diǎn)分割；利用點(diǎn)云對(duì)齊得到外參。

A. 數(shù)據(jù)表示

本文提出的方法是基于要標(biāo)定的傳感器的，利用3D激光雷達(dá)我們可以表示點(diǎn)云為（x,y,z）的集合。雙目相機(jī)得到的信息主要利用兩個(gè)：灰度圖和深度信息。其中深度信息是利用雙目匹配的算法，可以得到稠密的3D點(diǎn)云信息。在標(biāo)定過(guò)程中利用兩個(gè)點(diǎn)云的相似性來(lái)計(jì)算兩個(gè)傳感器之間的外參。激光的點(diǎn)云集合表示為P^l_0，相機(jī)的點(diǎn)云表示為 P^c_0。對(duì)于雙目的匹配，本文利用的是SGM算法，這種方法可以得到比較準(zhǔn)的深度估計(jì)的結(jié)果。其中邊界定位的問(wèn)題可以很好的利用反射信息解決。利用有一定的紋理的標(biāo)定板來(lái)解決雙目匹配的問(wèn)題。利用濾波器和距離信息可以很好的處理點(diǎn)云，保證處理后的點(diǎn)云來(lái)自兩個(gè)傳感器共視的部分來(lái)更好的做標(biāo)定。

B. 目標(biāo)分割

標(biāo)定算法的第一步是提取標(biāo)定板上不連續(xù)的點(diǎn)。所以針對(duì)來(lái)自激光雷達(dá)和雙目相機(jī)的點(diǎn)云執(zhí)行幾個(gè)連續(xù)分割的過(guò)程。分割的第一步是找到點(diǎn)云的表示集合形狀的點(diǎn)集。也即是找到標(biāo)定板上點(diǎn)。利用標(biāo)定板是一個(gè)平面的優(yōu)點(diǎn)，我們直接在原始點(diǎn)云中執(zhí)行平面檢測(cè)的算法。有一些嚴(yán)格的約束來(lái)保證我們找到的平面確實(shí)是標(biāo)定板的平面。第一是在執(zhí)行RANSAC時(shí)δ plane的選取是很嚴(yán)格的；第二是檢測(cè)出來(lái)的平面需要和傳感器坐標(biāo)系中的Z軸平行（角度差在一個(gè)很小的范圍內(nèi)）。

當(dāng)我們找到了標(biāo)定板的平面后，點(diǎn)和平面的距離大于閾值將被剔除，這樣我們就得到了平面上的點(diǎn)云。如圖3a和圖4a。然后是利用濾波的方法去除不是不連續(xù)點(diǎn)的點(diǎn)云，由于兩個(gè)傳感器采集到的點(diǎn)云的密度不同，這個(gè)過(guò)程是不同的。首先對(duì)于激光點(diǎn)云，本文主要參考Automatic Online Calibration of Cameras and Lasers中的方法來(lái)找到深度不連續(xù)的點(diǎn)。對(duì)于每一個(gè)在標(biāo)定板上的點(diǎn)，通過(guò)計(jì)算他和相鄰點(diǎn)的深度的差值來(lái)得到一個(gè)表示：

這表示第i個(gè)點(diǎn)和他左邊和右邊點(diǎn)深度差值的最大值，利用計(jì)算得到的值我們可以剔除所有連續(xù)的點(diǎn)，如圖3b所示。而對(duì)于雙目相機(jī)的點(diǎn)云，我們保留投影到強(qiáng)度圖像邊緣的點(diǎn)，然后基于Sobel濾波器在雙目的左目進(jìn)行濾波，在計(jì)算得到的Sobel圖像中對(duì)應(yīng)值比較低的點(diǎn)被剔除，如圖4b所示。

C. 圓檢測(cè)

最后一個(gè)步驟是從標(biāo)定板上找到三個(gè)圓在的位置，圓的圓心會(huì)用來(lái)當(dāng)做關(guān)鍵點(diǎn)做兩幀點(diǎn)云的對(duì)齊工作。為了提升圓檢測(cè)的準(zhǔn)確度，本文進(jìn)行了一個(gè)濾波的操作來(lái)去除不屬于圓的點(diǎn)。根據(jù)不連續(xù)性已經(jīng)把標(biāo)定板上的點(diǎn)基本濾干凈了，但是標(biāo)定板邊緣的點(diǎn)還存在。對(duì)于激光點(diǎn)云，只保存線上有圓的那些線束的點(diǎn)，然后在濾除環(huán)中的一些外點(diǎn)。對(duì)于雙目相機(jī)的點(diǎn)云，由于標(biāo)定板的邊界被很密集的表示，所以我們可以基于線段來(lái)消除。利用采樣一致性檢測(cè)找到線段，并根據(jù)線段的方向和標(biāo)定板的尺寸選擇正確的是邊緣的線段，以防從圓中刪除有用的信息。實(shí)驗(yàn)表明了作者的邊界濾波的方法很好，能夠有效的濾除邊緣線段。經(jīng)過(guò)濾除邊緣后的表示如圖3c和4c表示。

現(xiàn)在相機(jī)和激光雷達(dá)的基于點(diǎn)表示的標(biāo)定板上的圓我們已經(jīng)可以得到了。最后在相機(jī)和激光雷達(dá)掃到的標(biāo)定板的2D平面上進(jìn)行圓的檢測(cè)。把第二階段得到的點(diǎn)云投影到平面上，然后基于平面方程的約束找到在平面上的點(diǎn)。然后在XY平面上根據(jù)已知的圓的半徑基于抽樣一致性檢測(cè)來(lái)檢測(cè)圓。為了避免檢測(cè)到錯(cuò)誤的結(jié)果。作者把標(biāo)定的傳感器到平面的距離也做為了一個(gè)約束。最后把檢測(cè)到的中心投影到3D的空間中。我們就可以得到圖3d和4d。這里需要注意的是由于圓的分割是在二維空間進(jìn)行的，所以這里的傳感器需要分辨率稍微高一點(diǎn)（論文中說(shuō)16線的就可以），平面上的圓可以由三個(gè)點(diǎn)確定，所以需要兩個(gè)雷達(dá)的線穿過(guò)每個(gè)圓（這樣就最少有四個(gè)點(diǎn)在圓上）。

如果標(biāo)定板在兩個(gè)傳感器的表示足夠準(zhǔn)確，我們就可以基于檢測(cè)得到的四個(gè)圓心進(jìn)行配準(zhǔn)。同時(shí)為了提升對(duì)過(guò)程中噪聲的魯棒性，圓心在n幀的窗口上累積。然后利用聚類算法在點(diǎn)云配準(zhǔn)階段找到點(diǎn)云的質(zhì)心。所以本文的方法要求在標(biāo)定的整個(gè)過(guò)程標(biāo)定系統(tǒng)都保持靜止。本文基于歐幾里得距離的聚類策略設(shè)定一定的閾值來(lái)進(jìn)行聚類來(lái)剔除異常值對(duì)系統(tǒng)的影響。根據(jù)窗口長(zhǎng)度的約束，可以最每個(gè)點(diǎn)集中點(diǎn)的最大值和最小值進(jìn)行嚴(yán)格的約束。

D.匹配

最后一步的匹配通過(guò)最小化聚類的圓的中心的參考點(diǎn)的距離來(lái)得到相機(jī)和激光雷達(dá)的外參。匹配的過(guò)程主要分為兩個(gè)部分：在假設(shè)沒(méi)有旋轉(zhuǎn)的情況下求最優(yōu)的變換矩陣，這其實(shí)就是在計(jì)算兩個(gè)點(diǎn)云之間的平移，可以通過(guò)一個(gè)三維的向量來(lái)表示三個(gè)軸的平移。通過(guò)四個(gè)參考點(diǎn)得到12個(gè)方程基于最小二乘的可以求出三個(gè)軸上的平移。

激光系下和相機(jī)系下每對(duì)點(diǎn)可以構(gòu)建三個(gè)方程，點(diǎn)通過(guò)他在標(biāo)定板的不同位置來(lái)命名：top-left (tl), top-right (tr), bottom-left, (bl) and bottom-right (br)。最后基于QR分解找到方程的最小二乘解。最后是匹配的過(guò)程，這個(gè)過(guò)程估計(jì)平移和旋轉(zhuǎn)（xyz rpy）。作者直接基于A method for Registration of 3-D Shapes的方法來(lái)最小化距離點(diǎn)之間的距離。

實(shí)驗(yàn)

作者分別在仿真環(huán)境和真實(shí)環(huán)境中做了精度評(píng)價(jià)的實(shí)驗(yàn)。作者基于Gazebo環(huán)境中配置了九種不同的傳感器設(shè)置，具體的擺放如下表所示：

仿真中用的傳感器是一個(gè)雙目相機(jī)+16線的激光雷達(dá)，評(píng)估出來(lái)的精度：

作者也在實(shí)際場(chǎng)景中了實(shí)驗(yàn)，由于無(wú)法采集真值，所以智能通過(guò)投影的效果來(lái)判斷標(biāo)定的效果：

仿真環(huán)境搭建：

下載仿真環(huán)境的代碼：https://github.com/beltransen/velo2cam_gazebo，如果你用的是ubuntu18.04則需要更新/velo2cam_gazebo-master/gazebo_plugins/velodyne_gazebo_plugin中的內(nèi)容，利用https://bitbucket.org/DataspeedInc/velodyne_simulator/src/master/中對(duì)應(yīng)文件夾中的文件替換。

把/velo2cam_gazebo-master/gazebo_models文件夾中文件移到.gazebo/models

直接利用launch文件進(jìn)行仿真，例如：roslaunch velo2cam_gazebo mono_hdl64_p1_real.launch ，這里正常情況下會(huì)報(bào)一大推找不到模型的錯(cuò)誤。

我們需要在https://github.com/osrf/gazebo_models找對(duì)應(yīng)的model下載，但是好像缺的太多了，我就很暴力的把他們都下載了下來(lái)，大家可以在百度云下載，然后把他們放到.gazebo/models，再運(yùn)行就好了（這里在launch文件中g(shù)ui是false，我們需要把他改為true才會(huì)自動(dòng)打開(kāi)gazebo）：鏈接: https://pan.baidu.com/s/1J0ftVrmewjYeLdCQZ4wqpg 提取碼: nfgx 。

運(yùn)行結(jié)果如下：

我們可以根據(jù)算法得到的結(jié)果和真值比較判斷算法的精度。

[1] Guindel C , J Beltrán, D Martín, et al. Automatic Extrinsic Calibration for Lidar-Stereo Vehicle Sensor Setups[J]. IEEE, 2017.