ORB_SLAM2代碼閱讀（4）——LoopClosing線程

• 1.說明
• 2.簡介
• 3.檢測回環(huán)
• 4.計算Sim3
• • 4.1 為什么在進(jìn)行回環(huán)檢測的時候需要計算相似變換矩陣，而不是等距變換？
  • 4.2 累積誤差是如何使尺度因子發(fā)生漂移的？
  • 4.3 計算Sim3的過程
• 5.回環(huán)校正

1.說明

在前幾篇博客介紹完tracking和localmaping線程后，本文接著介紹閉環(huán)檢測（LoopClosing）線程中的內(nèi)容。該部分內(nèi)容中有很多細(xì)節(jié)部分還沒有弄清楚，暫時先將整體思路捋順，更多細(xì)節(jié)內(nèi)容以后再補(bǔ)充。

2.簡介

LoopClosing線程整體思路比較簡介清晰，主要分為三大部分：檢測回環(huán)、計算Sim3和回環(huán)校正。該線程的入口為Run()函數(shù)（相當(dāng)于主函數(shù)）。

首先從流程圖中看看LoopClosing線程的整體邏輯（該流程圖對應(yīng)Run()函數(shù)中的內(nèi)容）：

其中較為重要的變量有：

變量名	變量類型	說明
mpCurrentKF	KeyFrame*	當(dāng)前關(guān)鍵幀
mpMatchedKF	KeyFrame*	匹配關(guān)鍵幀
mvConsistentGroups	vector< ConsistentGroup >	所有的連續(xù)關(guān)鍵幀組集
mvpEnoughConsistentCandidates	vector<KeyFrame*>	充分連接的候選關(guān)鍵幀組
mlpLoopKeyFrameQueue	list<KeyFrame*>	待回環(huán)檢測的關(guān)鍵幀隊列

接下來，分別介紹檢測回環(huán)、計算Sim3和回環(huán)校正三部分內(nèi)容。

3.檢測回環(huán)

檢測回環(huán)部分對應(yīng)代碼中的bool LoopClosing::DetectLoop() 函數(shù)，該函數(shù)整體邏輯較為清晰，但是一致性檢測部分有點(diǎn)不好理解，并且相關(guān)資料較少。有些內(nèi)容是我自己的理解，正確性有待商榷。

檢測回環(huán)的主要思想：

1. 在關(guān)鍵幀隊列中提取隊首元素作為當(dāng)前關(guān)鍵幀。關(guān)鍵幀隊列中的元素是局部建圖線程添加的。
2. 獲取當(dāng)前關(guān)鍵幀的共視關(guān)鍵幀，并計算當(dāng)前關(guān)鍵幀與每個共視關(guān)鍵幀之間的BOW向量得分，記錄最小得分minScore。
3. 根據(jù)最小得分minScore在關(guān)鍵幀數(shù)據(jù)庫mpKeyFrameDB中查找當(dāng)前關(guān)鍵幀的回環(huán)候選關(guān)鍵幀vpCandidateKFs。
4. 對每個候選關(guān)鍵幀進(jìn)行一致性檢測。
5. 經(jīng)過一致性檢測后，如果存在充分連接的候選關(guān)鍵幀，則證明檢測到了回環(huán)；否則，回環(huán)檢測失敗。

該過程的具體流程如下圖所示：

對于計算最小得分和獲取候選回環(huán)關(guān)鍵幀等操作并不難理解，根據(jù)最小得分查找候選回環(huán)關(guān)鍵幀的原理以后在進(jìn)行介紹。這里主要說一下一致性檢測的原理。

一致性檢測部分是檢測回環(huán)的關(guān)鍵，光看代碼的話有點(diǎn)難理解，而且網(wǎng)上關(guān)于這部分介紹的也不多。接下來的內(nèi)容是我自己的理解，可能不完全正確，姑且述之。

先說一下該部分涉及的變量。

