SegGraph：室外場(chǎng)景三維點(diǎn)云閉環(huán)檢測(cè)算法

廖瑞杰 楊紹發(fā) 孟文霞, 董春梅

1(計(jì)算機(jī)科學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室(中國(guó)科學(xué)院軟件研究所) 北京 100190)2(中國(guó)科學(xué)院大學(xué) 北京 100190)3 (中國(guó)科學(xué)院軟件研究所 北京 100190)

隨著機(jī)器人技術(shù)的快速發(fā)展，移動(dòng)機(jī)器人正在逐步走向完全自主化，而同時(shí)定位與地圖創(chuàng)建(sim-ultaneous localization and mapping, SLAM)是機(jī)器人能否真正實(shí)現(xiàn)完全自主化的關(guān)鍵技術(shù)之一[1].SLAM是指機(jī)器人在未知環(huán)境中從某個(gè)未知位置開(kāi)始移動(dòng)，根據(jù)運(yùn)動(dòng)傳感器(如里程計(jì)等)獲得的信息及環(huán)境傳感器(如相機(jī)、激光雷達(dá)等)獲得的場(chǎng)景信息，在移動(dòng)過(guò)程中同時(shí)自主近似計(jì)算出當(dāng)前位置、姿態(tài)、運(yùn)動(dòng)軌跡并漸進(jìn)地構(gòu)建環(huán)境地圖.每一步的近似計(jì)算和漸進(jìn)建圖都因運(yùn)動(dòng)傳感器的信息偏差而產(chǎn)生誤差，誤差會(huì)不斷累積.閉環(huán)檢測(cè)(loop-closure detection)[2]是避免誤差過(guò)多累積的關(guān)鍵模塊.閉環(huán)檢測(cè)的任務(wù)是根據(jù)環(huán)境傳感器信息判斷當(dāng)前機(jī)器人位置是否與之前某時(shí)刻位置鄰近，以抵消運(yùn)動(dòng)傳感器的累積誤差.機(jī)器人在差距較大的不同時(shí)刻所經(jīng)過(guò)的2個(gè)距離較近的不同位置合稱(chēng)為1個(gè)閉環(huán).閉環(huán)檢測(cè)的準(zhǔn)確度和效率對(duì)SLAM均很關(guān)鍵.如實(shí)際閉環(huán)被正確檢出，則可大幅減少相應(yīng)2個(gè)時(shí)刻位姿等信息估計(jì)的誤差，并進(jìn)而校正全局相關(guān)時(shí)刻位姿和地圖信息的誤差.如對(duì)在實(shí)際相距較遠(yuǎn)位置所得的2組點(diǎn)云誤判為閉環(huán)，則可能導(dǎo)致全局位姿和地圖信息的近似計(jì)算出現(xiàn)較大偏差，甚至導(dǎo)致約束信息不一致而不可解.SLAM的未來(lái)發(fā)展趨勢(shì)是應(yīng)能支持機(jī)器人在大范圍場(chǎng)景長(zhǎng)時(shí)間自主移動(dòng)[3].在這些應(yīng)用場(chǎng)景中，閉環(huán)檢測(cè)尤其關(guān)鍵，難度也更大.

閉環(huán)檢測(cè)研究依應(yīng)用場(chǎng)景大致可分為室內(nèi)場(chǎng)景和室外場(chǎng)景2類(lèi)，依所用環(huán)境傳感器可大致分為基于視覺(jué)傳感器(如雙目相機(jī))所得圖像數(shù)據(jù)和基于三維激光雷達(dá)所得點(diǎn)云數(shù)據(jù)兩大類(lèi).本文專(zhuān)注于基于三維激光雷達(dá)的室外場(chǎng)景閉環(huán)檢測(cè).深度相機(jī)能同時(shí)獲得關(guān)聯(lián)的圖像及點(diǎn)云數(shù)據(jù)，但其只適用于室內(nèi)場(chǎng)景，而且其所得點(diǎn)云深度信息精度遠(yuǎn)小于激光雷達(dá).在室外場(chǎng)景中，由于視覺(jué)傳感器所得圖像對(duì)光照、天氣等的影響甚為敏感，而三維激光雷達(dá)通過(guò)激光對(duì)環(huán)境進(jìn)行高分辨率掃描獲得環(huán)境中各點(diǎn)在雷達(dá)坐標(biāo)系中的位置信息，其所得信息不受光照、天氣等影響，而且精度遠(yuǎn)高于從視覺(jué)傳感器圖像所估計(jì)出來(lái)的位置幾何信息.故室外場(chǎng)景閉環(huán)檢測(cè)當(dāng)前研究更傾向于基于三維激光雷達(dá)所得點(diǎn)云數(shù)據(jù).

Fig. 1 Flow of algorithm SegGraph圖1 SegGraph算法流程

基于三維激光雷達(dá)所得點(diǎn)云的閉環(huán)檢測(cè)本質(zhì)是將2組點(diǎn)云進(jìn)行比較，以判斷它們是否有較高相似度.在SLAM過(guò)程中，將當(dāng)前時(shí)刻所得的1組點(diǎn)云分別與過(guò)去某時(shí)間段內(nèi)選取的若干組點(diǎn)云一一進(jìn)行閉環(huán)檢測(cè)，將判斷為閉環(huán)的2組點(diǎn)云認(rèn)為是在相鄰近位置所得，并將由此2組點(diǎn)云鄰近而產(chǎn)生的全局約束融入到全局的定位與建圖計(jì)算中.本文提出適用于室外場(chǎng)景基于三維激光雷達(dá)所得點(diǎn)云數(shù)據(jù)的新的閉環(huán)檢測(cè)算法，命名為SegGraph.對(duì)作為輸入的2組點(diǎn)云A和B，SegGraph的流程如圖1所示，主要分為點(diǎn)云分割、點(diǎn)云簇圖構(gòu)建、點(diǎn)云簇特征提取、K-公共子圖檢測(cè)四大模塊.

