ORB SLAM2原始碼解讀(十一)：LoopClosing類
LoopClosing是專門負責做閉環的類，它的主要功能就是檢測閉環，計算閉環幀的相對位姿病以此做閉環修正。
老規矩，先上一張圖，清晰明瞭
從圖裡可以看出，這個類最主要的就是三個函式，下面分別介紹
1. DetectLoop：檢測閉環
它的主要流程包括：
1）如果地圖中的關鍵幀數小於10，那麼不進行閉環檢測
2）獲取共視關鍵幀，並計算他們和當前關鍵幀之間的BoW分數，求得最低分
3）透過上一步計算出的最低分數到資料庫中查找出候選關鍵幀，這一步相當於是找到了曾經到過此處的關鍵幀們
4）對候選關鍵幀集進行連續性檢測
bool
LoopClosing
：：
DetectLoop
（）
{
// 取出一個關鍵幀
{
unique_lock
<
mutex
>
lock
（
mMutexLoopQueue
）；
mpCurrentKF
=
mlpLoopKeyFrameQueue
。
front
（）；
mlpLoopKeyFrameQueue
。
pop_front
（）；
// Avoid that a keyframe can be erased while it is being process by this thread
mpCurrentKF
->
SetNotErase
（）；
}
//If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
// 如果距離上次閉環小於10幀，則不進行閉環檢測
if
（
mpCurrentKF
->
mnId
<
mLastLoopKFid
+
10
）
{
mpKeyFrameDB
->
add
（
mpCurrentKF
）；
mpCurrentKF
->
SetErase
（）；
return
false
；
}
// Compute reference BoW similarity score
// This is the lowest score to a connected keyframe in the covisibility graph
// We will impose loop candidates to have a higher similarity than this
//計算當前幀及其共視關鍵幀的詞袋模型匹配得分，獲得minScore
const
vector
<
KeyFrame
*>
vpConnectedKeyFrames
=
mpCurrentKF
->
GetVectorCovisibleKeyFrames
（）；
const
DBoW2
：：
BowVector
&
CurrentBowVec
=
mpCurrentKF
->
mBowVec
；
float
minScore
=
1
；
for
（
size_t
i
=
0
；
i
<
vpConnectedKeyFrames
。
size
（）；
i
++
）
{
KeyFrame
*
pKF
=
vpConnectedKeyFrames
［
i
］；
if
（
pKF
->
isBad
（））
continue
；
const
DBoW2
：：
BowVector
&
BowVec
=
pKF
->
mBowVec
；
float
score
=
mpORBVocabulary
->
score
（
CurrentBowVec
，
BowVec
）；
if
（
score
<
minScore
）
minScore
=
score
；
}
// Query the database imposing the minimum score
//在除去當前幀共視關係的關鍵幀資料中，檢測閉環候選幀（這個函式在KeyFrameDatabase中）
//閉環候選幀刪選過程：
//1，BoW得分>minScore；
//2，統計滿足1的關鍵幀中有共同單子最多的單詞數maxcommonwords
//3，篩選出共同單詞數大於mincommons（=0。8*maxcommons）的關鍵幀
//4，相連的關鍵幀分為一組，計算組得分（總分），得到最大總分bestAccScore，篩選出總分大於minScoreToRetain（=0。75*bestAccScore）的組
//用得分最高的候選幀IAccScoreAndMathch代表該組，計算組得分的目的是剔除單獨一幀得分較高，但是沒有共視關鍵幀作為閉環來說不夠魯棒
//對於通過了閉環檢測的關鍵幀，還需要透過連續性檢測（連續三幀都透過上面的篩選），才能作為閉環候選幀
vector
<
KeyFrame
*>
vpCandidateKFs
=
mpKeyFrameDB
->
DetectLoopCandidates
（
mpCurrentKF
，
minScore
）；
// If there are no loop candidates， just add new keyframe and return false
if
（
vpCandidateKFs
。
empty
（））
{
mpKeyFrameDB
->
add
（
mpCurrentKF
）；
mvConsistentGroups
。
clear
（）；
mpCurrentKF
->
SetErase
（）；
return
false
；
}
// For each loop candidate check consistency with previous loop candidates
// Each candidate expands a covisibility group （keyframes connected to the loop candidate in the covisibility graph）
// A group is consistent with a previous group if they share at least a keyframe
// We must detect a consistent loop in several consecutive keyframes to accept it
// 在候選幀中檢測具有連續性的候選幀
// 每個候選幀將與自己相連的關鍵幀構成一個“候選組spCandidateGroup”
// 檢測“候選組”中每一個關鍵幀是否存在於“連續組”，如果存在nCurrentConsistency++，將該“候選組”放入“當前連續組vCurrentConsistentGroups”
// 如果nCurrentConsistency大於等於3，那麼該”子候選組“代表的候選幀過關，進入mvpEnoughConsistentCandidates
mvpEnoughConsistentCandidates
。
clear
（）；
vector
<
ConsistentGroup
>
vCurrentConsistentGroups
；
vector
<
bool
>
vbConsistentGroup
（
mvConsistentGroups
。
size
（），
false
）；
for
（
size_t
i
=
0
，
iend
=
vpCandidateKFs
。
size
（）；
i
<
iend
；
i
++
）
{
KeyFrame
*
pCandidateKF
=
vpCandidateKFs
［
i
］；
// 將自己以及與自己相連的關鍵幀構成一個“候選組”
set
<
KeyFrame
*>
spCandidateGroup
=
pCandidateKF
->
GetConnectedKeyFrames
（）；
spCandidateGroup
。
insert
（
pCandidateKF
）；
bool
bEnoughConsistent
=
false
；
bool
bConsistentForSomeGroup
=
false
；
// 遍歷之前的“連續組”
for
（
size_t
iG
=
0
，
iendG
=
mvConsistentGroups
。
size
（）；
iG
<
iendG
；
iG
++
）
{
// 取出一個之前的連續組
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
））
{
bConsistent
=
true
；
bConsistentForSomeGroup
=
true
；
// 該“候選組”至少與一個”連續組“相連
break
；
}
}
if
（
bConsistent
）
{
int
nPreviousConsistency
=
mvConsistentGroups
［
iG
］。
second
；
int
nCurrentConsistency
=
nPreviousConsistency
+
1
；
if
（
！
vbConsistentGroup
［
iG
］）
{
ConsistentGroup
cg
=
make_pair
（
spCandidateGroup
，
nCurrentConsistency
）；
vCurrentConsistentGroups
。
push_back
（
cg
）；
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 zero
// 如果該“候選組”的所有關鍵幀都不存在於“連續組”，那麼vCurrentConsistentGroups將為空，
// 於是就把“子候選組”全部複製到vCurrentConsistentGroups，並最終用於更新mvConsistentGroups，計數設為0，重新開始
if
（
！
bConsistentForSomeGroup
）
{
ConsistentGroup
cg
=
make_pair
（
spCandidateGroup
，
0
）；
vCurrentConsistentGroups
。
push_back
（
cg
）；
}
}
// Update Covisibility Consistent Groups
mvConsistentGroups
=
vCurrentConsistentGroups
；
// Add Current Keyframe to database
mpKeyFrameDB
->
add
（
mpCurrentKF
）；
if
（
mvpEnoughConsistentCandidates
。
empty
（））
{
mpCurrentKF
->
SetErase
（）；
return
false
；
}
else
{
return
true
；
}
mpCurrentKF
->
SetErase
（）；
return
false
；
}
2. ComputeSim3：計算兩幀之間的相對位姿
主要流程包括：
1）對每一個閉環幀，透過BoW的matcher方法進行第一次匹配，匹配閉環幀和當前關鍵幀之間的匹配關係，如果對應關係少於20個，則丟棄，否則構造一個Sim3求解器並儲存起來。
2）對上一步得到的每一個滿足條件的閉環幀，透過RANSAC迭代，求解Sim3。
3）透過返回的Sim3進行第二次匹配。
4）使用非線性最小二乘法最佳化Sim3。
5）使用非線性最小二乘法最佳化Sim3。
6）使用投影得到更多的匹配點，如果匹配點數量充足，則接受該閉環。
bool
LoopClosing
：：
ComputeSim3
（）
{
// For each consistent loop candidate we try to compute a Sim3
const
int
nInitialCandidates
=
mvpEnoughConsistentCandidates
。
size
（）；
// We compute first ORB matches for each candidate
// If enough matches are found， we setup a Sim3Solver
ORBmatcher
matcher
（
0。75
，
true
）；
vector
<
Sim3Solver
*>
vpSim3Solvers
；
vpSim3Solvers
。
resize
（
nInitialCandidates
）；
vector
<
vector
<
MapPoint
*>
>
vvpMapPointMatches
；
vvpMapPointMatches
。
resize
（
nInitialCandidates
）；
vector
<
bool
>
vbDiscarded
；
vbDiscarded
。
resize
（
nInitialCandidates
）；
int
nCandidates
=
0
；
//candidates with enough matches
for
（
int
i
=
0
；
i
<
nInitialCandidates
；
i
++
）
{
// 閉環候選幀中取出一幀關鍵幀pKF
KeyFrame
*
pKF
=
mvpEnoughConsistentCandidates
［
i
］；
// avoid that local mapping erase it while it is being processed in this thread
// 防止在LocalMapping中KeyFrameCulling函式將此關鍵幀作為冗餘幀剔除
pKF
->
SetNotErase
（）；
if
（
pKF
->
isBad
（））
{
vbDiscarded
［
i
］
=
true
；
continue
；
}
// 將當前幀mpCurrentKF與閉環候選關鍵幀pKF匹配
// 透過bow加速得到mpCurrentKF與pKF之間的匹配特徵點，vvpMapPointMatches是匹配特徵點對應的MapPoints
int
nmatches
=
matcher
。
SearchByBoW
（
mpCurrentKF
，
pKF
，
vvpMapPointMatches
［
i
］）；
// 匹配的特徵點數太少，剔除
if
（
nmatches
<
20
）
{
vbDiscarded
［
i
］
=
true
；
continue
；
}
else
{
// 構造Sim3求解器
// 如果mbFixScale為true，則是6DoFf最佳化，如果是false，則是7DoF最佳化（單目）
Sim3Solver
*
pSolver
=
new
Sim3Solver
（
mpCurrentKF
，
pKF
，
vvpMapPointMatches
［
i
］，
mbFixScale
）；
pSolver
->
SetRansacParameters
（
0。99
，
20
，
300
）；
vpSim3Solvers
［
i
］
=
pSolver
；
}
// 參與Sim3計算的候選關鍵幀數加1
nCandidates
++
；
}
bool
bMatch
=
false
；
// 標記是否有一個候選幀透過Sim3的求解
// Perform alternatively RANSAC iterations for each candidate
// until one is succesful or all fail
// 一直迴圈所有的候選幀，每個候選幀迭代5次，如果5次迭代後得不到結果，就換下一個候選幀
// 直到有一個候選幀首次迭代成功，或者某個候選幀總的迭代次數超過限制，直接將它剔除
while
（
nCandidates
>
0
&&
！
bMatch
）
{
for
（
int
i
=
0
；
i
<
nInitialCandidates
；
i
++
）
{
if
（
vbDiscarded
［
i
］）
continue
；
KeyFrame
*
pKF
=
mvpEnoughConsistentCandidates
［
i
］；
// Perform 5 Ransac Iterations
vector
<
bool
>
vbInliers
；
int
nInliers
；
bool
bNoMore
；
// 對有較好的匹配的關鍵幀求取Sim3變換
Sim3Solver
*
pSolver
=
vpSim3Solvers
［
i
］；
cv
：：
Mat
Scm
=
pSolver
->
iterate
（
5
，
bNoMore
，
vbInliers
，
nInliers
）；
// If Ransac reachs max。 iterations discard keyframe
// 總迭代次數達到最大限制還沒有求出合格的Sim3變換，該候選幀剔除
if
（
bNoMore
）
{
vbDiscarded
［
i
］
=
true
；
nCandidates
——
；
}
// If RANSAC returns a Sim3， perform a guided matching and optimize with all correspondences
if
（
！
Scm
。
empty
（））
{
vector
<
MapPoint
*>
vpMapPointMatches
（
vvpMapPointMatches
［
i
］。
size
（），
static_cast
<
MapPoint
*>
（
NULL
））；
for
（
size_t
j
=
0
，
jend
=
vbInliers
。
size
（）；
j
<
jend
；
j
++
）
{
// 儲存inlier的MapPoint
if
（
vbInliers
［
j
］）
vpMapPointMatches
［
j
］
=
vvpMapPointMatches
［
i
］［
j
］；
}
// 透過步驟3求取的Sim3變換引導關鍵幀匹配彌補步驟2中的漏匹配
cv
：：
Mat
R
=
pSolver
->
GetEstimatedRotation
（）；
cv
：：
Mat
t
=
pSolver
->
GetEstimatedTranslation
（）；
const
float
s
=
pSolver
->
GetEstimatedScale
（）；
// 查詢更多的匹配 使用SearchByBoW進行特徵點匹配時會有漏匹配
// 透過Sim3變換，確定pKF1的特徵點在pKF2中的大致區域，同理，確定pKF2的特徵點在pKF1中的大致區域
// 在該區域內透過描述子進行匹配pKF1和pKF2之前漏匹配的特徵點，更新匹配vpMapPointMatches
matcher
。
SearchBySim3
（
mpCurrentKF
，
pKF
，
vpMapPointMatches
，
s
，
R
，
t
，
7。5
）；
// Sim3最佳化，只要有一個候選幀透過Sim3的求解與最佳化，就跳出停止對其它候選幀的判斷
g2o
：：
Sim3
gScm
（
Converter
：：
toMatrix3d
（
R
），
Converter
：：
toVector3d
（
t
），
s
）；
// 最佳化mpCurrentKF與pKF對應的MapPoints間的Sim3，得到最佳化後的量gScm
const
int
nInliers
=
Optimizer
：：
OptimizeSim3
（
mpCurrentKF
，
pKF
，
vpMapPointMatches
，
gScm
，
10
，
mbFixScale
）；
// If optimization is succesful stop ransacs and continue
if
（
nInliers
>=
20
）
{
bMatch
=
true
；
// mpMatchedKF是最終閉環檢測出來與當前幀形成閉環的關鍵幀
mpMatchedKF
=
pKF
；
// 得到從世界座標系到該候選幀的Sim3變換
g2o
：：
Sim3
gSmw
（
Converter
：：
toMatrix3d
（
pKF
->
GetRotation
（）），
Converter
：：
toVector3d
（
pKF
->
GetTranslation
（）），
1。0
）；
// 得到最佳化後從世界座標系到當前幀的Sim3變換
mg2oScw
=
gScm
*
gSmw
；
mScw
=
Converter
：：
toCvMat
（
mg2oScw
）；
mvpCurrentMatchedPoints
=
vpMapPointMatches
；
break
；
//跳出對其它候選幀的判斷
}
}
}
}
// 沒有一個閉環匹配候選幀透過Sim3的求解與最佳化
if
（
！
bMatch
）
{
for
（
int
i
=
0
；
i
<
nInitialCandidates
；
i
++
）
mvpEnoughConsistentCandidates
［
i
］
->
SetErase
（）；
mpCurrentKF
->
SetErase
（）；
return
false
；
}
// Retrieve MapPoints seen in Loop Keyframe and neighbors
// 取出閉環匹配上關鍵幀的相連關鍵幀，得到它們的MapPoints放入mvpLoopMapPoints
// 將mpMatchedKF相連的關鍵幀全部取出來放入vpLoopConnectedKFs
vector
<
KeyFrame
*>
vpLoopConnectedKFs
=
mpMatchedKF
->
GetVectorCovisibleKeyFrames
（）；
vpLoopConnectedKFs
。
push_back
（
mpMatchedKF
）；
mvpLoopMapPoints
。
clear
（）；
for
（
vector
<
KeyFrame
*>：：
iterator
vit
=
vpLoopConnectedKFs
。
begin
（）；
vit
！=
vpLoopConnectedKFs
。
end
（）；
vit
++
）
{
KeyFrame
*
pKF
=
*
vit
；
vector
<
MapPoint
*>
vpMapPoints
=
pKF
->
GetMapPointMatches
（）；
for
（
size_t
i
=
0
，
iend
=
vpMapPoints
。
size
（）；
i
<
iend
；
i
++
）
{
MapPoint
*
pMP
=
vpMapPoints
［
i
］；
if
（
pMP
）
{
if
（
！
pMP
->
isBad
（）
&&
pMP
->
mnLoopPointForKF
！=
mpCurrentKF
->
mnId
）
{
mvpLoopMapPoints
。
push_back
（
pMP
）；
pMP
->
mnLoopPointForKF
=
mpCurrentKF
->
mnId
；
}
}
}
}
// Find more matches projecting with the computed Sim3
// 將閉環匹配上關鍵幀以及相連關鍵幀的MapPoints投影到當前關鍵幀進行投影匹配
// 根據投影查詢更多的匹配
// 根據Sim3變換，將每個mvpLoopMapPoints投影到mpCurrentKF上，並根據尺度確定一個搜尋區域，
// 根據該MapPoint的描述子與該區域內的特徵點進行匹配，如果匹配誤差小於TH_LOW即匹配成功，更新mvpCurrentMatchedPoints
matcher
。
SearchByProjection
（
mpCurrentKF
，
mScw
，
mvpLoopMapPoints
，
mvpCurrentMatchedPoints
，
10
）；
// If enough matches accept Loop
// 斷當前幀與檢測出的所有閉環關鍵幀是否有足夠多的MapPoints匹配
int
nTotalMatches
=
0
；
for
（
size_t
i
=
0
；
i
<
mvpCurrentMatchedPoints
。
size
（）；
i
++
）
{
if
（
mvpCurrentMatchedPoints
［
i
］）
nTotalMatches
++
；
}
if
（
nTotalMatches
>=
40
）
{
for
（
int
i
=
0
；
i
<
nInitialCandidates
；
i
++
）
if
（
mvpEnoughConsistentCandidates
［
i
］
！=
mpMatchedKF
）
mvpEnoughConsistentCandidates
［
i
］
->
SetErase
（）；
return
true
；
}
else
{
for
（
int
i
=
0
；
i
<
nInitialCandidates
；
i
++
）
mvpEnoughConsistentCandidates
［
i
］
->
SetErase
（）；
mpCurrentKF
->
SetErase
（）；
return
false
；
}
}
3. CorrectLoop：根據閉環做校正
主要流程包括：
1）如果有全域性BA運算在執行的話，終止之前的BA運算。
2）使用傳播法計算每一個關鍵幀正確的Sim3變換值
3）最佳化圖
4）全域性BA最佳化
void
LoopClosing
：：
CorrectLoop
（）
{
cout
<<
“Loop detected！”
<<
endl
；
// Send a stop signal to Local Mapping
// Avoid new keyframes are inserted while correcting the loop
//請求區域性地圖停止
mpLocalMapper
->
RequestStop
（）；
// If a Global Bundle Adjustment is running， abort it
if
（
isRunningGBA
（））
{
unique_lock
<
mutex
>
lock
（
mMutexGBA
）；
mbStopGBA
=
true
；
mnFullBAIdx
++
；
if
（
mpThreadGBA
）
{
mpThreadGBA
->
detach
（）；
delete
mpThreadGBA
；
}
}
// Wait until Local Mapping has effectively stopped
while
（
！
mpLocalMapper
->
isStopped
（））
{
usleep
（
1000
）；
}
// Ensure current keyframe is updated
// 根據共視關係更新當前幀與其它關鍵幀之間的連線
mpCurrentKF
->
UpdateConnections
（）；
// Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
// 得到Sim3最佳化後，與當前幀相連的關鍵幀的位姿，以及它們的MapPoints
// 透過相對位姿關係，可以確定這些相連的關鍵幀與世界座標系之間的Sim3變換
// 取出與當前幀相連的關鍵幀，包括當前關鍵幀
mvpCurrentConnectedKFs
=
mpCurrentKF
->
GetVectorCovisibleKeyFrames
（）；
mvpCurrentConnectedKFs
。
push_back
（
mpCurrentKF
）；
KeyFrameAndPose
CorrectedSim3
，
NonCorrectedSim3
；
CorrectedSim3
［
mpCurrentKF
］
=
mg2oScw
；
cv
：：
Mat
Twc
=
mpCurrentKF
->
GetPoseInverse
（）；
{
// Get Map Mutex
unique_lock
<
mutex
>
lock
（
mpMap
->
mMutexMapUpdate
）；
// 得到Sim3調整後其它與當前幀相連關鍵幀的位姿
for
（
vector
<
KeyFrame
*>：：
iterator
vit
=
mvpCurrentConnectedKFs
。
begin
（），
vend
=
mvpCurrentConnectedKFs
。
end
（）；
vit
！=
vend
；
vit
++
）
{
KeyFrame
*
pKFi
=
*
vit
；
cv
：：
Mat
Tiw
=
pKFi
->
GetPose
（）；
if
（
pKFi
！=
mpCurrentKF
）
{
// 得到當前幀到pKFi幀的相對變換
cv
：：
Mat
Tic
=
Tiw
*
Twc
；
cv
：：
Mat
Ric
=
Tic
。
rowRange
（
0
，
3
）。
colRange
（
0
，
3
）；
cv
：：
Mat
tic
=
Tic
。
rowRange
（
0
，
3
）。
col
（
3
）；
g2o
：：
Sim3
g2oSic
（
Converter
：：
toMatrix3d
（
Ric
），
Converter
：：
toVector3d
（
tic
），
1。0
）；
// 當前幀的位姿固定不動，其它的關鍵幀根據相對關係得到Sim3調整的位姿
g2o
：：
Sim3
g2oCorrectedSiw
=
g2oSic
*
mg2oScw
；
//Pose corrected with the Sim3 of the loop closure
// 得到閉環g2o最佳化後各個關鍵幀的位姿
CorrectedSim3
［
pKFi
］
=
g2oCorrectedSiw
；
}
cv
：：
Mat
Riw
=
Tiw
。
rowRange
（
0
，
3
）。
colRange
（
0
，
3
）；
cv
：：
Mat
tiw
=
Tiw
。
rowRange
（
0
，
3
）。
col
（
3
）；
g2o
：：
Sim3
g2oSiw
（
Converter
：：
toMatrix3d
（
Riw
），
Converter
：：
toVector3d
（
tiw
），
1。0
）；
//Pose without correction
// 當前幀相連關鍵幀，沒有進行閉環最佳化的位姿
NonCorrectedSim3
［
pKFi
］
=
g2oSiw
；
}
// Correct all MapPoints obsrved by current keyframe and neighbors， so that they align with the other side of the loop
// 上一步得到調整相連幀位姿後，修正這些關鍵幀的地圖點
for
（
KeyFrameAndPose
：：
iterator
mit
=
CorrectedSim3
。
begin
（），
mend
=
CorrectedSim3
。
end
（）；
mit
！=
mend
；
mit
++
）
{
KeyFrame
*
pKFi
=
mit
->
first
；
g2o
：：
Sim3
g2oCorrectedSiw
=
mit
->
second
；
g2o
：：
Sim3
g2oCorrectedSwi
=
g2oCorrectedSiw
。
inverse
（）；
g2o
：：
Sim3
g2oSiw
=
NonCorrectedSim3
［
pKFi
］；
vector
<
MapPoint
*>
vpMPsi
=
pKFi
->
GetMapPointMatches
（）；
for
（
size_t
iMP
=
0
，
endMPi
=
vpMPsi
。
size
（）；
iMP
<
endMPi
；
iMP
++
）
{
MapPoint
*
pMPi
=
vpMPsi
［
iMP
］；
if
（
！
pMPi
）
continue
；
if
（
pMPi
->
isBad
（））
continue
；
if
（
pMPi
->
mnCorrectedByKF
==
mpCurrentKF
->
mnId
）
continue
；
// Project with non-corrected pose and project back with corrected pose
cv
：：
Mat
P3Dw
=
pMPi
->
GetWorldPos
（）；
Eigen
：：
Matrix
<
double
，
3
，
1
>
eigP3Dw
=
Converter
：：
toVector3d
（
P3Dw
）；
Eigen
：：
Matrix
<
double
，
3
，
1
>
eigCorrectedP3Dw
=
g2oCorrectedSwi
。
map
（
g2oSiw
。
map
（
eigP3Dw
））；
cv
：：
Mat
cvCorrectedP3Dw
=
Converter
：：
toCvMat
（
eigCorrectedP3Dw
）；
pMPi
->
SetWorldPos
（
cvCorrectedP3Dw
）；
pMPi
->
mnCorrectedByKF
=
mpCurrentKF
->
mnId
；
pMPi
->
mnCorrectedReference
=
pKFi
->
mnId
；
pMPi
->
UpdateNormalAndDepth
（）；
}
// Update keyframe pose with corrected Sim3。 First transform Sim3 to SE3 （scale translation）
Eigen
：：
Matrix3d
eigR
=
g2oCorrectedSiw
。
rotation
（）。
toRotationMatrix
（）；
Eigen
：：
Vector3d
eigt
=
g2oCorrectedSiw
。
translation
（）；
double
s
=
g2oCorrectedSiw
。
scale
（）；
eigt
*=
（
1。
/
s
）；
//［R t/s；0 1］
cv
：：
Mat
correctedTiw
=
Converter
：：
toCvSE3
（
eigR
，
eigt
）；
pKFi
->
SetPose
（
correctedTiw
）；
// Make sure connections are updated
pKFi
->
UpdateConnections
（）；
}
// Start Loop Fusion
// Update matched map points and replace if duplicated
// 檢查當前幀的MapPoints與閉環匹配幀的MapPoints是否存在衝突，對沖突的MapPoints進行替換或填補
for
（
size_t
i
=
0
；
i
<
mvpCurrentMatchedPoints
。
size
（）；
i
++
）
{
if
（
mvpCurrentMatchedPoints
［
i
］）
{
MapPoint
*
pLoopMP
=
mvpCurrentMatchedPoints
［
i
］；
MapPoint
*
pCurMP
=
mpCurrentKF
->
GetMapPoint
（
i
）；
if
（
pCurMP
）
// 如果有重複的MapPoint，則用匹配幀的代替現有的
pCurMP
->
Replace
（
pLoopMP
）；
else
// 如果當前幀沒有該MapPoint，則直接新增
{
mpCurrentKF
->
AddMapPoint
（
pLoopMP
，
i
）；
pLoopMP
->
AddObservation
（
mpCurrentKF
，
i
）；
pLoopMP
->
ComputeDistinctiveDescriptors
（）；
}
}
}
}
// Project MapPoints observed in the neighborhood of the loop keyframe
// into the current keyframe and neighbors using corrected poses。
// Fuse duplications。
// 透過將閉環時相連關鍵幀的mvpLoopMapPoints投影到這些關鍵幀中，進行MapPoints檢查與替換
SearchAndFuse
（
CorrectedSim3
）；
// After the MapPoint fusion， new links in the covisibility graph will appear attaching both sides of the loop
// 更新當前關鍵幀之間的共視相連關係，得到因閉環時MapPoints融合而新得到的連線關係
map
<
KeyFrame
*
，
set
<
KeyFrame
*>
>
LoopConnections
；
// 遍歷當前幀相連關鍵幀
for
（
vector
<
KeyFrame
*>：：
iterator
vit
=
mvpCurrentConnectedKFs
。
begin
（），
vend
=
mvpCurrentConnectedKFs
。
end
（）；
vit
！=
vend
；
vit
++
）
{
KeyFrame
*
pKFi
=
*
vit
；
// 得到與當前幀相連關鍵幀的相連關鍵幀（二級相連）
vector
<
KeyFrame
*>
vpPreviousNeighbors
=
pKFi
->
GetVectorCovisibleKeyFrames
（）；
// Update connections。 Detect new links。
// 更新一級相連關鍵幀的連線關係
pKFi
->
UpdateConnections
（）；
// 取出該幀更新後的連線關係
LoopConnections
［
pKFi
］
=
pKFi
->
GetConnectedKeyFrames
（）；
// 從連線關係中去除閉環之前的二級連線關係，剩下的是由閉環得到的連線關係
for
（
vector
<
KeyFrame
*>：：
iterator
vit_prev
=
vpPreviousNeighbors
。
begin
（），
vend_prev
=
vpPreviousNeighbors
。
end
（）；
vit_prev
！=
vend_prev
；
vit_prev
++
）
{
LoopConnections
［
pKFi
］。
erase
（
*
vit_prev
）；
}
// 從連線關係中去除閉環之前的一級連線關係，剩下的是由閉環得到的連線關係
for
（
vector
<
KeyFrame
*>：：
iterator
vit2
=
mvpCurrentConnectedKFs
。
begin
（），
vend2
=
mvpCurrentConnectedKFs
。
end
（）；
vit2
！=
vend2
；
vit2
++
）
{
LoopConnections
［
pKFi
］。
erase
（
*
vit2
）；
}
}
// Optimize graph
// 進行EssentialGraph最佳化，LoopConnections是形成閉環後新生成的連線關係
Optimizer
：：
OptimizeEssentialGraph
（
mpMap
，
mpMatchedKF
，
mpCurrentKF
，
NonCorrectedSim3
，
CorrectedSim3
，
LoopConnections
，
mbFixScale
）；
mpMap
->
InformNewBigChange
（）；
// Add loop edge
// 添加當前幀與閉環匹配幀之間的邊
mpMatchedKF
->
AddLoopEdge
（
mpCurrentKF
）；
mpCurrentKF
->
AddLoopEdge
（
mpMatchedKF
）；
// Launch a new thread to perform Global Bundle Adjustment
// 新建一個執行緒用於全域性BA最佳化
mbRunningGBA
=
true
；
mbFinishedGBA
=
false
；
mbStopGBA
=
false
；
mpThreadGBA
=
new
thread
（
&
LoopClosing
：：
RunGlobalBundleAdjustment
，
this
，
mpCurrentKF
->
mnId
）；
// Loop closed。 Release Local Mapping。
mpLocalMapper
->
Release
（）；
mLastLoopKFid
=
mpCurrentKF
->
mnId
；
}