變量名	變量類型	說明
vpCandidateKFs	vector<KeyFrame*>	候選回環(huán)關(guān)鍵幀向量
pCandidateKF	KeyFrame*	當(dāng)前候選關(guān)鍵幀
spCandidateGroup	set<KeyFrame*>	候選關(guān)鍵幀的共視關(guān)鍵幀以及候選關(guān)鍵幀構(gòu)成了"子候選組"——當(dāng)前子候選組
vCurrentConsistentGroups	vector<ConsistentGroup	當(dāng)前連續(xù)關(guān)鍵幀組構(gòu)成的向量
mvConsistentGroups	vector<ConsistentGroup	由當(dāng)前關(guān)鍵幀的前一關(guān)鍵幀確定的子連續(xù)組向量
mvpEnoughConsistentCandidates	vector<KeyFrame*>	充分連接的候選關(guān)鍵幀組成的向量
nCurrentConsistency	int	用于記錄當(dāng)前子候選組的一致性

一致性檢測的過程：

1. 在候選關(guān)鍵幀向量中選取一個候選關(guān)鍵幀pCandidateKF，將候選關(guān)鍵幀的共視關(guān)鍵幀和候選關(guān)鍵幀組成當(dāng)前子候選組spCandidateGroup。
2. 遍歷 mvConsistentGroups 中的子連續(xù)組。如果 mvConsistentGroups 中的子連續(xù)組sPreviousGroup包含當(dāng)前子候選組中的關(guān)鍵幀，則說明該子連續(xù)組sPreviousGroup與當(dāng)前子候選組是相連的。
3. 如果當(dāng)前子候選組與之前的子連續(xù)組是相連的，則將當(dāng)前子候選組添加到vCurrentConsistentGroups中，并將nCurrentConsistency設(shè)置為與之相連的子連續(xù)組的一致性加一，即nCurrentConsistency = nPreviousConsistency + 1;然后將當(dāng)前子候選組添加到vCurrentConsistentGroups中。
4. 如果nCurrentConsistency大于閾值，則說明當(dāng)前子候選組有足夠多的連接組。因此將當(dāng)前候選關(guān)鍵幀pCandidateKF添加到mvpEnoughConsistentCandidates中用于下一步的Sim3計算。
5. 如果當(dāng)前子候選組與之前的連續(xù)組均無交集，則將其一致性置為0，然后將其添加到vCurrentConsistentGroups中。
6. 重復(fù)該過程直至遍歷完所有的候選關(guān)鍵幀。
7. 用vCurrentConsistentGroups更新mvConsistentGroups。

一致性檢測的原理：
首先要明白回環(huán)處的關(guān)鍵幀會有一定時間和空間上的連續(xù)性。在進(jìn)行一致性檢測的時候，mvConsistentGroups中保存著由上一關(guān)鍵幀確定的連續(xù)關(guān)鍵幀組，這些連續(xù)關(guān)鍵幀組相當(dāng)于確定了一個回環(huán)處的大致范圍。由當(dāng)前關(guān)鍵幀確定的候選關(guān)鍵幀構(gòu)建的子候選組與mvConsistentGroups中的連續(xù)關(guān)鍵幀組由交集，則說明該候選關(guān)鍵幀在回環(huán)處附近。一旦由候選關(guān)鍵幀構(gòu)建的子候選組與mvConsistentGroups中的多個連續(xù)關(guān)鍵幀組有交集時，則說明該候選關(guān)鍵幀在回環(huán)處的可能性更大，因此將其加入到mvpEnoughConsistentCandidates中用于下一步計算相似矩陣。

舉例說明：

