0. 筆者個人體會

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

但激光雷達SLAM呢？相比視覺SLAM就稍顯尷尬了。Lego-LOAM是基于軌跡位姿方法的回環(huán)檢測，使用KDtree尋找當(dāng)前位姿的歷史最近位姿，之后把歷史位姿作為候選，用ICP算法修正位姿。Cartographer采用scanMatch將scan和submaps匹配，通過分支定界加快搜索，然后降低位姿殘差。當(dāng)然也有一些深度學(xué)習(xí)回環(huán)方法，比如波恩大學(xué)開源的OverlapNet。但不管怎么說，激光雷達SLAM的回環(huán)檢測目前還是非常簡單，并且假設(shè)位姿優(yōu)化結(jié)果偏移小，無法應(yīng)對大尺度漂移。另一方面，現(xiàn)有的雷達SLAM回環(huán)方法很難去修正6自由度位姿。

中科院沈自所的崔蕓閣博士近期開源的BoW3D似乎解決了這個痛點。崔博同時開源了新的點云特征描述方式Link3D以及基于Link3D的BoW3D詞袋庫。本文將帶領(lǐng)讀者一起欣賞崔博的最新工作！當(dāng)然筆者水平有限，如果有理解不當(dāng)?shù)牡胤綒g迎各位讀者一起探討，共同學(xué)習(xí)！

注：本文參考了崔蕓閣博士的直播分享，感謝崔博的精彩報告。

來源：公眾號「3D視覺工坊」

1. 論文信息

LinK3D：

標(biāo)題：LinK3D: Linear Keypoints Representation for 3D LiDAR Point Cloud

作者：Yunge Cui, Yinlong Zhang, Jiahua Dong, Haibo Sun, Feng Zhu

機構(gòu)：中科院沈陽自動化研究所

原文連接：https://arxiv.org/abs/2206.05927

BoW3D：

標(biāo)題：BoW3D: Bag of Words for Real-Time Loop Closing in 3D LiDAR SLAM

作者：Yunge Cui,?Xieyuanli Chen,?Yinlong Zhang,?Jiahua Dong,?Qingxiao Wu,?Feng Zhu

機構(gòu)：中科院沈陽自動化研究所來源：2022 RAL

原文鏈接：https://arxiv.org/abs/2208.07473代碼鏈接：https://github.com/yungecui/bow3d

2. 摘要

回環(huán)是自主移動系統(tǒng)同步定位與地圖創(chuàng)建(SLAM)的一個基本部分。在視覺SLAM領(lǐng)域，詞袋(BoW)在回環(huán)方面取得了巨大的成功。用于回環(huán)搜索的BoW特征也可用于后續(xù)的6-DoF回環(huán)校正。然而，對于3D LiDAR SLAM，現(xiàn)有方法可能無法實時有效地識別回環(huán)，且通常無法校正完整的6-DoF回環(huán)位姿。為了解決這個問題，我們提出了一種用于3D LiDAR SLAM中實時閉環(huán)的詞袋模型BoW3D。我們的方法不僅有效地識別了重訪的回環(huán)位置，而且實時地修正了完整的6-DoF回環(huán)位姿。BoW3D基于三維LiDAR特征LinK3D構(gòu)建詞袋，該詞袋高效、姿態(tài)不變，可用于精確的點對點匹配。我們進一步將提出的方法嵌入到3D LiDAR里程計系統(tǒng)中評估閉環(huán)性能。我們在公共數(shù)據(jù)集上測試了我們的方法，并與其他先進的算法進行了比較。BoW3D在大多數(shù)場景下的F1 max和擴展精度分?jǐn)?shù)表現(xiàn)出更好的性能。值得注意的是，BoW3D在酷睿i7@2.2 GHz處理器的筆記本上執(zhí)行時，識別和糾正KITTI 00 (包括4K+64線LiDAR掃描)上的回環(huán)平均需要48 ms。

3. 視覺BoW回顧

由于崔博設(shè)計的BoW3D和視覺詞袋模型BoW2/3有異曲同工之妙，因此在介紹BoW3D之前，我們先來回顧一下用于視覺回環(huán)的BoW2詞袋。

視覺圖像匹配往往涉及到關(guān)鍵點+描述子的表達方式，如果把描述子看做單詞，那么就可以構(gòu)建相應(yīng)的詞袋模型。BoW2庫是2012年由西班牙薩拉戈薩大學(xué)的López等人提出的開源軟件庫，它首先是通過K-means聚類得到一個深度為d的k叉樹（作者設(shè)置的k=6，d=10）。在訓(xùn)練過程中，作者選取了1萬張圖片，每張圖片提取100個特征，利用這100萬個特征訓(xùn)練得到字典模型。

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

視覺詞袋模型有什么優(yōu)點呢？首先它場景識別速度快，可以加速特征匹配。其次它擴展性好，對多種圖像特征都實用，比如ORB、SIFT。同時它依賴少，僅依賴OpenCV和Boost庫。

