基于激光點(diǎn)云語(yǔ)義信息的閉環(huán)檢測(cè)

GOSMatch: Graph-of-Semantics Matching for Detecting Loop Closures in 3D LiDAR data

摘要

利用激光雷達(dá)的點(diǎn)云信息檢測(cè)閉環(huán)是一件很不容易的事情，因?yàn)榧す饫走_(dá)的數(shù)據(jù)是不穩(wěn)定的。本文提出了一種叫做GOSMatch的方法依靠語(yǔ)義級(jí)別的信息做場(chǎng)景重識(shí)別，本文的方法利用語(yǔ)義物體之間的空間關(guān)系產(chǎn)生新穎的描述符。本文還設(shè)計(jì)了一種由粗到細(xì)的測(cè)量來(lái)有效的找到閉環(huán)。此外， 一旦確定找到了閉環(huán)幀，系統(tǒng)就會(huì)給出準(zhǔn)確的六自由度的位姿估計(jì)結(jié)果，本文在kitti做了測(cè)試取得了不錯(cuò)的效果。

介紹

當(dāng)前傳統(tǒng)的激光閉環(huán)檢測(cè)方法是利用局部的特征點(diǎn)或者其他全局的特征，這種方法太注意局部的細(xì)節(jié)而忽略了更高層的特征約束。本文提出了一種利用語(yǔ)義信息來(lái)檢測(cè)閉環(huán)的方法。本文利用語(yǔ)義信息構(gòu)建了局部和全局的描述子。全局的描述子來(lái)高效的查找最相似的前K個(gè)閉環(huán)候選幀，局部的描述子用來(lái)計(jì)算當(dāng)前幀和閉環(huán)候選幀之間的距離。本文的主要貢獻(xiàn)如下：

• 提出了GOSMatch，一個(gè)在城市道路場(chǎng)景下僅利用雷達(dá)觀(guān)測(cè)的基于物體級(jí)語(yǔ)義信息來(lái)可靠場(chǎng)景識(shí)別的方法。

• 通過(guò)編碼語(yǔ)義物體之間的空間關(guān)系來(lái)構(gòu)建全局和局部描述符以高效的進(jìn)行兩步閉環(huán)搜索。

• 開(kāi)源了GOSMatch到https://github.com/zhuyachen/GOSMatch，但是代碼是m文件

• 相關(guān)工作

A.傳統(tǒng)的基于雷達(dá)的場(chǎng)景識(shí)別

從點(diǎn)云中隨機(jī)選擇特征點(diǎn)提取區(qū)域形狀描述符。隨后，使用投****策略在特征點(diǎn)中找到最近鄰。

在range image中檢測(cè)每個(gè)特征點(diǎn)的描述向量，利用kdtree來(lái)管理高維向量。為了達(dá)到更好的效果，可以利用Normal Aligned RadialFeature (NARF)描述符和詞袋匹配的方法。

其他的方法關(guān)注點(diǎn)云的表示，如直方圖；基于外觀(guān)的方法如NDT；還有很有名的Scan Context，把點(diǎn)云分成2D的小方塊并編碼為點(diǎn)云高度的最大值。由于匹配這種矩陣類(lèi)型的描述子需要很大的計(jì)算量，Kim改進(jìn)Scan Context，利用ring key來(lái)構(gòu)建kd tree來(lái)加速搜索。

B. 基于high-level描述子的場(chǎng)景重識(shí)別

提取點(diǎn)云中的線(xiàn)面特征，然后基于ICP去找閉環(huán)

可以利用聚類(lèi)的方法得到更魯棒的描述子

C. 利用圖表示的數(shù)據(jù)關(guān)聯(lián)