圖中的藍(lán)色點(diǎn)為當(dāng)前關(guān)鍵幀，黃色點(diǎn)為當(dāng)前關(guān)鍵幀的上一關(guān)鍵幀；棕色框框住的內(nèi)容為mvConsistentGroups中的連續(xù)關(guān)鍵幀組（P1：123，P2:56，P3: 78）。假設(shè)P1的一致性指數(shù)為2，P2和P3的一致性指數(shù)為1；當(dāng)前關(guān)鍵幀確定的候選關(guān)鍵幀為節(jié)點(diǎn)4和節(jié)點(diǎn)7，節(jié)點(diǎn)4構(gòu)成的子候選組為Q1：345，節(jié)點(diǎn)7構(gòu)成的子候選組為Q2：678。子候選組Q1與P1和P2均相連，所以一致性指數(shù)為3，滿足條件。因此候選關(guān)鍵幀7將被添加到mvpEnoughConsistentCandidates中用來進(jìn)行下一步計算。子候選組Q2與P2和P3相連，但是其一致性指數(shù)為2，不滿足條件。因此要被剔除。

該部分內(nèi)容對應(yīng)的代碼為：

    mvpEnoughConsistentCandidates.clear();     // 當(dāng)前的連續(xù)組vector<ConsistentGroup> vCurrentConsistentGroups;vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++){KeyFrame* pCandidateKF = vpCandidateKFs[i];// 候選關(guān)鍵幀的共視關(guān)鍵幀以及候選關(guān)鍵幀構(gòu)成了"子候選組"set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();spCandidateGroup.insert(pCandidateKF);bool bEnoughConsistent = false;bool bConsistentForSomeGroup = false;//遍歷之前的"子連續(xù)組"for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++){//之前的子連續(xù)組set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;bool bConsistent = false;for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++){if(sPreviousGroup.count(*sit))   //如果之前子連續(xù)組中包含"子候選組"中的幀,則說明該關(guān)鍵幀組與之前的組是連續(xù)的{bConsistent=true;bConsistentForSomeGroup=true;break;}}if(bConsistent)  // 如果與之前的連續(xù)組是連續(xù)的  則將它加入到當(dāng)前連續(xù)組中{int nPreviousConsistency = mvConsistentGroups[iG].second;int nCurrentConsistency = nPreviousConsistency + 1;if(!vbConsistentGroup[iG])   // 如果當(dāng)前連續(xù)組沒有在當(dāng)前連續(xù)組集中,則將其加入{ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);vCurrentConsistentGroups.push_back(cg);    //當(dāng)前連續(xù)組vbConsistentGroup[iG]=true; //this avoid to include the same group more than once }if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)  {mvpEnoughConsistentCandidates.push_back(pCandidateKF);bEnoughConsistent=true; //this avoid to insert the same candidate more than once}}}// If the group is not consistent with any previous group insert with consistency counter set to zeroif(!bConsistentForSomeGroup){ConsistentGroup cg = make_pair(spCandidateGroup,0);vCurrentConsistentGroups.push_back(cg);}}// Update Covisibility Consistent Groups   更新連續(xù)組mvConsistentGroups = vCurrentConsistentGroups;

4.計算Sim3

一旦檢測到閉環(huán)，接下來就要根據(jù)選取的候選關(guān)鍵幀來計算相似變換矩陣并重新計算相關(guān)的地圖點(diǎn)?？梢哉f計算相似矩陣是LoopClosing線程的關(guān)鍵所在。

在介紹該線程中如何計算相似矩陣之前，先了解一下相似變換。
我們知道剛體運(yùn)動可以分解為旋轉(zhuǎn)運(yùn)動和平移運(yùn)動，分別用旋轉(zhuǎn)矩陣R和平移向量t表示。為了能使剛體運(yùn)動能夠進(jìn)行線性計算，我們構(gòu)造了變換矩陣T（變換矩陣T也成為等距變換）：
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
相似變換相當(dāng)于在等距變換的基礎(chǔ)上加上一個尺度因子，表示為
$$S=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$$
為了直觀的體驗一下等距變換與相似變換的區(qū)別，我們可以看一下相似變換對二維圖像的處理效果。

可以直觀的看出圖像經(jīng)過相似變換之后，與等距變換相比，相似變換的結(jié)果尺度發(fā)生了很大的變化。這個效果類似于對圖像下采樣，構(gòu)建圖像金字塔的效果。

4.1 為什么在進(jìn)行回環(huán)檢測的時候需要計算相似變換矩陣，而不是等距變換？