1) SegGraph對(duì)點(diǎn)云A和B分別分割成多個(gè)點(diǎn)云簇，分割前先對(duì)點(diǎn)云作預(yù)處理移除大地平面.大地平面通常是場(chǎng)景中最大的平面及對(duì)點(diǎn)云相似度判定最沒(méi)有參考價(jià)值的平面，并且該平面連接了大部分的小平面如建筑物的外墻、汽車(chē)表面等，移除大地平面能避免其對(duì)點(diǎn)云分割準(zhǔn)確度的影響.

2) 以A中點(diǎn)云簇為頂點(diǎn)、以點(diǎn)云簇圖心間距離為邊權(quán)值構(gòu)建完全的帶權(quán)無(wú)向圖GA，使用同樣的方法對(duì)B構(gòu)建完全圖GB.

3) 判斷GA和GB是否存在有K個(gè)頂點(diǎn)的公共子圖，其中K是設(shè)定的參數(shù).在匹配GA和GB的子圖時(shí)，我們以邊權(quán)值(即點(diǎn)云簇圖心間距離)為主要匹配依據(jù).這是因?yàn)辄c(diǎn)云數(shù)據(jù)中的噪聲及點(diǎn)云分割方法的不完美會(huì)導(dǎo)致從鄰近地點(diǎn)得到的2組點(diǎn)云被分割成差別很大的點(diǎn)云簇集，尤其是同一物體表面可能會(huì)表現(xiàn)為面積差別很大的點(diǎn)云簇.然而點(diǎn)云簇圖心間距離則相對(duì)穩(wěn)定，這在第3節(jié)中有詳細(xì)例子說(shuō)明.另一方面，為提高子圖匹配效率，我們分別對(duì)GA和GB中點(diǎn)云簇提取其具有代表性的局部特征，以用于對(duì)匹配過(guò)程進(jìn)行剪枝操作和優(yōu)化，這些特征包括圖心、法向量等.

K-公共子圖的判定是很著名的NP-hard問(wèn)題[4].我們提出了一個(gè)近似算法，基本思路如下：在初始化階段，分別從GA和GB中選取匹配的邊，并組成匹配的2-公共子圖.假如選擇不唯一，則從符合要求的邊對(duì)中隨機(jī)選取.在遞歸階段，假設(shè)當(dāng)前已找到GA的-子圖HA和GB的-子圖HB，使得HA與HB匹配,其中

本文主要貢獻(xiàn)有3個(gè)方面：

1) 提出以邊匹配為主要依據(jù)的基于K-公共子圖判定的室外場(chǎng)景三維點(diǎn)云閉環(huán)檢測(cè)算法SegGraph，提出解決其中核心問(wèn)題——K-公共子圖判定的近似算法.

2) 基于C++語(yǔ)言和開(kāi)源的第三方點(diǎn)云庫(kù)(point cloud library, PCL)①http:pointclouds.org實(shí)現(xiàn)完整的SegGraph算法，代碼在GitHub上發(fā)布②https:github.comJarilySegGraph.

3) 以廣被采用的KITTI(Karlsruhe Institute of Technology and Toyota Technological Institute)城市街景數(shù)據(jù)集③http:www.cvlibs.netdatasetskittieval_odometry.php評(píng)估SegGraph，實(shí)驗(yàn)結(jié)果顯示SegGraph有良好的準(zhǔn)確度和運(yùn)行效率.

1 相關(guān)研究工作

基于激光雷達(dá)所得三維點(diǎn)云的室外場(chǎng)景閉環(huán)檢測(cè)是近年來(lái)的研究熱點(diǎn)，仍為一大挑戰(zhàn).主要有3個(gè)難點(diǎn)：1)單組點(diǎn)云含點(diǎn)個(gè)數(shù)數(shù)量龐大(如KITTI數(shù)據(jù)集中單組點(diǎn)云約含12萬(wàn)個(gè)點(diǎn))，點(diǎn)在空間分布不均勻，疏密不一；2)點(diǎn)云中各點(diǎn)數(shù)據(jù)噪聲很大，這是目前激光雷達(dá)測(cè)量技術(shù)的不完美所致；3)激光雷達(dá)只能測(cè)量不被阻擋的物體表面的點(diǎn)的空間位置，對(duì)被阻擋的表面無(wú)法取得數(shù)據(jù)，這就可能導(dǎo)致在相鄰很近但視角不同而得的2組點(diǎn)云可能差別很大.閉環(huán)檢測(cè)的本質(zhì)是判別2組點(diǎn)云是否有較高的相似度，從已有研究工作看主要有2類(lèi)方法.

第1類(lèi)方法是2組點(diǎn)云點(diǎn)對(duì)點(diǎn)的直接匹配.在這類(lèi)方法中最著名并被廣泛采用的是最近鄰迭代算法(iterative closest point, ICP)[5]和它的改進(jìn)算法[6].假設(shè)從里程計(jì)信息或其他方式已取得2組點(diǎn)云的大致幾何轉(zhuǎn)換對(duì)應(yīng)關(guān)系，ICP算法利用迭代一步步近似計(jì)算出誤差越來(lái)越小的2組點(diǎn)云的幾何轉(zhuǎn)換對(duì)應(yīng)關(guān)系.ICP算法的局限性是需要已有大致精確的幾何轉(zhuǎn)換對(duì)應(yīng)關(guān)系.點(diǎn)對(duì)點(diǎn)直接匹配方法還有適用范圍更廣并且不需要先驗(yàn)信息的4點(diǎn)(4-points congruent sets, 4PCS)算法[7]和它們的改進(jìn)算法，如超級(jí)4點(diǎn)(super-4PCS)算法[8]等.因單組點(diǎn)云含點(diǎn)個(gè)數(shù)龐大，點(diǎn)對(duì)點(diǎn)直接匹配計(jì)算效率并不高.另外，此方法對(duì)個(gè)別點(diǎn)測(cè)量誤差較為敏感，容易因局部部分點(diǎn)誤差而導(dǎo)致全局匹配的大偏差.

