功能包介紹

該功能包提供了一個(gè)手動(dòng)校準(zhǔn)Livox雷達(dá)和相機(jī)之間外參的方法，已經(jīng)在Mid-40，Horizon和Tele-15上進(jìn)行了驗(yàn)證。

其中包含了計(jì)算相機(jī)內(nèi)參，獲得標(biāo)定數(shù)據(jù)，優(yōu)化計(jì)算外參和雷達(dá)相機(jī)融合應(yīng)用相關(guān)的代碼。

本方案中使用了標(biāo)定板角點(diǎn)作為標(biāo)定目標(biāo)物，由于Livox雷達(dá)非重復(fù)性掃描的特點(diǎn)，點(diǎn)云的密度較大，比較易于找到雷達(dá)點(diǎn)云中角點(diǎn)的準(zhǔn)確位置。

相機(jī)雷達(dá)的標(biāo)定和融合也可以得到不錯(cuò)的結(jié)果。

功能包名稱：livox_camera_lidar_calibration

功能包使用環(huán)境：Ubuntu 64-bit 16.04

使用步驟

采集數(shù)據(jù)

連接雷達(dá)檢查標(biāo)定板角點(diǎn)是否在點(diǎn)云中

輸入點(diǎn)云可視化的命令查看點(diǎn)云

roslaunch livox_ros_driver livox_lidar_rviz.launch

這樣就在rviz中顯示了點(diǎn)云

連接相機(jī)檢查標(biāo)定板角點(diǎn)是否在照片中

打開(kāi)相機(jī)，檢查獲取照片的質(zhì)量，并檢查標(biāo)定板角點(diǎn)是否在照片中。具體方法根據(jù)相機(jī)型號(hào)來(lái)了。

采集照片和點(diǎn)云數(shù)據(jù)

拍攝照片運(yùn)行指令錄制點(diǎn)云

roslaunch livox_ros_driver livox_lidar_msg.launchrosbag record /livox/lidar

每個(gè)位置保存一張照片和10s左右的rosbag即可

數(shù)據(jù)采集完成后，將照片放在data/photo文件夾下; 雷達(dá)rosbag放在data/lidar文件夾下

以上步驟數(shù)據(jù)就采集好了，下面進(jìn)行標(biāo)定工作

標(biāo)定角點(diǎn)

設(shè)置相機(jī)內(nèi)參

首先需要把得到的內(nèi)參和畸變糾正參數(shù)以下圖的格式保存在data/parameters/intrinsic.txt文件下 。

distortion下面對(duì)應(yīng)５個(gè)畸變糾正參數(shù)，按順序是k1和k2 (RadialDistortion)，p1和p2 (TangentialDistortion)，最后一個(gè)是k3，一般默認(rèn)是０

獲得照片中的角點(diǎn)坐標(biāo)

配置cornerPhoto.launch文件中的照片路徑，運(yùn)行

roslaunch camera_lidar_calibration cornerPhoto.launch

程序會(huì)在UI中打開(kāi)對(duì)應(yīng)的照片。在這個(gè)UI界面上只要把鼠標(biāo)移到標(biāo)定板的各個(gè)角上，窗口左下角就會(huì)顯示對(duì)應(yīng)的坐標(biāo)數(shù)據(jù)。

確定一個(gè)順序，一般從左上角的角點(diǎn)開(kāi)始，逆時(shí)針旋轉(zhuǎn)按順序記錄下四個(gè)角點(diǎn)坐標(biāo)。

記錄完畢后選中顯示的圖片按任意鍵，進(jìn)入坐標(biāo)輸入流程。

把記錄下的四個(gè)坐標(biāo)”x y”按順序輸入，x和y中間要有空格(比如: “635 487”)，輸入完成后輸入”0 0”即可結(jié)束輸入流程(如下圖例所示)。

程序會(huì)算出四個(gè)更精確的float類型坐標(biāo)顯示出來(lái)，并保存在data/corner_photo.txt中。然后按任意鍵結(jié)束整個(gè)流程。

更改cornerPhoto.launch文件中的照片路徑，重復(fù)上述流程，直至獲得所有照片的角點(diǎn)坐標(biāo)。

獲得雷達(dá)點(diǎn)云中的角點(diǎn)坐標(biāo)

檢查pcdTransfer.launch文件中的rosbag路徑，設(shè)置rosbag的數(shù)量，并將rosbag以0.bag, 1.bag…命名。

運(yùn)行指令將rosbag批量轉(zhuǎn)化成PCD文件，PCD文件默認(rèn)保存在data/pcdFiles文件夾中