回答這個問題需要從單目slam的尺度不確定性說起。在單目視覺里程計中，求解相機(jī)之間的位姿需要運(yùn)用對極約束求解本質(zhì)矩陣，但是本質(zhì)矩陣具有尺度等價性，這就造成了單目slam的尺度不確定性。為了能使單目slam系統(tǒng)能夠正常運(yùn)行，在起始時刻，有一個初始化過程。初始化的過程是指在單目視覺中，對兩張圖像的平移向量 t 歸一化，相當(dāng)于固定了尺度。雖然我們不知道它的實(shí)際長度為多少，但我們以這時的 t 為單位 1，計算相機(jī)運(yùn)動和特征點(diǎn)的 3D 位置。 在初始化之后，就可以用 3D-2D 來計算相機(jī)運(yùn)動。

在初始化過程完成之后，相當(dāng)于兩張圖像之間的平移關(guān)系已經(jīng)確定，這時可以利用三角化的方式計算特征點(diǎn)的空間坐標(biāo)，在得到特征點(diǎn)的空間坐標(biāo)之后便可以通過3D-2D的方式求解圖像之間的運(yùn)動關(guān)系，從而完成視覺里程計的計算過程。換句話說，這個過程就是利用圖像間的運(yùn)動關(guān)系計算特征點(diǎn)的空間坐標(biāo)，然后利用特征點(diǎn)的空間坐標(biāo)和像素坐標(biāo)計算圖像間的運(yùn)動關(guān)系的不斷向前重復(fù)進(jìn)行的過程。理想情況下，如果系統(tǒng)沒有任何誤差，那么在整個過程中尺度不會發(fā)生漂移。但是由于累積誤差的存在，使得尺度會發(fā)生漂移。

系統(tǒng)為了能夠修正整個尺度漂移，所以在回環(huán)檢測階段計算相似變換矩陣。通過計算得到的尺度因子修正累計誤差造成的尺度漂移。而變換矩陣中并不在尺度因子，所以在回環(huán)檢測的時候需要計算相似變換矩陣而不是等距變換。

4.2 累積誤差是如何使尺度因子發(fā)生漂移的？

先弄清尺度因子到底表示的是什么物理意義。在初始化的過程中，將平移向量t 進(jìn)行了歸一化，也就是說令平移向量的模值為1，但它的真實(shí)模值并不是1。所以平移向量的真實(shí)模值與歸一化之后的模值之比就是尺度因子。

在將平移向量進(jìn)行歸一化處理后，我們會運(yùn)用三角化的方式計算特征點(diǎn)的空間坐標(biāo)（也就是計算特征點(diǎn)的深度），所以尺度因子也可以表示為特征點(diǎn)的真實(shí)深度與用歸一化平移向量計算出的深度之比。如果系統(tǒng)沒有任何誤差，那么在整個過程中尺度不會發(fā)生漂移。但是由于存在誤差，并且誤差會進(jìn)行累計，所以系統(tǒng)運(yùn)行時間越長，我們計算出的特征點(diǎn)的深度與特征點(diǎn)的真實(shí)深度之比（即尺度因子）就會發(fā)生變化。也就是發(fā)生了尺度漂移。

而且尺度漂移和累積誤差是相互影響的，尺度漂移越嚴(yán)重，累積誤差越大；累積誤差越大，也會導(dǎo)致尺度漂移越嚴(yán)重。

使用雙目相機(jī)或深度相機(jī)的時候為什么要計算相似矩陣？
理論上來說，用雙目相機(jī)或深度相機(jī)不存在尺度不確定的問題。從而也就不用考慮尺度漂移的問題。但是相似矩陣在變換矩陣的基礎(chǔ)上多了尺度因子，如果能確定相似矩陣那肯定也就能確定變換矩陣。
這種理解方式是我個人的理解方式，我也不知道正確與否。

4.3 計算Sim3的過程

在代碼中，計算Sim3的主要思路是：

