用于3D激光雷達(dá)SLAM回環(huán)檢測(cè)的實(shí)時(shí)詞袋模型BoW3D

0. 筆者個(gè)人體會(huì)

回環(huán)檢測(cè)對(duì)于SLAM系統(tǒng)的全局一致性有著至關(guān)重要的影響?，F(xiàn)有的視覺(jué)SLAM回環(huán)檢測(cè)大多是基于詞袋模型，也就是2012年推出的BoW2庫(kù)和2017年推出的改進(jìn)版本BoW3，例如ORB-SLAM2和VINS-Mono。2021年ORB-SLAM3的橫空出世更是將基于詞袋的回環(huán)檢測(cè)/重定位推上了新高度。

但激光雷達(dá)SLAM呢？相比視覺(jué)SLAM就稍顯尷尬了。Lego-LOAM是基于軌跡位姿方法的回環(huán)檢測(cè)，使用KDtree尋找當(dāng)前位姿的歷史最近位姿，之后把歷史位姿作為候選，用ICP算法修正位姿。Cartographer采用scanMatch將scan和submaps匹配，通過(guò)分支定界加快搜索，然后降低位姿殘差。

當(dāng)然也有一些深度學(xué)習(xí)回環(huán)方法，比如波恩大學(xué)開(kāi)源的OverlapNet。但不管怎么說(shuō)，激光雷達(dá)SLAM的回環(huán)檢測(cè)目前還是非常簡(jiǎn)單，并且假設(shè)位姿優(yōu)化結(jié)果偏移小，無(wú)法應(yīng)對(duì)大尺度漂移。另一方面，現(xiàn)有的雷達(dá)SLAM回環(huán)方法很難去修正6自由度位姿。

中科院沈自所的崔蕓閣博士近期開(kāi)源的BoW3D似乎解決了這個(gè)痛點(diǎn)。崔博同時(shí)開(kāi)源了新的點(diǎn)云特征描述方式Link3D以及基于Link3D的BoW3D詞袋庫(kù)。

2. 摘要

回環(huán)是自主移動(dòng)系統(tǒng)同步定位與地圖創(chuàng)建(SLAM)的一個(gè)基本部分。在視覺(jué)SLAM領(lǐng)域，詞袋(BoW)在回環(huán)方面取得了巨大的成功。用于回環(huán)搜索的BoW特征也可用于后續(xù)的6-DoF回環(huán)校正。然而，對(duì)于3D LiDAR SLAM，現(xiàn)有方法可能無(wú)法實(shí)時(shí)有效地識(shí)別回環(huán)，且通常無(wú)法校正完整的6-DoF回環(huán)位姿。

為了解決這個(gè)問(wèn)題，我們提出了一種用于3D LiDAR SLAM中實(shí)時(shí)閉環(huán)的詞袋模型BoW3D。我們的方法不僅有效地識(shí)別了重訪的回環(huán)位置，而且實(shí)時(shí)地修正了完整的6-DoF回環(huán)位姿。BoW3D基于三維LiDAR特征LinK3D構(gòu)建詞袋，該詞袋高效、姿態(tài)不變，可用于精確的點(diǎn)對(duì)點(diǎn)匹配。

我們進(jìn)一步將提出的方法嵌入到3D LiDAR里程計(jì)系統(tǒng)中評(píng)估閉環(huán)性能。我們?cè)诠矓?shù)據(jù)集上測(cè)試了我們的方法，并與其他先進(jìn)的算法進(jìn)行了比較。BoW3D在大多數(shù)場(chǎng)景下的F1 max和擴(kuò)展精度分?jǐn)?shù)表現(xiàn)出更好的性能。值得注意的是，BoW3D在酷睿i7@2.2 GHz處理器的筆記本上執(zhí)行時(shí)，識(shí)別和糾正KITTI 00 (包括4K+64線LiDAR掃描)上的回環(huán)平均需要48 ms。

3. 視覺(jué)BoW回顧

由于崔博設(shè)計(jì)的BoW3D和視覺(jué)詞袋模型BoW2/3有異曲同工之妙，因此在介紹BoW3D之前，我們先來(lái)回顧一下用于視覺(jué)回環(huán)的BoW2詞袋。 視覺(jué)圖像匹配往往涉及到關(guān)鍵點(diǎn)+描述子的表達(dá)方式，如果把描述子看做單詞，那么就可以構(gòu)建相應(yīng)的詞袋模型。