第2類(lèi)方法是基于描述子的點(diǎn)云匹配.首先為2組點(diǎn)云各計(jì)算出1個(gè)較簡(jiǎn)約的描述子，然后通過(guò)比較描述子來(lái)衡量2組點(diǎn)云的相似度.與點(diǎn)對(duì)點(diǎn)直接匹配方法相比，基于描述子的方法計(jì)算量相對(duì)較小，受局部點(diǎn)測(cè)量誤差影響也相對(duì)較小.描述子的計(jì)算方法主要基于3個(gè)方面：1)基于場(chǎng)景點(diǎn)云的局部特征；2)基于場(chǎng)景點(diǎn)云的全局特征；3)基于對(duì)場(chǎng)景點(diǎn)云進(jìn)行分割所得的平面集或者物體集.

基于點(diǎn)云局部特征的描述子算法大多是從點(diǎn)云中選取關(guān)鍵點(diǎn)并從這些關(guān)鍵點(diǎn)抽取局部特征形成特征向量.Bosse和Zlot[9]通過(guò)構(gòu)建1個(gè)投票矩陣計(jì)算每點(diǎn)經(jīng)其最近的若干個(gè)鄰近點(diǎn)投票所得權(quán)值，再基于這些權(quán)值選取關(guān)鍵點(diǎn)建立描述子，稱(chēng)為三維Gestalt描述子.Gawel和Cieslewski[10]也用了類(lèi)似的方法.Zhuang和Jiang[11]則將點(diǎn)云的局部點(diǎn)集轉(zhuǎn)換成方位圖像,并從這些圖像中計(jì)算出SURF(speed up robust features)描述子.Rusu等人[12]采用較經(jīng)典的快速點(diǎn)特征直方圖(fast point feature histograms, FPFH)構(gòu)建描述子.受FPFH描述子構(gòu)建方法的啟發(fā),又產(chǎn)生了視點(diǎn)特征直方圖(viewpoint feature histogram, VFH)[13]描述子和CVFH(clustered viewpoint feature histogram)[14]描述子等.

基于全局特征的描述子計(jì)算主要是提取點(diǎn)云的若干全局特征并加以組合構(gòu)成全局描述子.Rohling等人[15]首先將點(diǎn)云中的點(diǎn)按高度值分成若干層，然后為每層計(jì)算出一維的直方圖，最后將這些直方圖組合起來(lái)構(gòu)成全局描述子.2組點(diǎn)云的全局描述子以它們之間的Wasserstein距離來(lái)衡量相似度.Granstr?m等人[16]提取出點(diǎn)云中具有旋轉(zhuǎn)不變性的全局特征并將其組合構(gòu)成全局描述子，這些具有旋轉(zhuǎn)不變性的特征包括體積、法向量、距離直方圖等.描述子的匹配用機(jī)器學(xué)習(xí)中用于分類(lèi)的AdaBoost算法來(lái)完成，項(xiàng)皓東[17]繼續(xù)了該方法的研究，在一些細(xì)節(jié)方面做出了相關(guān)改進(jìn)，同樣是把機(jī)器學(xué)習(xí)中的相關(guān)算法和傳統(tǒng)方法相結(jié)合.Magnusson等人[18]首先將點(diǎn)云按三維網(wǎng)格劃分為多個(gè)子集，然后計(jì)算每個(gè)子集中局部點(diǎn)云的形狀屬性(如球形、線狀或平面等)，最后將每個(gè)子集的形狀屬性描述組合構(gòu)成點(diǎn)云的全局描述子.He等人[19]首先計(jì)算點(diǎn)云在多個(gè)預(yù)先選定的二維平面上的投影并對(duì)每個(gè)投影構(gòu)建1個(gè)向量，然后將從各投影所得向量組合成1個(gè)全局矩陣，最后以對(duì)全局矩陣進(jìn)行奇異值分解所得的左、右奇異向量構(gòu)成全局描述子.各投影向量描述的計(jì)算方法是將平面分成許多小塊并計(jì)算各小塊里面所包含的點(diǎn)的個(gè)數(shù).

基于點(diǎn)云分割所得平面和物體構(gòu)建描述子是近年來(lái)較新的思路,其能兼顧到點(diǎn)云中的局部特征和全局特征.Fernandez-Moral 等人[20]研究基于RGB深度相機(jī)所得點(diǎn)云數(shù)據(jù)的室內(nèi)場(chǎng)景閉環(huán)檢測(cè)，其方法首先從2組點(diǎn)云中分別檢測(cè)出屬于某個(gè)平面的若干點(diǎn)子集，再對(duì)2組點(diǎn)云的平面進(jìn)行匹配，尋找相匹配的構(gòu)成場(chǎng)景某一局部的2組平面子集.該方法只適用于有較多含平面表面物體的室內(nèi)場(chǎng)景，并依賴(lài)RGB圖像選取場(chǎng)景某一局部，并不適用于室外場(chǎng)景激光雷達(dá)所得三維點(diǎn)云的閉環(huán)檢測(cè).

