0. 筆者個人體會

回環檢測對于SLAM系統的全局一致性有著至關重要的影響?，F有的視覺SLAM回環檢測大多是基于詞袋模型，也就是2012年推出的BoW2庫和2017年推出的改進版本BoW3，例如ORB-SLAM2和VINS-Mono。2021年ORB-SLAM3的橫空出世更是將基于詞袋的回環檢測/重定位推上了新高度。

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

當然也有一些深度學習回環方法，比如波恩大學開源的OverlapNet。但不管怎么說，激光雷達SLAM的回環檢測目前還是非常簡單，并且假設位姿優化結果偏移小，無法應對大尺度漂移。另一方面，現有的雷達SLAM回環方法很難去修正6自由度位姿。

中科院沈自所的崔蕓閣博士近期開源的BoW3D似乎解決了這個痛點。崔博同時開源了新的點云特征描述方式Link3D以及基于Link3D的BoW3D詞袋庫。

2. 摘要

回環是自主移動系統同步定位與地圖創建(SLAM)的一個基本部分。在視覺SLAM領域，詞袋(BoW)在回環方面取得了巨大的成功。用于回環搜索的BoW特征也可用于后續的6-DoF回環校正。然而，對于3D LiDAR SLAM，現有方法可能無法實時有效地識別回環，且通常無法校正完整的6-DoF回環位姿。

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

我們進一步將提出的方法嵌入到3D LiDAR里程計系統中評估閉環性能。我們在公共數據集上測試了我們的方法，并與其他先進的算法進行了比較。BoW3D在大多數場景下的F1 max和擴展精度分數表現出更好的性能。值得注意的是，BoW3D在酷睿i7@2.2 GHz處理器的筆記本上執行時，識別和糾正KITTI 00 (包括4K+64線LiDAR掃描)上的回環平均需要48 ms。

3. 視覺BoW回顧

由于崔博設計的BoW3D和視覺詞袋模型BoW2/3有異曲同工之妙，因此在介紹BoW3D之前，我們先來回顧一下用于視覺回環的BoW2詞袋。 視覺圖像匹配往往涉及到關鍵點+描述子的表達方式，如果把描述子看做單詞，那么就可以構建相應的詞袋模型。

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

在實際使用時，首先計算圖像的特征點和描述子。然后將描述子利用DBoW庫計算得到詞袋向量，向量中要么為0，表示沒有這個單詞，要么為單詞權重，最終可以得到一個稀疏向量。比較兩個圖像各自的稀疏向量，可以得到相似性得分，進而確定是否檢測到了回環。

視覺詞袋模型有什么優點呢？

首先它場景識別速度快，可以加速特征匹配。其次它擴展性好，對多種圖像特征都實用，比如ORB、SIFT。同時它依賴少，僅依賴OpenCV和Boost庫。

那么它有什么缺點呢？

首先字典占用空間大，在SLAM系統運行時需要先加載一個幾百兆的大字典。同時詞袋模型對于重復紋理可能不是那么魯棒。



前面說到，詞袋向量中存儲的是單詞權重。

那么這個權重值具體如何計算呢？

這里用到的就是TF-IDF。TF指的是詞頻，也就是說一個單詞在圖像中出現的頻率越高，那么它區分度也就越高。這個很容易理解，比如我們在說話時如果反復提到某個詞，那么這個詞就是我們說話的關鍵詞，也就越重要。詞頻TF是在計算圖像詞袋向量時實時得到的。

IDF指的是逆向文本頻率，也就是說一個單詞在字典中出現的頻率越低，那么它區分度也就越高。逆向文本頻率IDF是在字典訓練階段就已經確定。



前面都是視覺詞袋的知識，下面的知識就比較重要了。尤其是逆向索引，是BoW3D的核心思想之一。 詞袋模型定義了正向索引（直接索引）與逆向索引。逆向索引記錄單詞在哪些圖像中出現，以及單詞的權重。如果當前幀的一個單詞在以前幀中出現，那么通過逆向索引可以直接知道這個單詞在哪些幀中出現過。

所以逆向索引主要進行位置識別。正向索引主要記錄節點ID，以及對應特征在圖像中的編號。所以正向索引主要進行加速匹配，比如ORB-SLAM的SearchByBoW函數就是利用正向索引來加速匹配。

4. LinK3D

到這里就涉及到DBoW3D的核心內容了！DBoW3D是基于LinK3D特征來實現詞袋模型的，所以我們首先介紹LinK3D特征。 首先放棄繁瑣的公式推導與數學描述，崔博士繪制了一張生動形象的漫畫來介紹LinK3D的具體原理！非常通俗易懂！

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



顯然，計算機無法輕易理解單純的文字描述。所以我們可以將上述位置信息進行向量化表達：



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

那么具體如何實現呢？

現在轉到3D點云空間，我們希望對圖中的黑色點進行描述。首先需要對黑色點所處空間進行劃分，分成一個個小區域，然后利用區域中的點進行描述。對所有關鍵點都進行類似的描述，就可以實現精確的點到點匹配！



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



在具體提取過程中，提取到的邊緣點會有兩類：一類是紅框中的散點，一類是藍框中成簇狀的點。顯然，用散點進行描述效果較差。因為他們可能只是在這一幀出現，在下一幀可能就會消失。

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