那么它有什么缺點呢？首先字典占用空間大，在SLAM系統(tǒng)運行時需要先加載一個幾百兆的大字典。同時詞袋模型對于重復(fù)紋理可能不是那么魯棒。

前面說到，詞袋向量中存儲的是單詞權(quán)重。那么這個權(quán)重值具體如何計算呢？

這里用到的就是TF-IDF。TF指的是詞頻，也就是說一個單詞在圖像中出現(xiàn)的頻率越高，那么它區(qū)分度也就越高。這個很容易理解，比如我們在說話時如果反復(fù)提到某個詞，那么這個詞就是我們說話的關(guān)鍵詞，也就越重要。詞頻TF是在計算圖像詞袋向量時實時得到的。IDF指的是逆向文本頻率，也就是說一個單詞在字典中出現(xiàn)的頻率越低，那么它區(qū)分度也就越高。逆向文本頻率IDF是在字典訓(xùn)練階段就已經(jīng)確定。

前面都是視覺詞袋的知識，下面的知識就比較重要了。尤其是逆向索引，是BoW3D的核心思想之一。

詞袋模型定義了正向索引（直接索引）與逆向索引。逆向索引記錄單詞在哪些圖像中出現(xiàn)，以及單詞的權(quán)重。如果當(dāng)前幀的一個單詞在以前幀中出現(xiàn)，那么通過逆向索引可以直接知道這個單詞在哪些幀中出現(xiàn)過。所以逆向索引主要進行位置識別。正向索引主要記錄節(jié)點ID，以及對應(yīng)特征在圖像中的編號。所以正向索引主要進行加速匹配，比如ORB-SLAM的SearchByBoW函數(shù)就是利用正向索引來加速匹配。

4. LinK3D

到這里就涉及到DBoW3D的核心內(nèi)容了！DBoW3D是基于LinK3D特征來實現(xiàn)詞袋模型的，所以我們首先介紹LinK3D特征。

首先放棄繁瑣的公式推導(dǎo)與數(shù)學(xué)描述，崔博士繪制了一張生動形象的漫畫來介紹LinK3D的具體原理！非常通俗易懂！

假如現(xiàn)在我們正處于一個街道的十字路口，如何對我們所處環(huán)境進行描述呢？

可以這樣表達，我們的三點鐘方向是一家醫(yī)院，五點鐘方向是一輛車，八點鐘方向是一家超市，九點鐘方向是一個紅綠燈。所以，如果別人也有這樣一個描述，那么我們大概率可以推斷是位于同一位置！

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

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

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

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

思想理解了，具體如何提取呢？

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

在具體提取過程中，提取到的邊緣點會有兩類：一類是紅框中的散點，一類是藍框中成簇狀的點。顯然，用散點進行描述效果較差。因為他們可能只是在這一幀出現(xiàn)，在下一幀可能就會消失。如果使用散點進行描述的話會降低系統(tǒng)的魯棒性。因此需要對提取到的點云進行進一步的分類，得到成簇分布的魯棒的聚合關(guān)鍵點。對于任何的聚合關(guān)鍵點，它附近的聚合關(guān)鍵點就類似漫畫中的超市、車這些特征。

因此，LinK3D的核心思想就很明顯了，就是用關(guān)鍵點來表征關(guān)鍵點！

在具體表示過程中，首先計算這些聚合關(guān)鍵點的均值點，并將他們投影到水平面。首先對這個平面進行劃分，作者是劃分成了180個區(qū)域，也就是說最后的描述子是180維的向量。然后，選擇當(dāng)前點到最近點的方向為主方向，也就是圖中的k0到k1。此外，還將主方向所在的區(qū)域劃定為第一個區(qū)域，其他區(qū)域逆時針排列。并在每個區(qū)域中選擇最近點進行描述。具體的向量值如何確定呢？每個向量的值，也就是當(dāng)前點在區(qū)域中和最近點的距離，如果區(qū)域中沒有最近點的話就設(shè)置為0。

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

但這么做還有一個問題，就是它對最近點較為敏感。

那么怎么做呢。作者實際上是選擇了3個最近點，并得到了三個描述子，根據(jù)三個點的遠近確定優(yōu)先級。最終描述子是優(yōu)先級最高的非零維向量。這么做還有一個好處，就是它對于動態(tài)物體非常的魯棒！比如其中一個是動態(tài)點，另外兩個就可以彌補這一方面。這個巧妙的設(shè)計實際上實現(xiàn)了非常大的性能提升。

在特征匹配階段，作者提出了一個由粗（聚合關(guān)鍵點）到精（邊緣關(guān)鍵點）的匹配算法，并將其與RANSAC算法結(jié)合得到兩幀之間精確的點到點的匹配。在下圖中可以看出，初始的匹配是存在大量誤匹配的，但是經(jīng)過RANSAC過濾后匹配得到了很好的優(yōu)化。

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

特征提取和特征匹配的耗時也展示了LinK3D良好的實時性。LinK3D的特征提取和匹配時間遠小于所需的100毫秒，總時間平均只需要40毫秒左右。同時，基于DNN的方法的總運行時間非常大，并且需要GPU。而手工比對方法通常需要更多的運行時間來提取特征和匹配兩個LiDAR掃描。

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