我們的方法SegGraph受SegMatch及其改進(jìn)方法的啟發(fā)，并克服了這2個(gè)方法的不足.SegMatch在將2組點(diǎn)云所得的點(diǎn)云簇集進(jìn)行匹配時(shí)，忽略了點(diǎn)云簇間距離的信息，故對(duì)閉環(huán)檢測(cè)準(zhǔn)確率影響很大.而改進(jìn)它的方法中，2組點(diǎn)云的點(diǎn)云簇要首先進(jìn)行匹配，但在真實(shí)的室外場(chǎng)景中，受限于點(diǎn)云分割算法的辨識(shí)能力及點(diǎn)云數(shù)據(jù)中的噪聲干擾，從相鄰很近地點(diǎn)所得的點(diǎn)云簇很難形成一對(duì)一的匹配.

SegGraph首先采用受噪聲點(diǎn)干擾程度更小的區(qū)域增長(zhǎng)分割算法將2組點(diǎn)云A和B各分割成多個(gè)點(diǎn)云簇，該方法分割出來(lái)的每個(gè)點(diǎn)云簇近似于1個(gè)光滑的表面，并保留了點(diǎn)云中的關(guān)鍵局部特征.假定A和B具有較高的相似度，即使A和B中有很多點(diǎn)云簇難以形成一對(duì)一的匹配對(duì)，但點(diǎn)云簇間距離是較為穩(wěn)定的信息，因此，我們以A和B中的點(diǎn)云簇為頂點(diǎn)構(gòu)建1個(gè)帶邊權(quán)值的完全圖GA和GB，其中邊的權(quán)值是點(diǎn)云簇圖心間的距離.然后我們?cè)僖赃吰ヅ錇橹?，檢測(cè)GA和GB是否含1個(gè)足夠大的(帶權(quán))公共子圖.在公共子圖檢測(cè)中，我們將點(diǎn)云簇在分割中較穩(wěn)定的局部特征如圖心、法向量等作為輔助判定依據(jù)，以提高公共子圖的檢測(cè)效率.在第4節(jié)中的實(shí)驗(yàn)結(jié)果顯示了SegGraph能夠取得良好的準(zhǔn)確度和運(yùn)行效率.

2 點(diǎn)云分割與點(diǎn)云簇局部特征提取

本節(jié)將詳細(xì)介紹 SegGraph算法的第1步——點(diǎn)云分割，即將2組輸入點(diǎn)云分別經(jīng)移除大地平面后分割成多個(gè)點(diǎn)云簇.為了提高SegGraph中K-公共子圖判定的效率，我們還將從每個(gè)點(diǎn)云簇中提取其具有代表性的局部特征，包括圖心、法向量、點(diǎn)云數(shù)量等.

2.1 大地平面移除

Fig. 2 Visualization of before and after ground plane removal for point cloud 2, sequence 06 KITTI圖2 KITTI 06 序列的第 2 組點(diǎn)云移除大地平面前后的可視化

SegGraph專(zhuān)注于處理室外場(chǎng)景的點(diǎn)云數(shù)據(jù)，大多數(shù)此類(lèi)場(chǎng)景如城市街景等均有1個(gè)較顯著的大地平面.通常大地平面是場(chǎng)景中面積最大的平面，也是對(duì)場(chǎng)景相似度比較最沒(méi)有參考價(jià)值的平面.故先將大地平面移除可以大大減少場(chǎng)景相似度比較的計(jì)算量.

SegGraph采用一致性分割算法(SAC segmen-tation)[24]來(lái)移除點(diǎn)云中的大地平面.一致性分割算法的功能是從點(diǎn)云中提取與指定目標(biāo)模型相對(duì)應(yīng)的點(diǎn)云子集，如平面、球體、圓柱等.SegGraph用該算法提取出點(diǎn)云中最大的1個(gè)平面，認(rèn)為其是大地平面，并將其移除.圖2顯示了KITTI數(shù)據(jù)集 06序列第2組三維點(diǎn)云數(shù)據(jù)的可視化圖像和將該組點(diǎn)云移除大地平面后的可視化圖像.

2.2 點(diǎn)云分割

SegGraph采用區(qū)域增長(zhǎng)算法[25]對(duì)移除大地平面后的點(diǎn)云數(shù)據(jù)進(jìn)行分割.其算法包含3個(gè)步驟：

1) 計(jì)算曲率.對(duì)點(diǎn)云中每個(gè)點(diǎn)p，以與p在1個(gè)很小的指定范圍內(nèi)的鄰近點(diǎn)子集信息計(jì)算p的曲率.

2) 選取初始種子點(diǎn)集.以點(diǎn)云中曲率小于指定閾值的點(diǎn)構(gòu)成初始種子點(diǎn)集.

3) 區(qū)域增長(zhǎng).從種子點(diǎn)集中取出1個(gè)點(diǎn)p，將以p為基礎(chǔ)點(diǎn)構(gòu)建1個(gè)可以近似認(rèn)為是平滑表面的點(diǎn)云簇，該點(diǎn)云簇包含與p距離在一定范圍內(nèi)且與p的法向量夾角小于指定閾值的所有點(diǎn).

點(diǎn)云數(shù)據(jù)經(jīng)移除大地平面和分割后，將形成1個(gè)點(diǎn)云簇集，每個(gè)點(diǎn)云簇可以近似地視為場(chǎng)景中某個(gè)平滑表面的一部分.圖3顯示了從KITTI數(shù)據(jù)集06序列第2組點(diǎn)云所得點(diǎn)云簇集合的可視化圖像，其中，同一個(gè)點(diǎn)云簇內(nèi)的點(diǎn)以相同顏色顯示，相鄰的點(diǎn)云簇用不同的顏色顯示.