1. 待回環(huán)關(guān)鍵幀（當(dāng)前關(guān)鍵幀）與回環(huán)候選關(guān)鍵幀進(jìn)行詞袋匹配，得到匹配地圖點(diǎn)
2. 如果匹配地圖點(diǎn)的數(shù)量滿足要求，則根據(jù)地圖點(diǎn)來初始化相似矩陣求解器。在該過程中會剔除一部分不滿足要求的候選關(guān)鍵幀
3. 迭代的方式求解相似矩陣。如果迭代次數(shù)達(dá)到最大，則剔除該候選關(guān)鍵幀。
4. 根據(jù)計算得到的相似矩陣重新進(jìn)行地圖點(diǎn)匹配，然后用重新匹配得到的地圖點(diǎn)優(yōu)化相似矩陣。根據(jù)優(yōu)化后的內(nèi)點(diǎn)數(shù)來判斷相似矩陣是否滿足要求。到此，相機(jī)矩陣的計算已經(jīng)結(jié)束。
5. 找到匹配幀的共視關(guān)鍵幀并獲取共視關(guān)鍵幀的地圖點(diǎn)mvpLoopMapPoints。
6. 將mvpLoopMapPoints投影匹配的方式與當(dāng)前關(guān)鍵幀進(jìn)行匹配。得到的匹配地圖點(diǎn)數(shù)量滿足要求，則說明成功找到了回環(huán)，否則失敗。

總結(jié)一下，該部分的內(nèi)容就是：匹配地圖點(diǎn)，迭代計算Sim3，重新匹配地圖點(diǎn)，優(yōu)化Sim3，再次匹配地圖點(diǎn)判斷回環(huán)是否真的發(fā)生。

至于Sim3的計算原理，后面單獨(dú)寫一篇博客來記錄。

該部分內(nèi)容具體的流程可以用以下流程圖表示：

5.回環(huán)校正

在經(jīng)過回環(huán)檢測和相似矩陣計算之后，接下倆就要進(jìn)行回環(huán)校正。在進(jìn)行回環(huán)校正之前，當(dāng)前關(guān)鍵幀、匹配關(guān)鍵幀和相似矩陣等變量均已知。

回環(huán)校正的主要步驟為：

1. 準(zhǔn)備工作：暫停局部建圖線程，停止正在進(jìn)行的全局優(yōu)化，更新當(dāng)前關(guān)鍵幀的連接情況。
2. 構(gòu)建當(dāng)前關(guān)鍵幀的連續(xù)組，并根據(jù)計算出的相似變換矩陣校正連續(xù)組中關(guān)鍵幀的位姿。
3. 將相鄰關(guān)鍵幀的所有地圖點(diǎn)都根據(jù)更新后的相機(jī)位姿(相似變換矩陣)重新計算地圖點(diǎn)世界坐標(biāo) 。
4. 進(jìn)行地圖點(diǎn)融合 。將當(dāng)前幀的地圖點(diǎn)和ComputeSim3過程中當(dāng)前關(guān)鍵幀與候選幀的共視關(guān)鍵幀匹配得到的地圖點(diǎn)進(jìn)行融合。
5. 根據(jù)矯正后的相機(jī)相似矩陣位姿匹配回環(huán)點(diǎn)和當(dāng)前關(guān)鍵幀,并融合得到的關(guān)鍵幀中匹配點(diǎn)和回環(huán)地圖點(diǎn)。代碼中的SearchAndFuse()函數(shù)。
6. 更新當(dāng)前關(guān)鍵幀的共視圖中各個關(guān)鍵幀的相連關(guān)鍵幀,更新連接之后,將這些相鄰關(guān)鍵幀全部加入LoopConnections容器。
7. 進(jìn)行位姿圖優(yōu)化和全局BA。

對于該過程中的SearchAndFuse()部分、位姿圖優(yōu)化和全局BA部分，還沒有仔細(xì)研究。以后有時間研究之后再來補(bǔ)充。

具體流程可以參考以下流程圖：

總結(jié)

以上是生活随笔為你收集整理的ORB_SLAM2代码阅读(4)——LoopClosing线程的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。