圖匹配對(duì)于成對(duì)的數(shù)據(jù)關(guān)聯(lián)問(wèn)題很重要。圖表示是描述對(duì)象及其拓?fù)涞某Ｓ梅椒āＴ谶@種情況下，找到兩個(gè)場(chǎng)景之間的對(duì)象的關(guān)聯(lián)轉(zhuǎn)換為計(jì)算圖之間的頂點(diǎn)和邊的對(duì)應(yīng)關(guān)系。但是，找到解決此問(wèn)題的精確方法始終是NP-hard。有工作生成一個(gè)對(duì)應(yīng)圖，然后搜索圖中最大的clique來(lái)得到兩個(gè)圖中頂尖和邊的對(duì)應(yīng)關(guān)系。但是這種方法只適合圖中頂點(diǎn)少的情況。

為了減少計(jì)算的復(fù)雜度，一種有效的方法是找到問(wèn)題可以容忍的近似解。其中一個(gè)工作X-view: Graph-based semantic multi-view localization是基于隨機(jī)游走技術(shù)的圖像核。圖中的每個(gè)節(jié)點(diǎn)，產(chǎn)生一定數(shù)量的游走序列作為節(jié)點(diǎn)的描述子，然后執(zhí)行匹配的過(guò)程來(lái)得到節(jié)點(diǎn)和節(jié)點(diǎn)之間的對(duì)應(yīng)關(guān)系。還有Characterizing structural relationships in scenes using graph kernels和Image classification with segmentation graph kernels也是相似的工作。

本文的方法

本文的方法如下圖所示，主要包含四個(gè)主要的模塊：語(yǔ)義信息獲取，圖描述子生成，節(jié)點(diǎn)匹配和幾何校驗(yàn)。

A. 語(yǔ)義檢測(cè)

有用的語(yǔ)義特征應(yīng)該是穩(wěn)定的，容易區(qū)分的，可重復(fù)的。本文專(zhuān)注于檢測(cè)停著的車(chē)輛，卡車(chē)，桿以及在城市道路場(chǎng)景中常見(jiàn)的特征。盡管停著的車(chē)輛是潛在運(yùn)動(dòng)的物體，這意味著舊的車(chē)可能會(huì)開(kāi)走，新的車(chē)停在那，實(shí)際中的閉環(huán)經(jīng)常時(shí)間間隔是很短的。因此在本文的試驗(yàn)中作者認(rèn)為停放的車(chē)輛與任何其他語(yǔ)義特征一樣重要，并且假設(shè)在兩次觀(guān)測(cè)期間，停放的車(chē)輛在同一位置的位置變化不大。（這里如果是基于穩(wěn)定圖層的閉環(huán)策略，這個(gè)假設(shè)應(yīng)該就不成立了，但是可以利用潛在運(yùn)動(dòng)的物體去匹配，只不過(guò)需要給他一個(gè)比較小的權(quán)重）

本文采用RangNet++來(lái)檢測(cè)3D激光數(shù)據(jù)的語(yǔ)義信息。這個(gè)端到端的神經(jīng)網(wǎng)絡(luò)檢測(cè)原始點(diǎn)云中每個(gè)點(diǎn)的語(yǔ)義信息。需要注意的是，我們不能通過(guò)一個(gè)點(diǎn)云的語(yǔ)義信息知道這個(gè)卡車(chē)是在運(yùn)動(dòng)還是在停著，所以利用前端的里程計(jì)來(lái)估計(jì)車(chē)輛的速度來(lái)辨別是停著的還是移動(dòng)的。

（最近查閱資料的時(shí)候大概了解到，基于激光點(diǎn)云的語(yǔ)義分割方法很難達(dá)到實(shí)時(shí)，而且目前類(lèi)別的標(biāo)簽只有六類(lèi)左右，但是基于視覺(jué)的語(yǔ)義分割方法很成熟，所以我們完全可以基于視覺(jué)得到標(biāo)簽，投影到點(diǎn)云上得到每個(gè)點(diǎn)云的標(biāo)簽，這樣不僅可以保證速度，而且可以保證類(lèi)別的多樣性。）

獲得了點(diǎn)云的語(yǔ)義標(biāo)簽后，利用歐式聚類(lèi)來(lái)檢索對(duì)象。對(duì)于所有的對(duì)象，通過(guò)計(jì)算他們的質(zhì)心來(lái)表示他們?cè)邳c(diǎn)云中的位置。