Fig.3 Visualization of point clusters resulting from segmentizing point cloud 2, sequence 06, KITTI圖3 KITTI 06序列第2組點(diǎn)云分割所得點(diǎn)云簇集的可視化

2.3 點(diǎn)云簇局部特征提取

為提高SegGraph后續(xù)K-公共子圖檢測(cè)的效率，我們對(duì)點(diǎn)云分割所得點(diǎn)云簇提取關(guān)鍵的局部特征.對(duì)每個(gè)點(diǎn)云簇，我們計(jì)算出圖心、法向量、曲率、點(diǎn)的個(gè)數(shù)等信息.這些信息將輔助SegGraph后續(xù)K-公共子圖檢測(cè)進(jìn)一步優(yōu)化.

3 K-公共子圖檢測(cè)

SegGraph的第2步是就2組點(diǎn)云A和B分割所得的點(diǎn)云簇構(gòu)建帶權(quán)值的完全圖GA和GB，并以檢測(cè)GA和GB是否含有K-公共子圖來(lái)判定A和B的相似度.從點(diǎn)云簇提取的特征將作為輔助匹配信息以提高K-公共子圖檢測(cè)的效率.本節(jié)將詳述SegGraph中建圖與K-公共子圖檢測(cè)算法.

3.1 建 圖

3.2 K-公共子圖檢測(cè)

我們將判定2組點(diǎn)云A和B是否相似轉(zhuǎn)化為檢測(cè)GA和GB是否含有K-公共子圖，其中K是預(yù)先設(shè)定的整數(shù).我們稱(chēng)GA和GB含有K-公共子圖當(dāng)且僅當(dāng)存在UA?VA，UB?VB，|UA|=K=|UB|，而且以UA為頂點(diǎn)集的子圖HA與以UB為頂點(diǎn)集的子圖HB能相匹配，其中HA和HB為完全圖.更確切地說(shuō)，存在從UA到UB的一一對(duì)應(yīng)的映射f，滿足：

2) 對(duì)UA中任意頂點(diǎn)uA，uA所代表的點(diǎn)云簇的圖心、法向量等特征與f(uA)所代表的點(diǎn)云簇的相應(yīng)特征的差值在指定閾值內(nèi).

第1節(jié)提到的SegMatch及其改進(jìn)方法均基于上述的假設(shè)，即依據(jù)點(diǎn)云簇的形狀、面積及其他局部特征能使2組點(diǎn)云A和B分割所得的大多數(shù)點(diǎn)云簇能夠形成唯一匹配，即A中大多數(shù)點(diǎn)云簇vA能在VB中找到唯一1個(gè)vB，使得vA與vB的形狀、面積及其他局部特征匹配.而這個(gè)假設(shè)在實(shí)際場(chǎng)景所得點(diǎn)云數(shù)據(jù)的分割中是很難滿足的.主要原因有3點(diǎn)：

1) 激光雷達(dá)測(cè)量的誤差會(huì)使三維點(diǎn)云數(shù)據(jù)含有一定的噪聲，并且激光雷達(dá)所獲取的點(diǎn)云中點(diǎn)的分布并不均勻，且疏密不一.

2) 激光雷達(dá)只能掃描未被阻擋的物體表面，視點(diǎn)與視角的輕微變化及場(chǎng)景中瞬間出現(xiàn)的動(dòng)態(tài)物體如車(chē)輛等均會(huì)使點(diǎn)云數(shù)據(jù)產(chǎn)生較大的偏差.

3) 點(diǎn)云分割算法有局限，無(wú)論是SegMatch中采用的歐基里德分割算法，還是本文采用的區(qū)域增長(zhǎng)分割算法，均不能完美地使分割出來(lái)的點(diǎn)云簇與場(chǎng)景中的實(shí)際物體表面一一對(duì)應(yīng)，其他點(diǎn)云分割算法也無(wú)法避免此問(wèn)題.

因而，我們將點(diǎn)云簇間距離作為點(diǎn)云相似度比較的主要依據(jù).圖4展示了KITTI數(shù)據(jù)集06序列第2組點(diǎn)云和第836組點(diǎn)云在分割后所得點(diǎn)云簇集的可視化圖像.

在圖4的2組點(diǎn)云局部圖中，同個(gè)點(diǎn)云簇內(nèi)的點(diǎn)以相同顏色顯示，鄰近的點(diǎn)云簇之間用不同的顏色顯示，根據(jù)KITTI提供的位姿信息，這2組點(diǎn)云獲取地點(diǎn)(即三維激光雷達(dá)所處位置)相差僅為0.265 m，可以將2組點(diǎn)云近似看作是對(duì)同一個(gè)場(chǎng)景的掃描，圖4中X與Y為同一個(gè)位置分割出來(lái)的2個(gè)點(diǎn)云簇，但其面積差別很大，X′與Y′也是類(lèi)似的情況，但X圖心與X′圖心間距XX′和Y圖心與Y′圖心間距YY′相差卻不大，可作為2組點(diǎn)云相似度比較的主要依據(jù).另一方面，X與Y，X′與Y′的圖心、法向量等信息基本一致，可以作為輔助信息提高點(diǎn)云相似度比較的效率.

K-公共子圖檢測(cè)是NP-hard問(wèn)題.我們提出一種窮盡搜索算法來(lái)求解.

算法1.K-公共子圖窮盡搜索算法.

輸入：完全圖GA，GB，整數(shù)K，以VA，VB分別記GA，GB的頂點(diǎn)集，以w(v,v′)記GA或GB中邊(v,v′)的權(quán)值；

輸出：布爾值，即GA，GB是否含K-公共子圖.

①w(eA)與w(eB)相差小于指定閾值；

