自動駕駛系統的感測器標定方法
感測器標定是自動駕駛的基本需求，一個車上裝了多個/多種感測器，而它們之間的坐標關係是需要確定的。灣區自動駕駛創業公司ZooX的co-founder和CTO是Sebastia Thrun的學生Jesse Levinson，他的博士論文就是感測器標定。
這個工作可分成兩部分：內參標定和外參標定，內參是決定感測器內部的映射關係，比如攝像頭的焦距，偏心和像素橫縱比（+畸變係數），而外參是決定感測器和外部某個坐標系的轉換關係，比如姿態參數（旋轉和平移6自由度）。
攝像頭的標定曾經是計算機視覺中3-D重建的前提，張正友老師著名的的Zhang氏標定法，利用Absolute Conic不變性得到的平面標定演算法簡化了控制場。
這裡重點是，討論不同感測器之間的外參標定，特別是激光雷達和攝像頭之間的標定。
另外在自動駕駛研發中，GPS/IMU和攝像頭或者激光雷達的標定，雷達和攝像頭之間的標定也是常見的。不同感測器之間標定最大的問題是如何衡量最佳，因為獲取的數據類型不一樣：
- 攝像頭是RGB圖像的像素陣列；
- 激光雷達是3-D點雲距離信息（有可能帶反射值的灰度值）；
- GPS-IMU給的是車身位置姿態信息；
- 雷達是2-D反射圖。
這樣的話，實現標定誤差最小化的目標函數會因為不同感測器配對而不同。
另外，標定方法分targetless和target兩種，前者在自然環境中進行，約束條件少，不需要用專門的target；後者則需要專門的控制場，有ground truth的target，比如典型的棋盤格平面板。
這裡僅限於targetless方法的討論，依次給出標定的若干演算法。
首先是手-眼標定（Hand-Eye Calibration）。
這是一個被標定方法普遍研究的，一定約束條件下的問題：可以廣義的理解，一個「手」（比如GPS/IMU）和一個「眼」（激光雷達/攝像頭）都固定在一個機器上，那麼當機器運動之後，「手」和「眼」發生的姿態變化一定滿足一定的約束關係，這樣求解一個方程就可以得到「手」-「眼」之間的坐標轉換關係，一般是AX=XB形式的方程。
手眼系統分兩種：eye in hand和eye to hand，我們這裡顯然是前者，即手-眼都在動的情況。
手眼標定分兩步法和單步法，後者最有名的論文是「hand eye calibration using dual quaternion"。一般認為，單步法精度高於兩步法，前者估計旋轉之後再估計平移。
這裡通過東京大學的論文「LiDAR and Camera Calibration using Motion Estimated by Sensor Fusion Odometry」來看看激光雷達和攝像頭的標定演算法。
顯然它是求解一個手-眼標定的擴展問題-，即2D-3D標定，如圖所示：
求解激光雷達的姿態變化採用ICP，而攝像頭的運動採用特徵匹配。後者有一個單目SFM的scale問題，論文提出了一個基於感測器融合的解法：初始估計來自於無尺度的攝像頭運動和有尺度的激光雷達運動；之後有scale的攝像頭運動會在加上激光雷達點雲數據被重新估計。最後二者的外參數就能通過手-眼標定得到。下圖是演算法流程圖：
手眼標定的典型解法是兩步法：先求解旋轉矩陣，然後再估計平移向量，公式在下面給出：
現在因為scale問題，上述解法不穩定，所以要利用激光雷達的數據做文章，見下圖：
3-D點雲的點在圖像中被跟蹤，其2D-3D對應關係可以描述為如下公式：
而求解的問題變成了：
上面優化問題的初始解是通過經典的P3P得到的。
得到攝像頭的運動參數之後可以在兩步手眼標定法中得到旋轉和平移6參數，其中平移估計如下：
註：這裡估計攝像頭運動和估計手眼標定是交替進行的，以改進估計精度。除此之外，作者也發現一些攝像頭運動影響標定精度的策略，看下圖分析：
可以總結出：1）攝像頭實際運動a 越小，投影誤差越小；2） ( )越小，投影誤差越小。第一點說明標定時候攝像頭運動要小，第二點說明，標定的周圍環境深度要變化小，比如牆壁。
另外還發現，增加攝像頭運動的旋轉角，攝像頭運動估計到手眼標定的誤差傳播會小。
這個方法無法在室外自然環境中使用，因為點雲投影的圖像點很難確定。
有三篇關於如何優化激光雷達-攝像頭標定的論文，不是通過3-D點雲和圖像點的匹配誤差來估計標定參數，而是直接計算點雲在圖像平面形成的深度圖，其和攝像頭獲取的圖像存在全局匹配的測度。
不過這些方法，需要大量迭代，最好的做法是根據手眼標定產生初始值為好。
另外，密西根大學是採用了激光雷達反射值，悉尼大學在此基礎上改進，兩個都不如斯坦福大學方法方便，直接用點雲和圖像匹配實現標定。
斯坦福論文「Automatic Online Calibration of Cameras and Lasers」。
斯坦福的方法是在線修正標定的「漂移」，如下圖所示：精確的標定應該使圖中綠色點（深度不連續）和紅色邊緣（通過逆距離變換 IDT，即inverse distance transform）匹配。
標定的目標函數是這樣定義的：
其中w 是視頻窗大小，f 是幀#，(i, j) 是圖像中的像素位置，而p是點雲的3-D點。X表示激光雷達點雲數據，D是圖像做過IDT的結果。
下圖是實時在線標定的結果例子：
密西根大學的論文「Automatic Targetless Extrinsic Calibration of a 3D Lidar and Camera by Maximizing Mutual Information」。
這裡定義了標定的任務就是求解兩個感測器之間的轉換關係，如圖：求解R，T。
定義的Mutual Information (MI) 目標函數是一個熵值：
求解的演算法是梯度法：
下圖是一個標定的例子：RGB像素和點雲校準。
澳大利亞悉尼大學的論文「Automatic Calibration of Lidar and Camera Images using Normalized Mutual Information」。
本文是對上面方法的改進。感測器配置如圖：
標定的流程在下圖給出：
其中定義了一個新測度Gradient Orientation Measure (GOM)如下：
實際上是圖像和激光雷達點雲的梯度相關測度。
點雲數據和圖像數據匹配時候需要將點雲投影到柱面圖像上，如圖所示：
投影公式如下：
而點雲的梯度計算之前需要將點雲投影到球面上，公式如下：
最後，點雲的梯度計算方法如下：
標定的任務就是求解GOM最大，而文中採用了蒙特卡洛方法，類似particle filter。下圖是一個結果做例子：
IMU-攝像頭標定。
德國Fraunhofer論文「INS-Camera Calibration without Ground Control Points「。
本文雖然是給無人機的標定，對車輛也適合。
這是IMU定義的East, North, Up (ENU) 坐標系：
而實際上IMU-攝像頭標定和激光雷達-攝像頭標定都是類似的，先解決一個手眼標定，然後優化結果。只是IMU沒有反饋信息可用，只有姿態數據，所以就做pose graph optimization。下圖是流程圖：其中攝像頭還是用SFM估計姿態。
這是使用的圖像標定板：
激光雷達系統標定。
牛津大學論文「Automatic self-calibration of a full field-of-view 3D n-laser scanner".
本文定義點雲的「crispness」 作為質量測度，通過一個熵函數Re?nyi Quadratic Entropy (RQE）最小化作為在線標定激光雷達的優化目標。（註：其中作者還討論了激光雷達的時鐘偏差問題解決方案）
「crisp「其實是描述點雲分佈作為一個GMM（Gaussian Mixture Model）形式下的緻密度。根據信息熵的定義，RQE被選擇為測度：
下圖是一個標定後採集的點雲結果：
標定演算法如下：
雷達-攝像頭標定
西安交大論文「Integrating Millimeter Wave Radar with a Monocular Vision Sensor for On-Road Obstacle Detection Applications」。
在講感測器融合的時候提過這部分工作，這裡重點介紹標定部分。首先坐標系關係如下：
感測器配置如下：
標定環境如下：
標定其實是計算圖像平面和雷達反射面之間的homography矩陣參數，如下圖：
--------結束---------
推薦閱讀：