BoW2庫(kù)是2012年由西班牙薩拉戈薩大學(xué)的López等人提出的開(kāi)源軟件庫(kù)，它首先是通過(guò)K-means聚類得到一個(gè)深度為d的k叉樹(shù)（作者設(shè)置的k=6，d=10）。在訓(xùn)練過(guò)程中，作者選取了1萬(wàn)張圖片，每張圖片提取100個(gè)特征，利用這100萬(wàn)個(gè)特征訓(xùn)練得到字典模型。

在實(shí)際使用時(shí)，首先計(jì)算圖像的特征點(diǎn)和描述子。然后將描述子利用DBoW庫(kù)計(jì)算得到詞袋向量，向量中要么為0，表示沒(méi)有這個(gè)單詞，要么為單詞權(quán)重，最終可以得到一個(gè)稀疏向量。比較兩個(gè)圖像各自的稀疏向量，可以得到相似性得分，進(jìn)而確定是否檢測(cè)到了回環(huán)。

視覺(jué)詞袋模型有什么優(yōu)點(diǎn)呢？

首先它場(chǎng)景識(shí)別速度快，可以加速特征匹配。其次它擴(kuò)展性好，對(duì)多種圖像特征都實(shí)用，比如ORB、SIFT。同時(shí)它依賴少，僅依賴OpenCV和Boost庫(kù)。

那么它有什么缺點(diǎn)呢？

首先字典占用空間大，在SLAM系統(tǒng)運(yùn)行時(shí)需要先加載一個(gè)幾百兆的大字典。同時(shí)詞袋模型對(duì)于重復(fù)紋理可能不是那么魯棒。



前面說(shuō)到，詞袋向量中存儲(chǔ)的是單詞權(quán)重。

那么這個(gè)權(quán)重值具體如何計(jì)算呢？

這里用到的就是TF-IDF。TF指的是詞頻，也就是說(shuō)一個(gè)單詞在圖像中出現(xiàn)的頻率越高，那么它區(qū)分度也就越高。這個(gè)很容易理解，比如我們?cè)谡f(shuō)話時(shí)如果反復(fù)提到某個(gè)詞，那么這個(gè)詞就是我們說(shuō)話的關(guān)鍵詞，也就越重要。詞頻TF是在計(jì)算圖像詞袋向量時(shí)實(shí)時(shí)得到的。

IDF指的是逆向文本頻率，也就是說(shuō)一個(gè)單詞在字典中出現(xiàn)的頻率越低，那么它區(qū)分度也就越高。逆向文本頻率IDF是在字典訓(xùn)練階段就已經(jīng)確定。



前面都是視覺(jué)詞袋的知識(shí)，下面的知識(shí)就比較重要了。尤其是逆向索引，是BoW3D的核心思想之一。 詞袋模型定義了正向索引（直接索引）與逆向索引。逆向索引記錄單詞在哪些圖像中出現(xiàn)，以及單詞的權(quán)重。如果當(dāng)前幀的一個(gè)單詞在以前幀中出現(xiàn)，那么通過(guò)逆向索引可以直接知道這個(gè)單詞在哪些幀中出現(xiàn)過(guò)。

所以逆向索引主要進(jìn)行位置識(shí)別。正向索引主要記錄節(jié)點(diǎn)ID，以及對(duì)應(yīng)特征在圖像中的編號(hào)。所以正向索引主要進(jìn)行加速匹配，比如ORB-SLAM的SearchByBoW函數(shù)就是利用正向索引來(lái)加速匹配。

4. LinK3D

到這里就涉及到DBoW3D的核心內(nèi)容了！DBoW3D是基于LinK3D特征來(lái)實(shí)現(xiàn)詞袋模型的，所以我們首先介紹LinK3D特征。 首先放棄繁瑣的公式推導(dǎo)與數(shù)學(xué)描述，崔博士繪制了一張生動(dòng)形象的漫畫(huà)來(lái)介紹LinK3D的具體原理！非常通俗易懂！

假如現(xiàn)在我們正處于一個(gè)街道的十字路口，如何對(duì)我們所處環(huán)境進(jìn)行描述呢？ 可以這樣表達(dá)，我們的三點(diǎn)鐘方向是一家醫(yī)院，五點(diǎn)鐘方向是一輛車，八點(diǎn)鐘方向是一家超市，九點(diǎn)鐘方向是一個(gè)紅綠燈。所以，如果別人也有這樣一個(gè)描述，那么我們大概率可以推斷是位于同一位置！