2) 初始化.令UA，UB為空集，=0，fAB為空映射.在K-公共子圖搜索過(guò)程中，UA是GA頂點(diǎn)集的子集，UB是GB頂點(diǎn)集的子集，fAB是從UA到UB的一一映射.UA，UB，fAB聯(lián)合代表已構(gòu)建的含個(gè)頂點(diǎn)的公共子圖，其中|UA|==|UB|，≤K.

3) 公共子圖搜索.調(diào)用算法2搜索公共子圖，DetectSubGraph(GA,GB,K,UA，UB,fAB,).

算法2.DetectSubGraph公共子圖搜索算法.

輸入：GA,GB,K,UA,UB,fAB,,其中GA,GB,K同算法1中，UA，UB，fAB聯(lián)合構(gòu)成GA，GB的有個(gè)頂點(diǎn)的公共子圖，且≤K；

輸出：布爾值，即UA，UB，fAB是否能擴(kuò)展為GA，GB的K-公共子圖.

2) 枚舉所有滿足下列條件的頂點(diǎn)對(duì)(vA,vB)，記為CVP(candidate vertex pairs).

①vA是GA中在UA外的頂點(diǎn)，vB是GB中在UB外的頂點(diǎn).

3) 依次對(duì)CVP的頂點(diǎn)對(duì)(vA,vB)執(zhí)行下列操作.

① 遞歸調(diào)用DetectSubGraph(GA,GB,K,UA∪{vA}UB∪{vB},fAB∪{(vA,vB)},+1).

② 如果返回值為真，則返回真并退出算法.

4) 若至此，則說(shuō)明CVP中不存在頂點(diǎn)對(duì)能與UA，UB，fAB聯(lián)合擴(kuò)展為GA，GB的K-公共子圖，故搜索失敗，返回假并退出算法.

3.3 K-公共子圖檢測(cè)近似算法

對(duì)實(shí)際移動(dòng)機(jī)器人定位與建圖應(yīng)用，我們提出更高效的K-公共子圖檢測(cè)的隨機(jī)搜索算法.隨機(jī)搜索算法與窮盡搜索算法架構(gòu)相同，只需將DetectSubGraph算法的步驟3)和步驟4)替換為下列步驟：

隨機(jī)從CVP中選取1個(gè)頂點(diǎn)對(duì)(vA,vB)，然后執(zhí)行以下步驟.

① 遞歸調(diào)用DetectSubGraph(GA,GB,K,UA∪{vA}UB∪{vB},fAB∪{(vA,vB)},+1).

② 如果返回值為真，則返回真并退出算法；如果返回值為假，則返回假并退出算法.

很顯然，窮盡搜索的最壞復(fù)雜度是指數(shù)級(jí)的，隨機(jī)搜索的復(fù)雜度則是多項(xiàng)式級(jí)的.我們?cè)贙ITTI數(shù)據(jù)集的實(shí)驗(yàn)表明基于K-公共子圖隨機(jī)搜索算法的閉環(huán)檢測(cè)與基于K-公共子圖窮盡搜索算法的閉環(huán)檢測(cè)這兩者的準(zhǔn)確度差別不大，但是在時(shí)間耗損上，隨機(jī)搜索算法具有巨大的優(yōu)勢(shì).

4 實(shí)驗(yàn)與結(jié)果

本文所詳述的完整的SegGraph算法均以C++實(shí)現(xiàn)(KITTI數(shù)據(jù)預(yù)處理部分利用了Python語(yǔ)言和KITTI官方提供的Python第三方庫(kù)實(shí)現(xiàn)).我們采用開(kāi)源的PCL庫(kù)[26]來(lái)實(shí)現(xiàn)算法模塊的部分功能和可視化操作等.程序運(yùn)行在1個(gè)圖形工作站上，其CPU為英特爾雙核i7-2600,主頻3.4 GHz，內(nèi)存4 GB，操作系統(tǒng)64位Windows 7.我們選取KITTI三維點(diǎn)云數(shù)據(jù)集[27]中適合用于閉環(huán)檢測(cè)算法評(píng)估的00，05，06，07序列數(shù)據(jù)進(jìn)行實(shí)驗(yàn).這4個(gè)序列的三維點(diǎn)云數(shù)據(jù)是以車(chē)載三維激光雷達(dá)在真實(shí)城市街道中掃描獲得.KITTI官方提供由高精度GPSIMU導(dǎo)航系統(tǒng)測(cè)得的準(zhǔn)確激光雷達(dá)位姿信息，以供閉環(huán)檢測(cè)算法評(píng)估之用.這4個(gè)序列中各組點(diǎn)云的位姿信息用MatLab軟件可視化所得軌跡如圖5所示.

圖5中軌跡上每個(gè)點(diǎn)代表1組點(diǎn)云(即1次掃描)的位置.激光雷達(dá)的運(yùn)動(dòng)是從黃色的點(diǎn)開(kāi)始，由黃綠藍(lán)漸變的方向移動(dòng)，到達(dá)藍(lán)色的點(diǎn)終止.KITTI的00，05，06，07序列分別含4 541，2 761，1 101，1 101組點(diǎn)云.每組點(diǎn)云由激光雷達(dá)在某個(gè)位置掃描而得，約有12萬(wàn)個(gè)點(diǎn).我們以數(shù)據(jù)集提供的位姿信息作為評(píng)估閉環(huán)檢測(cè)算法準(zhǔn)確度的依據(jù).每個(gè)序列的各組點(diǎn)云是依次在運(yùn)行軌跡上的位置掃描所得，所以對(duì)掃描次序間隔不大的2組點(diǎn)云，其對(duì)機(jī)器人定位與建圖依里程計(jì)信息估計(jì)的誤差并不大，不需再?gòu)沫h(huán)境信息加以校正.因而我們?cè)趯?shí)驗(yàn)中只選取部分掃描次序間隔大于50的點(diǎn)云對(duì)作為閉環(huán)檢測(cè)實(shí)驗(yàn)的樣本，即對(duì)我們選取的每個(gè)點(diǎn)云對(duì)樣本(A,B)，A和B屬于同一個(gè)序列，若A是第iA次掃描所得，B是第iB次掃描所得，則|iA-iB|>50.