B. 圖描述子的生成

單次激光雷達(dá)的掃描可以由無(wú)向圖G=<V, E>表示，其中V和E分別代表頂點(diǎn)和邊的集合。把利用語(yǔ)義分割網(wǎng)絡(luò)得到的物體的位置作為頂點(diǎn)G，圖中的每個(gè)邊表示兩個(gè)頂點(diǎn)之間的歐氏距離。根據(jù)語(yǔ)義分割的結(jié)果，圖中有三種頂點(diǎn)(vehicle, trunk and pole)，六種邊(vehicle-vehicle, trunk-trunk, pole-pole,vehicle-trunk, trunk-pole and pole-vehicle)。（這里如果類(lèi)別比較多，計(jì)算量應(yīng)該就會(huì)很大，所以我們應(yīng)該選擇比較好的特征作為頂點(diǎn)和邊）。

基于直方圖的圖描述子由六部分組成，代表著上邊提到的六種邊。這里以pole-trunk邊為例介紹一下邊的計(jì)算流程。我們假設(shè)一個(gè)連續(xù)的bin標(biāo)號(hào)為b，這個(gè)頂點(diǎn)最短的邊為l_min，最長(zhǎng)的邊為l_max

每次掃描得到的圖描述符都存儲(chǔ)在數(shù)據(jù)庫(kù)中，當(dāng)需要查詢(xún)點(diǎn)云的時(shí)候，利用構(gòu)建kd樹(shù)來(lái)執(zhí)行k近鄰算法來(lái)快速得到最相似的N個(gè)候選閉環(huán)幀。

C. 頂點(diǎn)匹配

在本節(jié)，我們介紹頂點(diǎn)描述符來(lái)描述圖中的頂點(diǎn)。和圖描述符相似，頂點(diǎn)描述符也是基于直方圖的。和邊的描述符不同的是，頂點(diǎn)描述符中考慮的邊不再是整個(gè)圖中全部的邊，而是連接到所描述頂點(diǎn)v的邊。為了給頂點(diǎn)v（pole）構(gòu)建一個(gè)描述符，只有三種邊需要考慮，因?yàn)闃?gòu)成邊的兩個(gè)端點(diǎn)之一已經(jīng)被確定為pole。和圖描述子相似，一種邊可以形成頂點(diǎn)描述符的三個(gè)部分之一，可以描述為：

然后我們利用歐式距離找到當(dāng)前點(diǎn)云中頂點(diǎn)的描述符和候選幀中頂點(diǎn)描述符的匹配關(guān)系。

D. 幾何驗(yàn)證

該步驟為每個(gè)閉環(huán)候選幀選擇一組幾何一致的對(duì)應(yīng)點(diǎn)。利用RANSAC來(lái)優(yōu)化選取選取的對(duì)應(yīng)點(diǎn)。在每次RANSAC迭代的時(shí)候，利用SVD方法為絕對(duì)旋轉(zhuǎn)問(wèn)題找到最近的六自由度的變換矩陣。如果在這個(gè)矩陣下可以得到更多的內(nèi)點(diǎn)，我們就更新變換矩陣。對(duì)于每個(gè)候選幀，我們用下邊這個(gè)公式來(lái)評(píng)估閉環(huán)的loss:

C代表當(dāng)前掃描和閉環(huán)候選幀優(yōu)化后的對(duì)應(yīng)點(diǎn)的集合，T表示變換矩陣，c_iq和c_ic代表第i個(gè)C中的屬于當(dāng)前掃描和閉環(huán)候選幀的3D點(diǎn)。候選幀中l(wèi)oss最小的會(huì)利用閾值判斷是不是真正的存在一個(gè)閉環(huán)。只有在loss小于b的時(shí)候采認(rèn)為存在一個(gè)閉環(huán)。一旦閉環(huán)被確定，我們把這個(gè)變換矩陣作為6自由度的初始位姿。

實(shí)驗(yàn)設(shè)備：

Intel Core i7-6820HQ with 16 GB RAM，Nvidia TitanX with 12 GB RAM。