顯然，計(jì)算機(jī)無(wú)法輕易理解單純的文字描述。所以我們可以將上述位置信息進(jìn)行向量化表達(dá)：



注意，實(shí)際應(yīng)用過(guò)程中，很有可能會(huì)出現(xiàn)一種情況。也就是兩個(gè)人位于同一位置，但是朝向不同。如果這時(shí)不加以任何處理的話，得到的位置向量就完全不同！而最合適的想法是，通過(guò)某種算法，將所有朝向都對(duì)齊到一個(gè)主方向！這個(gè)思想有點(diǎn)類似ORB特征的旋轉(zhuǎn)不變性。

那么具體如何實(shí)現(xiàn)呢？

現(xiàn)在轉(zhuǎn)到3D點(diǎn)云空間，我們希望對(duì)圖中的黑色點(diǎn)進(jìn)行描述。首先需要對(duì)黑色點(diǎn)所處空間進(jìn)行劃分，分成一個(gè)個(gè)小區(qū)域，然后利用區(qū)域中的點(diǎn)進(jìn)行描述。對(duì)所有關(guān)鍵點(diǎn)都進(jìn)行類似的描述，就可以實(shí)現(xiàn)精確的點(diǎn)到點(diǎn)匹配！



思想理解了，具體如何提取呢？ 首先提取顯著的邊緣點(diǎn)，并進(jìn)一步提取更魯棒的聚合關(guān)鍵點(diǎn)。然后構(gòu)建聚合關(guān)鍵點(diǎn)之間的距離表和方向表，通過(guò)查表的方式加速描述子的生成。隨后，特征被有序地表示為一個(gè)向量，每一維都具有特定的含義。



在具體提取過(guò)程中，提取到的邊緣點(diǎn)會(huì)有兩類：一類是紅框中的散點(diǎn)，一類是藍(lán)框中成簇狀的點(diǎn)。顯然，用散點(diǎn)進(jìn)行描述效果較差。因?yàn)樗麄兛赡苤皇窃谶@一幀出現(xiàn)，在下一幀可能就會(huì)消失。

如果使用散點(diǎn)進(jìn)行描述的話會(huì)降低系統(tǒng)的魯棒性。因此需要對(duì)提取到的點(diǎn)云進(jìn)行進(jìn)一步的分類，得到成簇分布的魯棒的聚合關(guān)鍵點(diǎn)。對(duì)于任何的聚合關(guān)鍵點(diǎn)，它附近的聚合關(guān)鍵點(diǎn)就類似漫畫(huà)中的超市、車這些特征。



因此，LinK3D的核心思想就很明顯了，就是用關(guān)鍵點(diǎn)來(lái)表征關(guān)鍵點(diǎn)！ 在具體表示過(guò)程中，首先計(jì)算這些聚合關(guān)鍵點(diǎn)的均值點(diǎn)，并將他們投影到水平面。首先對(duì)這個(gè)平面進(jìn)行劃分，作者是劃分成了180個(gè)區(qū)域，也就是說(shuō)最后的描述子是180維的向量。然后，選擇當(dāng)前點(diǎn)到最近點(diǎn)的方向?yàn)橹鞣较颍簿褪菆D中的k0到k1。

此外，還將主方向所在的區(qū)域劃定為第一個(gè)區(qū)域，其他區(qū)域逆時(shí)針排列。并在每個(gè)區(qū)域中選擇最近點(diǎn)進(jìn)行描述。具體的向量值如何確定呢？每個(gè)向量的值，也就是當(dāng)前點(diǎn)在區(qū)域中和最近點(diǎn)的距離，如果區(qū)域中沒(méi)有最近點(diǎn)的話就設(shè)置為0。



也就是說(shuō)，最后得到的描述子同時(shí)包含距離信息和方向信息：



但這么做還有一個(gè)問(wèn)題，就是它對(duì)最近點(diǎn)較為敏感。 那么怎么做呢。作者實(shí)際上是選擇了3個(gè)最近點(diǎn)，并得到了三個(gè)描述子，根據(jù)三個(gè)點(diǎn)的遠(yuǎn)近確定優(yōu)先級(jí)。

最終描述子是優(yōu)先級(jí)最高的非零維向量。這么做還有一個(gè)好處，就是它對(duì)于動(dòng)態(tài)物體非常的魯棒！比如其中一個(gè)是動(dòng)態(tài)點(diǎn)，另外兩個(gè)就可以彌補(bǔ)這一方面。這個(gè)巧妙的設(shè)計(jì)實(shí)際上實(shí)現(xiàn)了非常大的性能提升。