Fig. 5 Pose information of KITTI sequences 00,05,06,07圖5 KITTI 00，05，06，07序列的位姿信息

對(duì)每個(gè)點(diǎn)云對(duì)樣本(A,B)，設(shè)A和B分別是激光雷達(dá)位于全局三維坐標(biāo)系中位置pA和pB處獲得，若pA，pB相距(歐基里德距離)小于3 m，則認(rèn)為A，B構(gòu)成閉環(huán)，稱(chēng)(A,B)為1個(gè)正樣本；若pA，pB相距大于或等于3 m，則認(rèn)為A，B不構(gòu)成閉環(huán)，稱(chēng)(A,B)為1個(gè)負(fù)樣本.

按上述實(shí)驗(yàn)設(shè)定，我們計(jì)算出KITTI 00序列共有7 403個(gè)正樣本，由于負(fù)樣本數(shù)量太多(由圖5可知，激光雷達(dá)車(chē)重復(fù)經(jīng)過(guò)的點(diǎn)占比很少)，我們只從00序列中負(fù)樣本中隨機(jī)選取10 274個(gè)，即對(duì)00序列，共采用17 677個(gè)樣本進(jìn)行閉環(huán)檢測(cè)算法評(píng)估.同理，對(duì)05，06，07序列，我們選擇所有的正樣本并隨機(jī)選取部分負(fù)樣本進(jìn)行實(shí)驗(yàn)，具體來(lái)說(shuō)，05序列共采用4 827個(gè)正樣本和10 274個(gè)負(fù)樣本，06序列1 577個(gè)正樣本和10 794個(gè)負(fù)樣本，07序列1 858個(gè)正樣本和11 242個(gè)負(fù)樣本.

我們依慣例[16]以檢測(cè)率D(detection rate)，丟失率MD(missed detection rate)，誤報(bào)率FA(false alarm rate)三個(gè)指標(biāo)來(lái)衡量閉環(huán)檢測(cè)的準(zhǔn)確度.令P表示實(shí)驗(yàn)中正樣本總數(shù)，N表示實(shí)驗(yàn)中負(fù)樣本總數(shù).用TP表示實(shí)驗(yàn)中被閉環(huán)檢測(cè)算法判定為閉環(huán)而實(shí)際為閉環(huán)的樣本數(shù)量，用FN表示被算法判定為非閉環(huán)而實(shí)際為閉環(huán)的樣本數(shù)量，用FP表示被算法判定為閉環(huán)而實(shí)際為非閉環(huán)的樣本數(shù)量，則3個(gè)指標(biāo)的計(jì)算公式為

其中,D+MD=1.

