車端激光和雙目相機的自動標定算法

摘要

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

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

一、介紹

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

由于激光和視覺都有他們的缺陷和優勢，所以兩個傳感器可以融合起來。但是我們融合的時候，一個精準的外參就很重要了。（比如我們要利用得到像素點的深度，需要利用外參把單幀或者多幀激光點云利用外參投影到圖像上獲得像素點的深度，如果外參不準，我們深度恢復的結果也很難準。還有一些語義信息的投影諸如此類的融合都需要一個比較準的外參）。

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

二、相關工作

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

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

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

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

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

三、標定算法

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

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

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

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

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

A. 數據表示

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

B. 目標分割

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

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

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

C. 圓檢測

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

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

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

D.匹配

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

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

實驗

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

仿真中用的傳感器是一個雙目相機+16線的激光雷達，評估出來的精度：

作者也在實際場景中了實驗，由于無法采集真值，所以智能通過投影的效果來判斷標定的效果：

仿真環境搭建：

下載仿真環境的代碼：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文件進行仿真，例如：roslaunch velo2cam_gazebo mono_hdl64_p1_real.launch ，這里正常情況下會報一大推找不到模型的錯誤。

我們需要在https://github.com/osrf/gazebo_models找對應的model下載，但是好像缺的太多了，我就很暴力的把他們都下載了下來，大家可以在百度云下載，然后把他們放到.gazebo/models，再運行就好了（這里在launch文件中gui是false，我們需要把他改為true才會自動打開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.