ORBSLAM2学习笔记（4）——闭环

ORBSLAM2学习笔记（4）——闭环

闭环

本章我们分析第三个线程LoopClosing,从入口Run函数开始分析

闭环检测

在检测到新的关键帧插入后,便进入DetectLoop函数检测闭环.步骤如下:
1.若距离上次闭环小于10帧,则直接退出。
2.遍历共视帧，计算通过bow匹配的得分，找出其中的最低分。这一步是因为闭环检测到的帧的匹配程度起码要和共视帧的匹配程度相当。之后进入DetectLoopCandidates，对所有关键帧进行闭环。
3.找出与当前帧相连的帧。这是因为这些帧之后将会被剔除闭环检测的检测对象外。遍历该帧的所有单词，提取出拥有这些单词的所有帧，存放在候选帧列表lKFsSharingWords中。
4.遍历候选帧列表，将其中共有单词数较多且匹配分数高于与共视帧的匹配分数的存放在列表lScoreAndMatch中。
5.将列表中每一帧与自己共视程度较高的10帧组成一组，选出得分最高的组,记录下该得分。将得分大于0.75最高分的组里的得分最高的帧都加入vpLoopCandidates中返回，成为备选帧的结果。
6.之后，还要进行连续性检测。也就是说，我们认为单个帧出现闭环不够稳定，必须是连续几帧都检测到闭环，才认为是真的闭环。

闭环匹配

在ComputeSim3函数中，通过三步实现闭环帧和当前帧的匹配。
1.利用Bow进行粗匹配：从闭环候选组中挑出一帧，调用SearchByBoW进行bow加速匹配，获得匹配特征点组。对上述帧进行5次sim3变换迭代求解，如果求解成功，将inlier的地图点保存在vpMapPointMatches中。
2…利用粗匹配结果获取更多匹配点，再次匹配：通过粗匹配获得了sim3,先进行一次sim3变化，再利用变化的结果调用SearchBySim3寻找之前匹配漏掉的匹配特征点。之后，调用一次优化器进行优化。
3.将匹配上的关键帧以及相连关键帧的地图点投影到当前帧进行第三次匹配：为了获得更准确的结果，我们需要获得更多的匹配。方法是遍历闭环帧及其相连帧的地图点，将地图点投影到当前帧上，再次在小范围内匹配特征点.

闭环矫正

1.首先,提取与当前帧相连的关键帧,计算这些帧与当前帧的相对位姿,在通过已经经过sim3匹配的当前位姿乘以相对位姿得出相连关键帧经过sim3匹配的位姿.同时,保存下未经过矫正的位姿,以便下一步地图点矫正使用.
2. 遍历这些关键帧的地图点,将它们从世界坐标系映射到相机坐标系,经过sim3矫正后再映射回世界坐标系,就完成了地图点位姿的矫正.
3. 步骤1只是获取了关键帧经过sim3匹配的位姿,并没有实际更新,因此这里通过setpose函数更新关键帧的位姿,之后更新它在共视图中的链接.
4. 将当前帧和闭环帧的地图点进行融合,将当前帧与相连关键帧的地图点进行融合,从而得到更多的地图点匹配,之后进行优化.
5. 之后,需要得到因为闭环的引入而新生成的帧之间的链接关系.方法是更新链接关系,去除闭环之前的一级链接关系和二级链接关系即可.
6. 对变换生成的新链接关系进行一次OptimizeEssentialGraph,这次优化只优化了参与到这次闭环中的帧和关键点.最后进行一次RunGlobalBundleAdjustment,来对所有的地图点和关键帧进行优化.