在特征匹配階段，作者提出了一個(gè)由粗（聚合關(guān)鍵點(diǎn)）到精（邊緣關(guān)鍵點(diǎn)）的匹配算法，并將其與RANSAC算法結(jié)合得到兩幀之間精確的點(diǎn)到點(diǎn)的匹配。

在下圖中可以看出，初始的匹配是存在大量誤匹配的，但是經(jīng)過(guò)RANSAC過(guò)濾后匹配得到了很好的優(yōu)化。



在定量對(duì)比階段，作者對(duì)比了同類的點(diǎn)云特征表示方法，并提供與LinK3D相近的關(guān)鍵點(diǎn)。KITTI數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果顯示，LinK3D在大多場(chǎng)景上的內(nèi)點(diǎn)數(shù)量和內(nèi)點(diǎn)百分率都取得了非常好的效果。



特征提取和特征匹配的耗時(shí)也展示了LinK3D良好的實(shí)時(shí)性。LinK3D的特征提取和匹配時(shí)間遠(yuǎn)小于所需的100毫秒，總時(shí)間平均只需要40毫秒左右。

同時(shí)，基于DNN的方法的總運(yùn)行時(shí)間非常大，并且需要GPU。而手工比對(duì)方法通常需要更多的運(yùn)行時(shí)間來(lái)提取特征和匹配兩個(gè)LiDAR掃描。



點(diǎn)云配準(zhǔn)實(shí)驗(yàn)也展示了LinK3D的應(yīng)用價(jià)值。LinK3D與一些傳統(tǒng)方法和深度學(xué)習(xí)的方法相比，在取得可比較的配準(zhǔn)性能的同時(shí)，還有卓越的實(shí)時(shí)性能，同時(shí)在大多數(shù)序列上都能取得較高的估計(jì)精度。

5. DBoW3D

說(shuō)完LinK3D以后，終于到了我們今天的主角DBoW3D！ DBoW3D的總體結(jié)構(gòu)是采用哈希表構(gòu)建單詞與位置的一對(duì)一直接關(guān)聯(lián)。選用哈希表的重要原因是它的計(jì)算復(fù)雜度理論為O（1），可以很好的提高實(shí)時(shí)性。字典中的單詞由LinK3D特征中的非零維度值和所在維度的ID組成。每個(gè)單詞對(duì)應(yīng)一個(gè)位置信息，是單詞對(duì)應(yīng)的位置集合，即該單詞所在幀的ID以及該單詞所在描述子的ID。



還記得我們剛開(kāi)始提到的逆向索引嗎？到這一步整體的知識(shí)就串起來(lái)了！ DBoW3D的核心原理就是逆向索引！忘了沒(méi)關(guān)系，我們重新表達(dá)一下：逆向索引記錄單詞在哪些幀中出現(xiàn)，以及單詞的權(quán)重。如果當(dāng)前幀的一個(gè)單詞在以前幀中出現(xiàn)，那么通過(guò)逆向索引可以直接知道這個(gè)單詞在哪些幀中出現(xiàn)過(guò)。

所以DBoW3D通過(guò)逆向索引可以很好得進(jìn)行位置識(shí)別！此外，由于LinK3D本身已經(jīng)可以很好得表達(dá)位置信息。所以這里也沒(méi)有必要將其轉(zhuǎn)化為更抽象的向量表達(dá)。 不知道讀者有沒(méi)有注意到一個(gè)很重要的信息，就是DBoW3D中的單詞是實(shí)時(shí)構(gòu)建的！ 這樣有什么好處呢？也就是說(shuō)DBoW3D相較于DBoW2/3，再也不用提前加載那幾百兆的字典文件了！ 到這里DBoW3D的原理部分其實(shí)就結(jié)束了（有沒(méi)有意猶未盡）。

下面我們來(lái)看看DBoW3D的具體實(shí)驗(yàn)效果。 崔博將DBoW3D嵌入了著名雷達(dá)算法A-LOAM中。首先提取邊緣點(diǎn)和平面，并進(jìn)一步的提取LinK3D特征。然后利用雷達(dá)里程計(jì)算法進(jìn)行由粗到精的位姿估計(jì)，并維護(hù)了一個(gè)局部地圖。最后，BoW3D被嵌入到閉環(huán)檢測(cè)線程中用于實(shí)時(shí)地識(shí)別及閉環(huán)校正。



