迴環檢測是SLAM中有效降低全局誤差，構建全局一致地圖的關鍵模塊。跟蹤丟失的時候，還可用於重定位。根據綜述文獻[1]，檢測迴環的方法有image-to-image, map-to-map, image-to-map三種方法。相比之下，image-to-image方法，也稱為appearance-based基於外觀的方法，具有更好的大場景適應性。現階段，在image-to-image的方法中，常用的方法是基於視覺詞袋方法（Bag of visual words）。針對BRIEF特徵，文獻[2]提出了詞袋方法：DBoW2方法。之後又推出了改進版DBoW3。

本文對DBoW，以及DBoW在ORB-SLAM中的使用細節進行了總結。

0. 迴環的評價指標

對於一個迴環的結果，可能有下面四種情況出現。

把一種迴環檢測演算法在一個大的數據集上測試，四種結果的數量分別為 N_{TP} , N_{TN} ， N_{FP} , N_{FN} 。我們可以獲得兩個統計量，分別為準確率（Precision）和召回率（Recall）

Precision=frac{N_{TP}}{N_{TP}+N_{FP}}

Recall = frac{N_{TP}}{N_{TP} +N_{FN}}

準確率描述的是檢測到是迴環的結果裡面有多少是真的迴環。在SLAM中，我們對這個指標要求很高，寧缺毋濫。錯誤的迴環會對SLAM系統帶來災難性的後果。在檢測到迴環之後，一般還需要經過各種校驗，才會接收這次迴環。

召回率描述的是所有實際的迴環中，我們能檢測出多少個迴環。當然這指標也是越多越好，但是實際上準確率和召回率往往是矛盾的。

對於一個迴環演算法，通過調整參數，在一個數據集上測試，可繪製出一個準確率-召回率曲線，如下圖。曲線的右上角越靠近右上角越好，越方越好，太繞了，O(∩_∩)O哈哈~。

準確率-召回率曲線，摘自[2]

1. DBoW2視覺詞袋

我們的目的是比較兩張圖像之間的相似度，通過相似度來判斷是否會迴環。那麼如何比較相似度呢？一種是可以通過匹配兩幀之間的特徵，統計一下匹配點的數量，這種方法太慢了。原因是什麼呢？特徵點本身是對圖像信息進行了篩選、抽象和降維，哪是不是可以再抽象一下呢？可以，把特徵再抽象為單詞。比如，「車」，「貓」，「人」，打比方而已哈。那麼我們可以分別統計兩張圖上有沒有車，貓，人。據此來判斷兩幅圖像是否相似。 這裡只有有、無（0、1）兩種量。

DBOW2將BRIEF的聚類成 n 個單詞： w_1, w_2, cdots,w_n 。一幅圖像上提取的特徵可以轉換為單詞表示：

A=1 cdot w_1 + 1 cdot w_2 + 0 cdot w_3 + cdots + 1cdot w_n = [w_1, w_2, w_3, cdots, w_n][1, 1, 0, cdots, 1]^T

因為視覺單詞是固定的，所以一幅圖像就可以用向量表示：

v_A = [1, 1, 0, cdots, 1]^T

也就是說我們只需要對比詞袋向量，就可以計算兩幅圖像的相似性。怎麼對比能，有不同的分數計算方法，例如：

s(A,B) = 1-frac{1}{n}||v_A -v_B||_1

這裡的計算類似於漢明距離，計算的是不相同的單詞數量。

需要注意的是，實際的DBoW2中也不是隻考慮單詞的有無，也是會根據單詞出現的多少考慮權重。另外，相似分數的計算方式也不太相同。後文會解釋。

那麼問題來了，上述的那些單詞如何來的呢？DBoW2通過在大量圖像中提取特徵，利用K-Means方法聚類出 n 個單詞。這個 n 個單詞也不是一次聚類而成的，而是利用了K叉樹。具體如下圖。在每一層依次用K-means演算法聚類成 K 類。最後形成的葉子節點就是單詞。深度為 d 的 K 叉樹形成的單詞數量為 n=K^d 。在DBoW2中默認的 K=10, d=5 ，可以形成10000個單詞。

利用 K 叉樹的好處是什麼呢？主要是加速。