好的閉環(huán)檢測(cè)算法應(yīng)做到使檢測(cè)率很高，丟失率偏低，而誤報(bào)率極低，這是因?yàn)樵跈C(jī)器人即時(shí)定位與建圖應(yīng)用中，閉環(huán)通常是在機(jī)器人軌跡中成段連續(xù)出現(xiàn)的，見(jiàn)圖5中黃色點(diǎn)和藍(lán)色點(diǎn)的重疊部分.具體地說(shuō)，如果存在第i組點(diǎn)云(由第i次掃描所得)與第j組點(diǎn)云形成閉環(huán)(i

每個(gè)序列參數(shù)K的設(shè)定是對(duì)該序列分別以K=7,8,9,10,11,12運(yùn)行并綜合選取準(zhǔn)確度最優(yōu)的對(duì)應(yīng)K值，原則是誤報(bào)率應(yīng)極低而檢測(cè)率較高.總的來(lái)說(shuō)，單組點(diǎn)云分割后所建的圖頂點(diǎn)數(shù)約為40，K的設(shè)定要適當(dāng).若K取得太小，則容易將許多稍有相似但實(shí)際上不是閉環(huán)的點(diǎn)云對(duì)誤判為閉環(huán)；若K取得太大，則因點(diǎn)云數(shù)據(jù)噪聲影響容易將許多實(shí)際為閉環(huán)的點(diǎn)云對(duì)誤判為非閉環(huán)，并且導(dǎo)致K-公共子圖判定的運(yùn)行時(shí)間過(guò)大.表1列出了KITTI 06序列在K取不同值時(shí)的實(shí)驗(yàn)結(jié)果：

Table 1 Accuracy of SegGraph on KITTI Sequence 06 forDifferent Values of K

從表1可以看出，當(dāng)K=10時(shí)，SegGraph在KITTI 06序列上的誤報(bào)率只有0.26%，而檢測(cè)率仍高達(dá)94.23%，所以對(duì)這個(gè)序列我們將參數(shù)K設(shè)定為10.其他序列參數(shù)K的設(shè)定與06序列同理.我們最終的實(shí)驗(yàn)結(jié)果見(jiàn)表2:

Table 2 Accuracy of SegGraph on KITTI Sequences00, 05, 06, 07

實(shí)驗(yàn)表明我們提出的SegGraph閉環(huán)檢測(cè)算法在KITTI 4個(gè)序列中都取得了理想的檢測(cè)率和較低的丟失率.在誤報(bào)率方面，06序列的效果最為明顯，僅為0.26%；00，05序列誤報(bào)率都能控制在3%左右；07序列的誤報(bào)率偏高，達(dá)到了8.74%，由圖5的位姿信息可看出，07序列和其他序列最大的不同在于07序列中激光雷達(dá)車(chē)幾乎沒(méi)有重復(fù)走過(guò)同一段路線，即圖5中顏色幾乎沒(méi)有重合的部分，所以導(dǎo)致了閉環(huán)信息較少，同時(shí)07序列中有2段相距很遠(yuǎn)但結(jié)構(gòu)與外觀都非常相似的街道，這就導(dǎo)致了算法將這些不是閉環(huán)的樣本誤判為閉環(huán)，該序列在其他閉環(huán)檢測(cè)算法的實(shí)驗(yàn)中[19]效果也不理想.

我們還測(cè)量了SegGraph的運(yùn)行時(shí)間，對(duì)單個(gè)點(diǎn)云對(duì)，SegGraph運(yùn)行分兩大部分：第1部分是點(diǎn)云分割和點(diǎn)云簇局部特征提取時(shí)間；第2部分是K-公共子圖檢測(cè).由于正樣本與負(fù)樣本在第2部分所花時(shí)間差距甚大，我們對(duì)正負(fù)樣本在第2部分所花時(shí)間分開(kāi)測(cè)量.表3給出了KITTI 06序列的運(yùn)行時(shí)間統(tǒng)計(jì)(單位為s)，其中tmin和tmax分別表示單個(gè)點(diǎn)云對(duì)的最小耗時(shí)和最大耗時(shí)，tavg表示所有點(diǎn)云對(duì)的平均耗時(shí).其他序列的運(yùn)行時(shí)間與06序列類(lèi)似.

Table 3 Running Time of SegGraph on KITTI Sequence 06表3 SegGraph在KITTI 06序列上的運(yùn)行時(shí)間 s

從表3可以看出，SegGraph第2部分時(shí)間中，正樣本的檢測(cè)時(shí)間大約是負(fù)樣本檢測(cè)時(shí)間的4倍，這是由于在正樣本中，邊匹配成功的較多，相應(yīng)迭代的次數(shù)就更多，所以花費(fèi)的時(shí)間就更長(zhǎng).從總體時(shí)間上看，第1部分占據(jù)了大部分時(shí)間，這是因?yàn)樵谠撨^(guò)程中，使用了區(qū)域增長(zhǎng)算法來(lái)進(jìn)行點(diǎn)云分割，該算法需要求出點(diǎn)云中每個(gè)點(diǎn)的曲率和法向量，這個(gè)步驟需要消耗大量的時(shí)間.相比于其他適用于城市街道的點(diǎn)云分割算法如歐基里德分割算法，雖然區(qū)域增長(zhǎng)算法在時(shí)間上不占優(yōu)勢(shì)，但是歐基里德算法分割的結(jié)果為無(wú)規(guī)則狀的點(diǎn)云簇，而我們所采用的區(qū)域增長(zhǎng)分割算法分割出來(lái)的點(diǎn)云簇則非常接近光滑的平面，這使得K-公共子圖檢測(cè)中基于點(diǎn)云簇局部特征優(yōu)化更加簡(jiǎn)潔和高效.同時(shí)，區(qū)域增長(zhǎng)分割算法相對(duì)歐基里德分割算法來(lái)說(shuō)更為精確且魯棒性更好，前者基于曲率的變化增長(zhǎng)，后者根據(jù)距離的遠(yuǎn)近聚類(lèi)[22]，假如存在一些雷達(dá)掃描不可避免的噪聲點(diǎn)，很有可能本應(yīng)屬于2個(gè)點(diǎn)云簇的局部點(diǎn)云被歐基里德算法聚為了同一類(lèi)，這是因?yàn)樵肼朁c(diǎn)干擾了算法中對(duì)距離的判定.而區(qū)域增長(zhǎng)分割算法受噪聲點(diǎn)干擾的程度遠(yuǎn)沒(méi)有歐基里德分割算法大.

區(qū)域增長(zhǎng)分割算法中的主要耗時(shí)在于求點(diǎn)云中每個(gè)點(diǎn)的曲率和法向量.每個(gè)點(diǎn)的曲率和法向量只需從其鄰近若干點(diǎn)的幾何信息計(jì)算得出[25]，因而對(duì)點(diǎn)云中各個(gè)點(diǎn)，其曲率及法向量的計(jì)算可以并行執(zhí)行.如果將這部分計(jì)算在多核或GPU平臺(tái)上實(shí)現(xiàn)，將能大大降低區(qū)域增長(zhǎng)分割算法的運(yùn)行時(shí)間.這將是我們下一步的工作.

5 總 結(jié)

本文提出適用于室外場(chǎng)景三維點(diǎn)云數(shù)據(jù)的閉環(huán)檢測(cè)算法SegGraph.算法主要有3個(gè)步驟：1)對(duì)輸入的2組點(diǎn)云分別移除大地平面后采用區(qū)域增長(zhǎng)算法分割為點(diǎn)云簇集，并提取每個(gè)點(diǎn)云簇的局部特征；2)分別以點(diǎn)云簇集為頂點(diǎn)集，以點(diǎn)云簇間距離為邊的權(quán)值，構(gòu)建無(wú)向完全圖；3)通過(guò)檢測(cè)它們是否含K-公共子圖來(lái)判定2組輸入點(diǎn)云是否有較高的相似度，其中K是根據(jù)實(shí)際情況可以調(diào)整的參數(shù).我們提出了檢測(cè)K-公共子圖的高效隨機(jī)搜索近似算法.在公開(kāi)數(shù)據(jù)集KITTI的4個(gè)序列上的實(shí)驗(yàn)表明SegGraph有良好的準(zhǔn)確度和運(yùn)行效率.