在檢索算法中，作者定義了一個(gè)類似逆向文本頻率的指標(biāo)，用來(lái)判斷當(dāng)前單詞區(qū)分度。它的值越高，就說(shuō)明區(qū)分度越低，也就越應(yīng)該舍棄，以提高算法的魯棒性。

實(shí)際檢索也是首先進(jìn)行粗檢索，隨后選擇最好的候選幀進(jìn)行進(jìn)一步的驗(yàn)證。



具體的閉環(huán)校正，是基于SVD分解的快速配準(zhǔn)算法，隨后進(jìn)行因子圖優(yōu)化。



筆者感覺(jué)，設(shè)計(jì)特征和詞袋，最重要的就是位姿不變性！這個(gè)就類似上面那個(gè)漫畫(huà)描述的問(wèn)題：兩個(gè)人站在同一位置，但是朝向不同，如何確定兩個(gè)人位于同一位置？

而DBoW3D顯然很好得處理了這個(gè)問(wèn)題，可以發(fā)現(xiàn)在一些視角變化的場(chǎng)景，DBoW3D可以很好得進(jìn)行閉環(huán)識(shí)別，并基于LinK3D構(gòu)建當(dāng)前幀與閉環(huán)幀的匹配。崔博表示，回環(huán)的最大容差可以達(dá)到45°。



同時(shí)，可以發(fā)現(xiàn)DBoW3D實(shí)現(xiàn)了很好的F1 max和extended precision指標(biāo)，同時(shí)DBoW3D是唯一一個(gè)實(shí)現(xiàn)六自由度位姿修正的方案，這個(gè)在很大程度上彌補(bǔ)了激光雷達(dá)回環(huán)方案的痛點(diǎn)。



閉環(huán)矯正精度和累計(jì)誤差的降低值也顯示了DBoW3D具有很好的精度，說(shuō)明DBoW3D可以很好得降低累計(jì)誤差。



同時(shí)，引入DBoW3D以后，可以發(fā)現(xiàn)相較于原始的A-LOAM，回環(huán)軌跡實(shí)現(xiàn)了很好的修正。



想必讀者一定非常關(guān)心運(yùn)行耗時(shí)問(wèn)題。作者對(duì)處理一次LiDAR掃描的回環(huán)進(jìn)行積分后，評(píng)估SLAM系統(tǒng)中每個(gè)模塊的平均運(yùn)行時(shí)間。要注意的是，系統(tǒng)的每個(gè)模塊在不同的線程中分別運(yùn)行。雖然建圖線程和位姿圖優(yōu)化( PGO )的運(yùn)行時(shí)間超過(guò)100 ms，但由于其使用頻率較低，可以實(shí)時(shí)執(zhí)行。重要的是，BoW3D處理一幀圖像的時(shí)間整體小于100 ms，保證了BoW3D應(yīng)用于3D LiDAR SLAM系統(tǒng)的實(shí)時(shí)性。

6. 筆者總結(jié)

筆者認(rèn)為BoW3D這篇文章是具有非常重要的意義的，它模仿視覺(jué)SLAM詞袋模型，設(shè)計(jì)了激光雷達(dá)SLAM的詞袋模型，很大程度上提高了激光雷達(dá)SLAM回環(huán)的精度和魯棒性，重要的是它可以實(shí)現(xiàn)6自由度的位姿修正，這些在以前的方案中都是沒(méi)有實(shí)現(xiàn)的。LinK3D和BoW3D算法已經(jīng)開(kāi)源，筆者認(rèn)為未來(lái)基于BoW3D可能會(huì)產(chǎn)生很多新的工作。 筆者在這里也大膽猜想，基于BoW3D可能有如下的可以進(jìn)行改進(jìn)的點(diǎn)：

1、在Lego-LOAM、LIO-SAM、LIV-SAM、R3Live等雷達(dá)SLAM方案中引入BoW3D，優(yōu)化回環(huán)精度和魯棒性；

2、BoW3D是基于逆向索引的，那么是否可以基于正向索引設(shè)計(jì)新的雷達(dá)SLAM詞袋方案；

3、利用平面點(diǎn)代替邊緣點(diǎn)；

4、在LinK3D和BoW3D中引入語(yǔ)義信息，或者基于其他特征設(shè)計(jì)新的LinK3D和BoW3D算法；

5、利用DBoW3D實(shí)現(xiàn)全局定位，比如建好一個(gè)地圖后，進(jìn)行僅定位。

審核編輯：劉清