ORB-SLAM2代码整理--LoopClosing线程
找到线程的Run函数
void LoopClosing::Run()
{
mbFinished =false;
while(1)
{
// Check if there are keyframes in the queue
// Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
// 在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
// 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
ResetIfRequested();
if(CheckFinish())
break;
//usleep(5000);
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
SetFinish();
}
只要闭环检测关键帧队列不为空下面的检测会一直执行
1 DetectLoop() 进行闭环检测
1.1 从队列中取出一个关键帧
1.2 判断:如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
1.3 遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
1.3.1 找出所有与当前关键帧相连的KeyFrame，这些相连Keyframe都是局部相连，在闭环检测的时候将被剔除
1.3.2 步骤1:找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）
1.3.3 遍历当前关键帧的每一个word
1.3.4 提取包含该word的关键帧pKFi,但不包括与当前关键帧相邻的关键帧
1.3.5 步骤2:统计所有闭环候选帧中与pKF具有共同单词最多的单词数
1.3.6 步骤3：遍历所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
1.3.7 步骤4: 单单计算当前帧和某一关键帧的相似性是不够的，这里将与对于上文筛选出来的pKFi相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
即:lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMat
1.3.8 步骤5：得到组得分大于minScoreToRetain的组，得到组中分数最高的关键帧
1.4 在候选帧中检测具有连续性的候选帧
// 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs-->spCandidateGroup
// 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前 连续组vCurrentConsistentGroups”
// 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进mvpEnoughConsistentCandidates1.5 将上一步中得到的候选闭环帧以及与自己相连的关键帧构成一个"子候选组"
1.6 遍历之前的"子连续组".....(子连续组是Covisibility图的连续性地图)
1.7 遍历每个“子候选组”，检测候选组中每一个关键帧在“子连续组”中是否存在
// 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续1.8 将该“子候选组”的该关键帧打上编号加入到“当前连续组”
1.9 如果该“子候选组”的所有关键帧都不存在于“子连续组”，那么vCurrentConsistentGroups将为空，
// 于是就把“子候选组”全部拷贝到vCurrentConsistentGroups，并最终用于更新mvConsistentGroups，计数器设为0，重新开始1.10 更新Covisibility连续地图
2 ComputeSim3()
2.1 步骤1: 从筛选的闭环候选帧中取出一帧关键帧pKF
2.2 步骤2：将当前帧mpCurrentKF与闭环候选关键帧pKF匹配
SearchByBoW (见Tracking线程的4.3)
2.3 构造Sim3求解器
2.4 一直循环所有的候选帧，每个候选帧迭代5次，如果5次迭代后得不到结果，就换下一个候选帧
// 直到有一个候选帧首次迭代成功bMatch为true，或者某个候选帧总的迭代次数超过限制，直接将它剔除 RANSAC：利用上面匹配上的地图点（虽然匹配上了，但是空间位置相差了一个Sim3），用RANSAC方法求解Sim3位姿
//这里有可能求解不出Sim3,也就是虽然匹配满足，但是空间几何姿态不满足vvpMapPointMatche
2.5 步骤3：对步骤2中有较好的匹配的关键帧求取Sim3变换
2.6 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
2.7 经过n次循环，每次迭代5次，总共迭代 n*5 次
// 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除2.8 如果RANSAC成功,则保存优化后的mappoint vpMapPointMatches
2.9 通过步骤3求取的Sim3变换根据计算出的Sim3(s, R, t)，去找更多的匹配点(SearchBySim3),更新vpMapPointMatches,弥补步骤2中的漏匹配
SearchBySim3
// 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12（之前使用SearchByBoW进行特征点匹配时会有漏匹配）2.9.1 获得当前关键帧的内参 R t 和 候选闭环帧的R t 和尺度s
2.9.2 vbAlreadyMatched1 用于记录该特征点是否被处理过
vbAlreadyMatched2 用于记录该特征点是否在pKF1中有匹配
2.9.3 用vpMatches12更新vbAlreadyMatched1和vbAlreadyMatched2
// vpMatch12是在SearchByBoW中的pKF2中与pKF1匹配的MapPoint
2.9.4 步骤3.1：通过Sim变换，确定pKF1的特征点在pKF2中的大致区域，在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12 （之前使用SearchByBoW进行特征点匹配时会有漏匹配）
2.9.5 把pKF1系下的MapPoint从world坐标系变换到camera1坐标系
再通过Sim3将该MapPoint从camera1变换到camera2坐标系
得到像素坐标
2.9.6 预测该MapPoint对应的特征点在图像金字塔哪一层
根据金字塔层数确定特征点的搜索半径
取出该区域内的所有特征点
2.9.7 遍历搜索区域内的所有特征点，与pMP进行描述子匹配 成功的放入 vnMatch1
2.9.8 步骤3.2：通过Sim变换，确定pKF2的特征点在pKF1中的大致区域，
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
// （之前使用SearchByBoW进行特征点匹配时会有漏匹配）与步骤3.1 相反
2.9.9 返回找到多少匹配点
2.10 步骤5：Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
2.11 优化成功后停止RANSAC
mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
得到从世界坐标系到该候选帧的Sim3变换，Scale=1
得到g2o优化后从世界坐标系到当前帧的Sim3变换
2.12 没有一个闭环匹配候选帧通过Sim3的求解与优化,清空mvpEnoughConsistentCandidates
2.13 步骤6：取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入mvpLoopMapPoints
注意是匹配上的那个关键帧：mpMatchedKF
// 将mpMatchedKF相连的关键帧全部取出来放入vpLoopConnectedKFs
// 将vpLoopConnectedKFs的MapPoints取出来放入mvpLoopMapPoints2.14 步骤7：将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
// 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数） // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域， // 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新 mvpCurrentMatchedPoints // mvpCurrentMatchedPoints将用于SearchAndFuse中检测当前帧MapPoints与匹配的MapPoints是否存在冲突
2.14.1 获得当前关键帧的内参
2.14.2 计算得到尺度s
2.14.3 使用set类型，并去除没有匹配的点，用于快速检索某个MapPoint是否有匹配
2.14.4 遍历所有的MapPoints
2.14.5 判断距离在范围内,判断视角是否在范围内
2.14.6 根据尺度确定搜索半径
2.14.7 遍历搜索区域内所有特征点，与该MapPoint的描述子进行匹配
2.14.8 该MapPoint与bestIdx对应的特征点匹配成功
2.15 步骤8：判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
2.16 清空mvpEnoughConsistentCandidates
3 CorrectLoop()
3.1 局部地图线程停止
3.2 根据共视关系更新当前帧与其它关键帧之间的连接
3.3 步骤2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
// 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，
// 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换
3.4 取出与当前帧相连的关键帧，包括当前关键帧
3.5 将当前帧的sim3变换存入
3.6 通过位姿传播，得到Sim3调整后其它与当前帧相连关键帧的位姿
//当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
// 得到闭环g2o优化后各个关键帧的位姿
//得到当前帧相连关键帧，没有进行闭环g2o优化的位姿
3.7 步骤2.2：步骤2.1得到调整相连帧位姿后，修正这些关键帧的MapPoints
3.8 将Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
3.9 根据共视关系更新当前帧与其它关键帧之间的连接
3.10 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
3.11 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
3.12 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
3.13 遍历当前帧相连关键帧（一级相连）
3.14 得到与当前帧相连关键帧的相连关键帧
3.15 更新一级相连关键帧的连接关系 UpdateConnections()
3.16 取出该帧更新后的连接关系
3.17 从连接关系中去除闭环之前的二级连接关系和一级链接关系，剩下的连接就是由闭环得到的连接关系(闭环链接关系哪里添加进去的.待查明)
3.18 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括下面的步骤中当前帧与闭环匹配帧之间的连接关系
3.19 添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
3.20 新建一个线程用于全局BA优化
//OptimizeEssentialGraph只是优化了一些主要关键帧的位姿，这里进行全局BA可以全局优化所有位姿和MapPoints
上一篇： Java方法调用练习题
下一篇： 从零开始Swift之控制流