基于固態(tài)激光雷達輔助的十六線機械雷達和單目相機的外參標定方法
激光視覺聯(lián)合標定的目的是構建激光點云和視覺像素點之間的關系，通過標定得到的激光雷達和相機之間的外參把三維的激光點投影到相機坐標系下，然后利用相機的模型把三維點投影到像素平面。由于標定的結果直接影響信息融合的效果，所以標定技術是多傳感器之間信息交互中的關鍵。
作者：Liam | 來源：微信公眾號「3D視覺工坊」
標定的關鍵是尋找場景中激光點云和圖像平面的對應點，利用對應點求出相機和激光雷達之間的外參。對于2D的像素點和3D的激光點，所以這個問題可以構建為如公式所示。
公式左邊表示激光在圖像坐標系中的像素的齊次坐標，右邊第一個矩陣為相機的內參矩陣，相機的針孔模型中的參數；第二個矩陣表示激光雷達到相機的外參，主要是旋轉矩陣R和平移向量t；最后一個向量表示激光點在激光坐標系下的齊次坐標。
由于機械激光雷達單幀的點云稀疏，直接基于機械點云和圖像匹配構建匹配關系比較困難，本課題引入具有非重復掃描特性的稠密的固態(tài)激光雷達點云作為中間標定過程。首先利用固態(tài)激光得到的場景的稠密點云，并基于如表所示的方法提取場景中的線特征，然后再視覺圖像中基于成熟的線段檢測器(Line Segment Detector, LSD)提取線特征，最后基于激光點云和圖像中的線特征進行數據關聯(lián)得到準確的固態(tài)雷達和相機之間的外參標定結果。
檢測出來的激光的線特征和視覺的線特征匹配，對于每一個激光點云中的線特征，采樣線上的點，每一個采樣點通過當前估計出來的外參投影到相機平面。
利用剛體變換可以得到激光點在相機坐標系下的位置，然后利用第二節(jié)介紹的針孔模型π和相機的畸變模型f把相機坐標系下的激光投影到點投影到圖像平面上，并得到去畸變后的激光點在圖像上的位置。
利用圖像中的線特征像素構成的kd-tree找到離當前投影點最近的若干個點，可以得到一個集合Q，計算最近像素點的均值和方差。
最后利用圖像線特征上的一點q和協(xié)方差矩陣的最小特征值對應的特征向量得到線特征垂線方向n得到線特征的數學表達式。同時驗證激光點云投影到圖像上的線特征和計算得到的n的正交性來快速的去除距離很近但是又不平行的錯誤匹配。利用激光點云中檢測到的線特征上的一個點LPi ，激光線特征匹配的圖像線特征的法向量ni及激光點云線特征對應的圖像線特征上的一點qi構建殘差函數。
得到殘差方程和優(yōu)化變量后就可以基于高斯牛頓或者其他的非線性優(yōu)化方法得到最優(yōu)的固態(tài)激光雷達和相機的外參。
得到了準確的相機和固態(tài)雷達的外參之后，下一步就是做機械雷達和固態(tài)雷達之間的外參Tlil標定，同樣的在靜態(tài)環(huán)境下采集點云信息，分階段的利用點云信息優(yōu)化外參，首先根據前邊介紹的NDT對齊輸入的兩幀點云得到外參的初值，然后檢測兩幀點云中的線特征和平面特征，構建誤差函數進一步優(yōu)化得到準確的機械雷達和固態(tài)雷達之間的外參。最后利用得到的固態(tài)和相機之間的外參Tcl1和機械雷達和固態(tài)雷達之間的外參Tlil就可以得到需要的相機和機械雷達之間的外參Tcl。
基于固態(tài)激光雷達的稠密點云對機械雷達和ZED相機的左目進行標定，相機、固態(tài)激光雷達和16線的機械雷達采集的數據如下圖所示。
傳感器采集信息示意圖
圖 (a)為機械雷達采集得到的點云，可以看到單幀激光點包含的信息很少很難直接和圖像對齊，而 (b)為固態(tài)激光雷達重復掃描得到的場景信息，可以看到場景中豐富的結構信息，較為容易和(c)所示的視覺信息對齊。
首先利用固態(tài)激光得到的場景的稠密點云和視覺圖像基于線特征進行標定，首先基于表1提出的方法提取固態(tài)激光點云中的線特征，然后利用LSD提取視覺圖像中的線特征。
由于不同方向的線特征對解算位姿不同的方向約束不同，所以尋找線特征較豐富且方向較多的場景，本文選擇基于圖書館外觀中豐富的線特征進行實驗。如圖(a)所示為固態(tài)激光雷達點云中提取的線特征，圖(b)為圖像中豐富的線特征。利用最近鄰搜素的方式匹配激光點云和圖像中的線特征，然后求解最優(yōu)相機和固態(tài)激光雷達間的外參，利用迭代優(yōu)化后的外參把激光點云投影到圖像上結果如圖所示。
得到相機和固態(tài)激光雷達的外參后，利用固態(tài)激光雷達點云和機械雷達點云利用icp或者其他的方法進行數據關聯(lián)，得到準確的外參，利用該外參把固態(tài)激光點云和機械雷達點云對齊后如下所示。
其中紅色點表示原始的機械雷達點云，藍色點表示對齊后的機械雷達點云，綠色點表示稠密的固態(tài)激光雷達點云，其中對齊的邊緣細節(jié)如圖所示。
基于固態(tài)雷達與相機的外參及固態(tài)雷達與機械雷達的外參就可以得到相機和機械雷達準確的外參，利用該外參把機械雷達點云投影到圖像上如圖所示。
參考文獻及代碼：
固態(tài)-單目相機標定：https://github.com/hku-mars/livox_camera_calib
論文在工坊分享過：針對高分辨率雷達和相機的無標定板的像素級外參自標定方法。
激光點云數據關聯(lián)方法：
https://github.com/ethz-asl/robust_point_cloud_registration
為了得到更準的位姿，可以自己寫一個手動調整優(yōu)化的小工具。也可以多階段，先獲取比較準的外參，然后再優(yōu)化一次。
本文僅做學術分享，如有侵權，請聯(lián)系刪文。
3D視覺工坊精品課程官網：3dcver.com
1.面向自動駕駛領域的多傳感器數據融合技術
2.面向自動駕駛領域的3D點基于固態(tài)激光雷達輔助的十六線機械雷達和單目相機的外參標定方法云目標檢測全棧學習路線！(單模態(tài)+多模態(tài)/數據+代碼)
3.徹底搞透視覺三維重建：原理剖析、代碼講解、及優(yōu)化改進
4.國內首個面向工業(yè)級實戰(zhàn)的點云處理課程
5.激光-視覺-IMU-GPS融合SLAM算法梳理和代碼講解
6.徹底搞懂視覺-慣性SLAM：基于VINS-Fusion正式開課啦
7.徹底搞懂基于LOAM框架的3D激光SLAM: 源碼剖析到算法優(yōu)化
8.徹底剖析室內、室外激光SLAM關鍵算法原理、代碼和實戰(zhàn)(cartographer+LOAM +LIO-SAM)
9.從零搭建一套結構光3D重建系統(tǒng)[理論+源碼+實踐]
10.單目深度估計方法：算法梳理與代碼實現
11.自動駕駛中的深度學習模型部署實戰(zhàn)
12.相機模型與標定(單目+雙目+魚眼）
13.重磅！四旋翼飛行器：算法與實戰(zhàn)
14.ROS2從入門到精通：理論與實戰(zhàn)
15.國內首個3D缺陷檢測教程：理論、源碼與實戰(zhàn)
更多干貨
歡迎加入【3D視覺工坊】交流群，方向涉及3D視覺、計算機視覺、深度學習、vSLAM、激光SLAM、立體視覺、自動駕駛、點云處理、三維重建、多視圖幾何、結構光、多傳感器融合、VR/AR、學術交流、求職交流等。工坊致力于干貨輸出，為3D領域貢獻自己的力量！歡迎大家一起交流成長~
添加小助手微信：dddvision，備注學校/公司+姓名+研究方向即可加入工坊一起學習進步。