roslaunch camera_lidar_calibration pcdTransfer.launch

使用pcl_viewer打開(kāi)PCD文件，按住shift+左鍵點(diǎn)擊即可獲得對(duì)應(yīng)的點(diǎn)坐標(biāo)。注意和照片采用相同的角點(diǎn)順序

pcl_viewer -use_point_picking xx.pcd

將xyz角點(diǎn)坐標(biāo)按如下格式保存在data/corner_lidar.txt中，將所有PCD文件中雷達(dá)點(diǎn)云的角點(diǎn)坐標(biāo)保存下來(lái)。

上面的步驟把相機(jī)和雷達(dá)角點(diǎn)獲得了，也就是同名點(diǎn)，下面就是計(jì)算外參了

參數(shù)設(shè)置

外參計(jì)算節(jié)點(diǎn)會(huì)讀取data/corner_photo.txt和data/corner_lidar.txt中的標(biāo)定數(shù)據(jù)來(lái)計(jì)算外參，數(shù)據(jù)需要保存成特定的格式才能被外參計(jì)算節(jié)點(diǎn)正確讀取。

參考以下格式，每行數(shù)據(jù)只有超過(guò)10個(gè)字母程序才會(huì)將其讀取為計(jì)算的參數(shù)，比如下圖用來(lái)編號(hào)的１２３４，lidar0和0.bmp這些標(biāo)題不會(huì)被讀取為計(jì)算參數(shù)。

程序讀到空行就會(huì)停止讀取參數(shù)開(kāi)始計(jì)算，所以保存時(shí)不要空行。

在計(jì)算前檢查一下雷達(dá)和相機(jī)兩個(gè)標(biāo)定數(shù)據(jù)中是否每行對(duì)應(yīng)的是同一個(gè)角點(diǎn)，并檢查數(shù)據(jù)量是否相同

計(jì)算外參

外參計(jì)算getExt1節(jié)點(diǎn)

計(jì)算前在getExt1.launch文件中配置好外參初值

輸入指令開(kāi)始計(jì)算外參

roslaunch camera_lidar_calibration getExt1.launch

終端中可以看到每次迭代運(yùn)算的cost，外參結(jié)果以齊次矩陣的格式保存到data/parameters/extrinsic.txt下。

可以根據(jù)優(yōu)化后的殘差和重投影誤差評(píng)估一下得到的外參，重投影會(huì)把誤差較大的數(shù)據(jù)打印在屏幕上，可以剔除異常標(biāo)定數(shù)據(jù)后再重新進(jìn)行優(yōu)化計(jì)算。

外參計(jì)算getExt２節(jié)點(diǎn)

getExt1節(jié)點(diǎn)只優(yōu)化外參，而getExt2節(jié)點(diǎn)在計(jì)算的時(shí)候會(huì)將一開(kāi)始計(jì)算的內(nèi)參作為初值和外參一起優(yōu)化。

輸入指令程序會(huì)得到一個(gè)新的內(nèi)參和外參，并用新的參數(shù)來(lái)進(jìn)行重投影驗(yàn)證。

roslaunch camera_lidar_calibration getExt2.launch

一般使用getExt1節(jié)點(diǎn)即可，如果在外參初值驗(yàn)證過(guò)，并且異常值已經(jīng)剔除后，優(yōu)化還是有較大的殘差，那么可以使用getExt2試一試。

使用的前提需要保證標(biāo)定數(shù)據(jù)量較大，并且要充分驗(yàn)證結(jié)果。

如果經(jīng)過(guò)驗(yàn)證getExt2計(jì)算的結(jié)果確實(shí)更好，那么把新的內(nèi)參更新在data/parameters/intrinsic.txt中

通過(guò)上面步驟已經(jīng)得到了外參的結(jié)果，結(jié)果的優(yōu)略可以通過(guò)下面的驗(yàn)證方法

結(jié)果驗(yàn)證

獲得外參后我們可以用兩個(gè)方式看一下融合的效果。第一個(gè)是將點(diǎn)云投影到照片上，第二個(gè)是點(diǎn)云的著色。

投影點(diǎn)云到照片在projectCloud.launch文件中配置點(diǎn)云和照片的路徑后，運(yùn)行指令，將rosbag中一定數(shù)量的點(diǎn)投影到照片上并且保存成新的照片。

roslaunch camera_lidar_calibration projectCloud.launch

點(diǎn)云著色在colorLidar.launch文件中配置點(diǎn)云和照片的路徑，運(yùn)行指令，可以在rviz中檢查著色的效果。

roslaunch camera_lidar_calibration colorLidar.launch

審核編輯：劉清