1. 可以加速判斷一個特徵屬於哪個單詞。如果不使用K叉樹，就需要與每個單詞比較（計算漢明距離），共計需要比較 K^d 次。而使用K叉樹之後，需要的比較次數變為 K imes (d-1) 次。大大提高了速度。這裡利用了K叉樹的快速查找特性。

2. DBoW中存儲了Direct Index，也就每個節點存儲有一幅圖像上所有歸屬與該節點的特徵的index。在兩幀進行特徵匹配的時候，只需要針對每一個節點進行匹配就好了，大大縮小了匹配空間，加速了匹配速度。ORB-SLAM幀間匹配都使用了這個trick。3. 除此之外，如下圖，DBoW的每個單詞中還存儲了Inverse index，每個Inverse Index中存儲有所有有此單詞的圖像index，以及對應的權重： <I_i, eta_i> 。在進行閉環搜索的時候，可以加快搜索過程的。具體的，我們只需要找與當前關鍵幀有相同單詞的關鍵幀就可以了。

下面再來說DBoW2是如何計算相似分數的。

在構建詞典的時候，使用了大量的數據。在這個環境中有的單詞出現的次數多，有的單詞出現的少。直觀想一下，出現次數多的單詞其實對環境不具有區分度，反而出現次數少的單詞很有區分性。比如一個單詞叫做「地磚」，環境中很多，用這個單詞來比較兩幅圖像的話就不能顯著區分兩個環境。因此，引入一個TF-IDF（Term Frequency-Inverse Document Frequency）的權重，來降低這種單詞的作用。

IDF_i= log{frac{n}{n_i}}

n_i 為該單詞出現的次數， n 為所有單詞的總次數。（註：這裡說的是訓練過程，詞典構建過程。）明顯的， n_i 越大，權重越小。

至此詞典就構建OK了：一個K叉樹，兩個index（Inverse index，Direct Index），一個權重（TF-IDF）。

下面開始使用詞典比較圖像相似度。將一幅圖像A的所有特徵轉換為詞袋向量，這裡不僅僅要考慮單詞有無的問題，還要考慮單詞的權重。A中有的單詞多，有的單詞少。單詞多自然要加大權重，單詞少自然要減少權重啦。引入另一個權重叫做TF（Term Frequency）：

TF_i = frac{n_i}{n}

最後當前圖像的一次單詞的權重取為IDF和TF的乘積

eta_i = TF_i imes IDF_i

這樣，就組成了一個帶權的詞袋向量。

A = { (w_1,eta_1), (w_2, eta_2), cdots, (w_N, eta_N) } :v_A

下面計算兩幅圖像A,B的相似度，這裡就要考慮權重了。計算相似度的方法由很多，DBoW2的庫裡面就有L1、L2、ChiSquare等六種計算方式。比如L1距離為：

s(v_A, v_B) = frac 1 2 sum_{i} {|v_{Ai} | + |v_{Bi} | - |v_{Ai}-v_{Bi} |}

實際上，在比較相似度的時候只需要計算上述的分數就好了，這個速度就比特徵匹配快的多。

2. ORB-SLAM中的迴環

ORB-SLAM維護了一個資料庫，這個資料庫裡面保存了每個單詞能看到的關鍵幀。（inverse index）。每個關鍵幀都要從這個資料庫中檢測迴環，檢測完迴環之後，每個關鍵幀都會加入到這個資料庫中。當一個關鍵幀被刪除之後，這個資料庫也會被同時更新。這樣做的好處就是加快迴環搜索的速度，這個與DBoW的inverse index是相同的思想，只不過ORB-SLAM自己去維護了以下而已。

2.1 搜索閉環候選幀

下面進入正式的流程，因為ORB-SLAM使用了共視圖，很多操作都可以採用共視圖輔助。

這個流程吳博的視頻已經講的很清楚了，我這裡做個搬運工作。

摘自吳博《ORB-SLAM 代碼詳細解讀》[3]
  1. 做了降頻，要求閉環與閉環之間至少經過10個關鍵幀。

2. 計算一個相對分數minsocre：計算當前幀與其共視圖中的幀的相似分數，取其中最小的那個作為minsocre。因為DBoW計算出的絕對分數不具有可比性，取當前幀與共視幀之間最小的相似度作為基準，如果大於這個基準纔有可能是閉環幀。

