ORB-SLAM: a Versatile and Accurate Monocular SLAM system---阅读笔记(four)

VII LOOP Closing回环检测

主要目的是消除局部地图构建时不断累计的误差，解决尺度漂移问题。

回环闭环线程接收局部地图处理的最后一个关键帧 $K_i$，并尝试检测和闭环。步骤如下：

A Loop Candidates Detection 回环候选检测

首先，计算关键帧 $K_i$ 的词袋向量与共视图（${\theta }_{min}=30$）中的所有相邻图像的相似度，得到最小相似度 $s_{min}$。然后查询识别数据库，丢弃所有得分比 $s_{min}$ 低的关键帧。这使用了在DBoW2中通过先前图像计算得到的归一化分数相似的操作去获得鲁棒性，但是这里使用的是共视信息。另外，所有和 $K_i$ 直接相关的关键帧都被从结果中抛弃。必须连续检测三个一致的候选回环帧（在共视图中相关的关键帧）才被认为回环帧。如果存在几个地方和 $K_i$ 相似，则可能有几个回环帧。

计算当前关键帧的词袋向量与共视图中相邻图像的相似度，得到最小相似度Smin；在关键帧数据库中找到相似度不小于最小相似度的关键帧作为回环候选关键帧，通过连续三次的关键帧对应相同的候选回环帧，则认为该帧为回环帧；

B Compute the Similarity Transformation 计算相似变换

在单目SLAM系统中，地图有7个自由度可以漂移，三个平移、三个旋转和一个比例因子。因此，为了闭环，我们需要计算从当前关键帧 $K_i$ 到闭环关键帧 $K_l$ 的相似变换，获得回环的累计误差。这个相似计算也被作为回环的几何验证。
按 III-E 部分所示，首先，计算在当前关键帧和回环候选关键帧中和地图点云相关的ORB之间的匹配点。此时，对每个回环候选，有3D-3D对应关系。我们交替地对每个候选关键帧执行RANSAC迭代，试图使用Horn方法找到相似变换。如果我们找到一个具有足够内部元素的相似度 $S_{il}$，我们将对其进行优化（参见附录），并执行更多对应关系的引导搜索。如果相似度 $S_{il}$被更多内部元素支持，再次优化它，闭环关键帧 $K_l$被接收。

当前关键帧到回环关键帧的相似变换，以获得回环的累积误差，也可以作为回环的几何验证。

C Loop Fusion环路闭合

环路校正的第一步是融合重复的地图点，并在共视图中插入新的边，这些边将连接回环闭合。首先，利用相似变换 $S_{il}$ 纠正当前关键帧的位姿 $T_{iw}$ ，这个纠正被应用到关键帧 $K_i$ 的所有临帧中，级联转换，以便回环两头对齐。如在 V-D中所示，所有被回环关键帧观察到的地图点云和其邻近点被映射到 关键帧集 $K_i$ 中，并且在地图周围的小片区域内搜索其临近点和匹配项。将所有匹配的地图点云和 相似变换$S_{il}$ 计算中的内部点进行融合。所有参与融合的关键帧在共视图中更新其边，从而有效的创建和闭环有关的边。

在共视图中插入与回环相关的新边，通过相似变换矫正当前关键帧位姿，然后将回环关键帧及其近邻能观测到的所有地图点云都映射到当前关键帧及其近邻中，并在映射的区域附近小范围内追踪它的对应匹配点，对所有匹配的地图点云和计算Sim3过程中的有效数据进行融合；

D Essential Gragh Optimization基本图优化

为了有效的实现闭环，执行通过沿图方向分布回环误差的基本图的位姿图优化，在第 III-D 部分描述的。通过相似变换执行优化以校正尺度漂移。误差项和代价函数在附录中详细介绍。在优化之后，根据观察到的关键帧之一的校正来转换每个地图点云。

对基本图（Essential Graph）进行全局位姿图优化，将回环闭合的误差分散到图像中，优化过程通过相似变换Sim3校正尺度偏移。