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
;
}