詳解 | SLAM回環(huán)檢測問題
點擊上方“小白學視覺”，選擇加"星標"或“置頂”
重磅干貨，第一時間送達
本文經(jīng)知乎作者fishmarch授權(quán)轉(zhuǎn)載，二次轉(zhuǎn)載請聯(lián)系作者
原文：https://zhuanlan.zhihu.com/p/45573552
在視覺SLAM問題中，位姿的估計往往是一個遞推的過程，即由上一幀位姿解算當前幀位姿，因此其中的誤差便這樣一幀一幀的傳遞下去，也就是我們所說的累積誤差。
如下圖所示，我們的位姿約束都是與上一幀建立的，第五幀的位姿誤差中便已經(jīng)積累了前面四個約束中的誤差。
但此時，如果我們發(fā)現(xiàn)第五幀位姿不一定要由第四幀推出來，還可以由第二幀推算出來，那顯然這樣計算誤差更小呀，因為只有兩個約束的誤差了嘛。像這樣與之前的某一幀建立位姿約束關(guān)系就叫做回環(huán)。回環(huán)通過減少了約束數(shù)，起到了減小累積誤差的作用。
那現(xiàn)在又有新的問題了，我們怎么知道可以由第二幀推算第五幀位姿呢？就像下圖，可能第一幀、第三幀也可以呀。確實，我們之所以用前一幀遞推下一幀位姿，因為這兩幀足夠近，肯定可以建立兩幀的約束，但是距離較遠的兩幀就不一定可以建立這樣的約束關(guān)系了。找出可以建立這種位姿約束的歷史幀，就是回環(huán)檢測。
那我們現(xiàn)在的重點就是回環(huán)檢測了。其實我們完全可以把以前的所有幀都拿過來和當前幀做匹配，匹配足夠好的就是回環(huán)嘛，但問題是計算量太大了，兩幀匹配本來就慢，這樣做的話還沒有比較好的初值，需要匹配的數(shù)目又如此巨大，CPU和我們都會瘋的。
但其實，任意兩幀是否構(gòu)成回環(huán)可以由更簡單的方法做一個初步的篩選，就像一幀中有一個房子，另一幀中是一棵樹，那這兩幀明顯關(guān)系不大嘛。通過這種方式，我們便可以對回環(huán)做出初步篩選。而這里說的房子、樹就是詞袋模型中的單詞。也就是描述子的進一步抽象集合。
單詞：差距較小的描述子的集合
字典：所有的單詞
因此每一幀都可以用單詞來描述，也就是這一幀中有哪些單詞，這里只關(guān)心了有沒有，而不必關(guān)心具體在哪里。只有兩幀中單詞種類相近才可能構(gòu)成回環(huán)。
因此，現(xiàn)在利用詞袋模型我們將回環(huán)檢測大致分為了以下三個步驟：
構(gòu)建字典（所有單詞的集合)
確定一幀中具有哪些單詞，用向量表示 (1表示具有該單詞，0表示沒有)
比較兩幀描述向量的差異
字典結(jié)構(gòu)
字典由單詞組成，而單詞來自于描述子。并不是說一個描述子就是一個單詞，而是一個單詞表示了一組多個描述子，同組內(nèi)的描述子差異較小。例如，描述子由256位組成，則描述子的種類便有 
種，這個數(shù)相當大了，我們確定單詞，構(gòu)建字典的過程就類似于將這 種描述子進行分類（聚類）的過程，我們可以指定具體分成多少類。那就很像我們現(xiàn)實中的字典了，有厚有薄，也就是看我們分類的多少了。
那現(xiàn)在字典的構(gòu)建也就是一個描述子聚類的過程。聚類算法也有很多了，這里采用了K-means算法，其過程也很簡單：
摘自《視覺SLAM14講》
也可以用下圖表示：
摘自https://www.cnblogs.com/ybjourney/p/4714870.html
現(xiàn)在通過聚類我們獲得了字典，但這里又有一個問題。回想我們平時查字典的過程，不可能直接打開一個一個去找吧，我們會利用目錄，首字母等方法方便我們查找。這里也一樣，一個個去查找速度太慢了，因此我們將字典構(gòu)建成一個K叉樹的結(jié)構(gòu)來加速查找。如下圖所示：
摘自《視覺SLAM14講》
我們在每一層中都用K-means算法進行了聚類，分成了k個類，共有這樣的d層（不包括根節(jié)點）因此在查找過程中，我們逐層向下，最終找到的葉節(jié)點也就是最終的單詞。
還可以用下圖理解：
摘自[2]
現(xiàn)在我們有了字典，但還有一點需要注意。我們利用單詞表示圖像，目的是發(fā)現(xiàn)回環(huán)，也就是兩幀圖像具有較高的相似性。那其實不同的單詞對這一目的的貢獻性是不同的。例如，我這篇文章中有很多“我們”這個詞，但這個詞并不能告訴我們太多信息。而有些單詞例如“回環(huán)”、“K-means”就比較具有代表性，能大概告訴我們這句話講了什么。因此不同的單詞應該具有不同的權(quán)重。
我們用兩個量來描述這種權(quán)重：
IDF（Inverse Document Frequency）：描述單詞在字典中出現(xiàn)的頻率（構(gòu)建字典時），越低越具有代表性
為所有描述子數(shù)， 
為該單詞出現(xiàn)次數(shù)。
的作用大概是降低量級，畢竟 很大。
TF（Term Frequency）：單詞在單幀圖像中出現(xiàn)的頻率，越高越具有代表性
為一幀圖像中所有單詞數(shù)， 為一幀圖像中該單詞出現(xiàn)次數(shù)。
因此將一幀圖像轉(zhuǎn)化為單詞表示時，我們要計算其單詞的權(quán)重：
因此一幀圖像 
由單詞 
、及對應的權(quán)重 
表示：
同時，這里我們要將一幀圖像中的所有的權(quán)重歸一化：
我們在計算兩幀圖像的差異時，就比較對應的權(quán)重即可。
計算q和d兩幀的差異（p表示范數(shù)，常取L1范數(shù)）
第二個等號進行了一個分類：只在q中有的單詞、只在d中有的單詞、在q、d中都有的單詞。
第四個等號：因為進行了歸一化，前兩項都等于1
而兩幀相似度的評分定義如下（越大表示越相似）：
當評分s足夠大時即可判斷兩幀可能為回環(huán)。
在這里簡單說一下，回環(huán)出現(xiàn)后的處理，看得不多，如有錯誤還請指正。
當然回環(huán)的判斷也并沒有這么簡單，含有很多的篩選環(huán)節(jié)，畢竟錯誤的回環(huán)將帶來巨大災難，寧可不要。例如某一位姿附近連續(xù)多次（ORB-SLAM中為3次）與歷史中某一位姿附近出現(xiàn)回環(huán)才判斷為回環(huán)；回環(huán)候選幀仍然要匹配，匹配點足夠才為回環(huán)。
在判斷出現(xiàn)回環(huán)后，兩幀計算Sim3變換（因為有尺度漂移），也就是從歷史幀直接推算當前位姿。
當我們用第m幀推算了回環(huán)幀n的位姿時，使得n的位姿漂移誤差較小，但其實同時可以用第n幀來計算n-1幀的位姿，使n-1幀的位姿漂移誤差也減小。因此，這里還要有一個位姿傳播。
另外我們可以優(yōu)化所有的位姿，也就是進行一個位姿圖優(yōu)化（由位姿變換構(gòu)建位姿約束）。
最后，我們還可以進行一起全局所有變量的BA優(yōu)化。
總結(jié)
總結(jié)來說，詞袋模型通過描述一幀圖像中有哪些單詞，來加速尋找可能閉環(huán)幀的過程。
另外我們也可以利用單詞加速特征點的匹配，例如幀A中的特征點a屬于單詞 
,那在幀B中尋找匹配時，也要去找屬于單詞 的特征點。
其實感覺詞袋模型更近一步就是語義了，也就是將單詞賦予真實的語義信息，同樣可以起到閉環(huán)幀篩選，兩幀特征點輔助匹配的作用。
最后簡單看一下DBoW3中的代碼
其中比較重要的大概就是以下幾個函數(shù)：
構(gòu)造函數(shù)，可以從文件中讀取字典，由圖像序列生成字典、復制字典等
其默認的樹狀結(jié)構(gòu)為分支數(shù)k=10，深度L=5（不包括根節(jié)點）;
權(quán)重為TF_IDF;
評分類型為L1范數(shù)
其主要步驟為：構(gòu)建樹、構(gòu)建單詞、計算權(quán)重（IDF）
構(gòu)建樹：
主要就是一個k_means過程，不過每層都要聚類一次，所以有一個遞歸。
構(gòu)建單詞
尋找葉節(jié)點，添加為單詞
計算權(quán)重
每個字典的IDF是不變的，所以在這里計算IDF部分的權(quán)重
該函數(shù)將一幀圖像轉(zhuǎn)換為單詞表示
當然這里默認是要normalize的
我們再來看一下權(quán)重計算公式
normalize之前我們相當于計算的是
我們將它歸一化，而沒必要計算準確的TF，但也相當于綜合考慮了TF和IDF。
計算兩幀相似度的評分
尋找具有相同單詞的權(quán)重按公式計算即可。
參考文獻：
[1] 高翔 《視覺SLAM十四講》
[2] D. Nister and H. Stewenius, “Scalable recognition with a vocabulary tree,”in 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR’06), vol. 2, pp. 2161–2168, IEEE, 2006.
交流群
歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學影像、GAN、算法競賽等微信群（以后會逐漸細分），請掃描下面微信號加群，備注：”昵稱+學校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據(jù)研究方向邀請進入相關(guān)微信群。請勿在群內(nèi)發(fā)送廣告，否則會請出群，謝謝理解~