5. DBoW3D

說完LinK3D以后，終于到了我們今天的主角DBoW3D！

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

還記得我們剛開始提到的逆向索引嗎？到這一步整體的知識就串起來了！

DBoW3D的核心原理就是逆向索引！忘了沒關(guān)系，我們重新表達一下：逆向索引記錄單詞在哪些幀中出現(xiàn)，以及單詞的權(quán)重。如果當(dāng)前幀的一個單詞在以前幀中出現(xiàn)，那么通過逆向索引可以直接知道這個單詞在哪些幀中出現(xiàn)過。所以DBoW3D通過逆向索引可以很好得進行位置識別！此外，由于LinK3D本身已經(jīng)可以很好得表達位置信息。所以這里也沒有必要將其轉(zhuǎn)化為更抽象的向量表達。

不知道讀者有沒有注意到一個很重要的信息，就是DBoW3D中的單詞是實時構(gòu)建的！

這樣有什么好處呢？也就是說DBoW3D相較于DBoW2/3，再也不用提前加載那幾百兆的字典文件了！到這里DBoW3D的原理部分其實就結(jié)束了（有沒有意猶未盡）。下面我們來看看DBoW3D的具體實驗效果。

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

在檢索算法中，作者定義了一個類似逆向文本頻率的指標(biāo)，用來判斷當(dāng)前單詞區(qū)分度。它的值越高，就說明區(qū)分度越低，也就越應(yīng)該舍棄，以提高算法的魯棒性。實際檢索也是首先進行粗檢索，隨后選擇最好的候選幀進行進一步的驗證。

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

筆者感覺，設(shè)計特征和詞袋，最重要的就是位姿不變性！這個就類似上面那個漫畫描述的問題：兩個人站在同一位置，但是朝向不同，如何確定兩個人位于同一位置？而DBoW3D顯然很好得處理了這個問題，可以發(fā)現(xiàn)在一些視角變化的場景，DBoW3D可以很好得進行閉環(huán)識別，并基于LinK3D構(gòu)建當(dāng)前幀與閉環(huán)幀的匹配。崔博表示，回環(huán)的最大容差可以達到45°。

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

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

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

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

6. 筆者總結(jié)

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

筆者在這里也大膽猜想，基于BoW3D可能有如下的可以進行改進的點：

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

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

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

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

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

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請聯(lián)系刪文。

3D視覺工坊精品課程官網(wǎng)：3dcver.com
1.面向自動駕駛領(lǐng)域的3D點云目標(biāo)檢測全棧學(xué)習(xí)路線！(單模態(tài)+多模態(tài)/數(shù)據(jù)+代碼)
2.徹底搞透視覺三維重建：原理剖析、代碼講解、及優(yōu)化改進
3.國內(nèi)首個面向工業(yè)級實戰(zhàn)的點云處理課程
4.激光-視覺-IMU-GPS融合SLAM算法梳理和代碼講解
5.徹底搞懂視覺-慣性SLAM：基于VINS-Fusion正式開課啦
6.徹底搞懂基于LOAM框架的3D激光SLAM: 源碼剖析到算法優(yōu)化
7.徹底剖析室內(nèi)、室外激光SLAM關(guān)鍵算法原理、代碼和實戰(zhàn)(cartographer+LOAM +LIO-SAM)
8.從零搭建一套結(jié)構(gòu)光3D重建系統(tǒng)[理論+源碼+實踐]
9.單目深度估計方法：算法梳理與代碼實現(xiàn)
10.自動駕駛中的深度學(xué)習(xí)模型部署實戰(zhàn)
11.相機模型與標(biāo)定(單目+雙目+魚眼）
12.重磅！四旋翼飛行器：算法與實戰(zhàn)
13.ROS2從入門到精通：理論與實戰(zhàn)
14.國內(nèi)首個3D缺陷檢測教程：理論、源碼與實戰(zhàn)
15.基于Open3D的點云處理入門與實戰(zhàn)教程
16.透徹理解視覺ORB-SLAM3：理論基礎(chǔ)+代碼解析+算法改進
17.不斷更新中......

重磅！粉絲學(xué)習(xí)交流群已成立

交流群主要有3D視覺、CV&深度學(xué)習(xí)、SLAM、三維重建、點云后處理、自動駕駛、多傳感器融合、CV入門、三維測量、VR/AR、3D人臉識別、醫(yī)療影像、缺陷檢測、行人重識別、目標(biāo)跟蹤、視覺產(chǎn)品落地、視覺競賽、車牌識別、硬件選型、ORB-SLAM系列源碼交流、深度估計、TOF、求職交流等方向。

添加小助理微信（dddvisiona）,一定要備注：研究方向+學(xué)校/公司+昵稱，例如：”3D視覺?+ 上海交大 + 靜靜“。請按照格式備注，可快速被通過且邀請進群。原創(chuàng)投稿也請聯(lián)系。