因此，LinK3D的核心思想就很明顯了，就是用關鍵點來表征關鍵點！ 在具體表示過程中，首先計算這些聚合關鍵點的均值點，并將他們投影到水平面。首先對這個平面進行劃分，作者是劃分成了180個區域，也就是說最后的描述子是180維的向量。然后，選擇當前點到最近點的方向為主方向，也就是圖中的k0到k1。

此外，還將主方向所在的區域劃定為第一個區域，其他區域逆時針排列。并在每個區域中選擇最近點進行描述。具體的向量值如何確定呢？每個向量的值，也就是當前點在區域中和最近點的距離，如果區域中沒有最近點的話就設置為0。



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



但這么做還有一個問題，就是它對最近點較為敏感。 那么怎么做呢。作者實際上是選擇了3個最近點，并得到了三個描述子，根據三個點的遠近確定優先級。

最終描述子是優先級最高的非零維向量。這么做還有一個好處，就是它對于動態物體非常的魯棒！比如其中一個是動態點，另外兩個就可以彌補這一方面。這個巧妙的設計實際上實現了非常大的性能提升。



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

在下圖中可以看出，初始的匹配是存在大量誤匹配的，但是經過RANSAC過濾后匹配得到了很好的優化。



在定量對比階段，作者對比了同類的點云特征表示方法，并提供與LinK3D相近的關鍵點。KITTI數據集上的實驗結果顯示，LinK3D在大多場景上的內點數量和內點百分率都取得了非常好的效果。



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

同時，基于DNN的方法的總運行時間非常大，并且需要GPU。而手工比對方法通常需要更多的運行時間來提取特征和匹配兩個LiDAR掃描。



點云配準實驗也展示了LinK3D的應用價值。LinK3D與一些傳統方法和深度學習的方法相比，在取得可比較的配準性能的同時，還有卓越的實時性能，同時在大多數序列上都能取得較高的估計精度。

5. DBoW3D

說完LinK3D以后，終于到了我們今天的主角DBoW3D！ DBoW3D的總體結構是采用哈希表構建單詞與位置的一對一直接關聯。選用哈希表的重要原因是它的計算復雜度理論為O（1），可以很好的提高實時性。字典中的單詞由LinK3D特征中的非零維度值和所在維度的ID組成。每個單詞對應一個位置信息，是單詞對應的位置集合，即該單詞所在幀的ID以及該單詞所在描述子的ID。



還記得我們剛開始提到的逆向索引嗎？到這一步整體的知識就串起來了！ DBoW3D的核心原理就是逆向索引！忘了沒關系，我們重新表達一下：逆向索引記錄單詞在哪些幀中出現，以及單詞的權重。如果當前幀的一個單詞在以前幀中出現，那么通過逆向索引可以直接知道這個單詞在哪些幀中出現過。

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

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



在檢索算法中，作者定義了一個類似逆向文本頻率的指標，用來判斷當前單詞區分度。它的值越高，就說明區分度越低，也就越應該舍棄，以提高算法的魯棒性。

實際檢索也是首先進行粗檢索，隨后選擇最好的候選幀進行進一步的驗證。



具體的閉環校正，是基于SVD分解的快速配準算法，隨后進行因子圖優化。



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

而DBoW3D顯然很好得處理了這個問題，可以發現在一些視角變化的場景，DBoW3D可以很好得進行閉環識別，并基于LinK3D構建當前幀與閉環幀的匹配。崔博表示，回環的最大容差可以達到45°。



同時，可以發現DBoW3D實現了很好的F1 max和extended precision指標，同時DBoW3D是唯一一個實現六自由度位姿修正的方案，這個在很大程度上彌補了激光雷達回環方案的痛點。



閉環矯正精度和累計誤差的降低值也顯示了DBoW3D具有很好的精度，說明DBoW3D可以很好得降低累計誤差。



同時，引入DBoW3D以后，可以發現相較于原始的A-LOAM，回環軌跡實現了很好的修正。



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

6. 筆者總結

筆者認為BoW3D這篇文章是具有非常重要的意義的，它模仿視覺SLAM詞袋模型，設計了激光雷達SLAM的詞袋模型，很大程度上提高了激光雷達SLAM回環的精度和魯棒性，重要的是它可以實現6自由度的位姿修正，這些在以前的方案中都是沒有實現的。LinK3D和BoW3D算法已經開源，筆者認為未來基于BoW3D可能會產生很多新的工作。 筆者在這里也大膽猜想，基于BoW3D可能有如下的可以進行改進的點：

1、在Lego-LOAM、LIO-SAM、LIV-SAM、R3Live等雷達SLAM方案中引入BoW3D，優化回環精度和魯棒性；

2、BoW3D是基于逆向索引的，那么是否可以基于正向索引設計新的雷達SLAM詞袋方案；

3、利用平面點代替邊緣點；

4、在LinK3D和BoW3D中引入語義信息，或者基于其他特征設計新的LinK3D和BoW3D算法；

5、利用DBoW3D實現全局定位，比如建好一個地圖后，進行僅定位。

審核編輯：劉清