3. 從上文中說的資料庫中查找閉環候選幀。因為資料庫中維護了Inverse Index，所以可以快速找到與當前幀有共同單詞的關鍵幀。其中就會有一個關鍵幀具有的共同單詞數最大，為maxcommonwords。根據這個設一個閾值，mincommons = 0.8 * maxcommonwords，據此去掉共同單詞太少的候選關鍵。再加上minscore的要求幹掉一部分關鍵幀。再再加上候選幀不能在當前幀的共視圖中的條件，又幹掉一部分。之後，把相連的的候選幀歸為一組，每個組計算一個累加分數，可以找到一個最大分數bestAccScore。據此，設置一個閾值，minScoreToRetain = 0.75*bestAccScore。用這個閾值幹掉一部分分組，這樣可以幹掉一些孤立的閉環幀候選幀（孤立的可能都是錯誤的閉環）。剩下的這些分組中的關鍵幀就是閉環候選幀啦。

4. 連續性檢測。現在，進一步對候選關鍵幀進行篩選。閉環不僅僅是單幀對單幀的匹配，好的閉環應該是多幀對多幀的閉環。也就是形成連續閉環，如下圖所示。這一步請參考[4]。

摘自[4]

5. 至此可以得到若干的比較可靠的候選關鍵幀了：mvpEnoughConsistentCandidates。上面那麼多的步驟，都是為了保證準確率。SLAM中要求閉環的準確率達到100%。

2.2 幾何校驗，sim3

下面，還要進行幾何校驗。對每一個候選幀與當前幀計算Sim3變換。

  1. 匹配當前幀與閉環幀之間的特徵點，利用DBoW加速一下（Direct Index的應用），匹配太少的候選幀直接就幹掉了。
  2. 計算一個Sim3初值。
  3. 利用Sim3初值將候選關鍵幀的地圖點再投影到當前幀，得到更多匹配。
  4. 然後優化Sim3. 第2-4步是在RANSAC框架下做的，只要有一個幀通過了測試，就跳出去了。這樣就選出了一個閉環幀。
  5. 把閉環幀的共視關鍵幀全部取出來，形成了局部地圖，把局部地圖點再投到當前幀匹配，又形成了更多的匹配。檢查一下匹配點的數量來確定是否接受此次閉環。

至此，已經找到了一個閉環幀，並且計算出了當前幀與閉環幀之間的Sim3變換。

2.3 Loop fusion

下面要去做Loop fusion：調整關鍵幀位姿，更新共視圖。

首先，把當前幀和當前幀的相鄰幀（共視幀&共視幀的共視幀），根據前面的Sim3變換對齊到閉環幀上。這樣做的好處是可以迅速把相機拉回到閉環位姿，並且把局部地圖也拉回去了，可以保證定位的連續性。

然後，更新共視圖關係，把閉環幀和閉環幀的共視幀上的地圖點投影到當前幀上來。進行地圖點融合，匹配，更新共視幀。

上面的步驟其實就是把當前幀的局部地圖和閉環幀的局部地圖縫合起來。

2.4 優化

接下來，就要開始優化啦。

首先基於Essential graph優化一下，然後再來一個全局BA。

參考資料

[1] Williams B , Cummins M , José Neira, et al. A comparison of loop closing techniques in monocular SLAM[J]. Robotics and Autonomous Systems, 2009, 57(12):1188-1197.

[2] Galvez-Lo?Pez D , Tardos J D . Bags of Binary Words for Fast Place Recognition in Image Sequences[J]. IEEE Transactions on Robotics, 2012, 28(5):1188-1197.

[3] 第三十六課：ORB-SLAM2源碼解析 -吳博

[4] blog.csdn.net/chishuide

----相關代碼----

ydsf16 - Overview?

github.com

----更多SLAM文章----

楊小東：[ORB-SLAM2] ORB-SLAM中的ORB特徵（提取）

楊小東：[ORB-SLAM2]單目初始化

楊小東：SLAM軌跡真值獲取裝置

楊小東：[PnP]PnP問題之EPnP解法

楊小東：[PnP] PnP問題之DLT解法

楊小東：[ORB-SLAM2]卡方分佈(Chi-squared)外點(outlier)剔除

楊小東：[ORB-SLAM2] ORB特徵提取策略對ORB-SLAM2性能的影響

楊小東：[PR-3]ArUco EKF SLAM 擴展卡爾曼SLAM

楊小東：[PR-2] PF 粒子濾波/蒙特卡羅定位

推薦閱讀：

相關文章