車(chē)端激光和雙目相機的自動(dòng)標定算法

摘要

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

代碼地址：在公眾號「計算機視覺(jué)工坊」，后臺回復「車(chē)端激光」，即可直接下載。

一、介紹

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

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

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

二、相關(guān)工作

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

標定的過(guò)程是需要找圖像平面和激光點(diǎn)云在不同視角下的對應點(diǎn)，所以一般都會(huì )用一些特殊的標定板作為目標，例如三角板，多邊形或者球體?，F有的方法中基于物體形狀的多樣性來(lái)找到他們在不同傳感器的位置，但是盡管如此，場(chǎng)景中的平面很普遍，因為可以利用深度信息和提供的特定的興中基于幾何的計算很容易檢測出來(lái)。另外，Scott等人基于運動(dòng)跟蹤的方法也解決了沒(méi)有共視關(guān)系的傳感器之間的標定的問(wèn)題（Exploiting Known Unknowns: Scene Induced Cross-Calibration of Lidar-Stereo Systems， 2015年IROS）。

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

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

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

三、標定算法

本文提出的標定方法標定的是雙目相機之間的外參和相機到激光雷達的外參。如下圖所示：

相機和激光雷達之間的剛體變換可以表示為：

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

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

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

A. 數據表示

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

B. 目標分割

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

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

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

C. 圓檢測

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

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

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

D.匹配

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

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

實(shí)驗

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

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

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

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

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

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

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

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

運行結果如下：

我們可以根據算法得到的結果和真值比較判斷算法的精度。

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