对Orb-slam2——LoopingClosing线程的理解

前言

对orb-slam2的LoopClosing线程的一些理解

回环检测

检测是否闭环

因为检测到的闭环对于优化过程是一个非常强烈的约束，所以对于闭环的判定标准也是非常严格，这个标准是：
（1）连续三个关键帧在历史场景中出现过
（2）这连续的三帧在历史场景中也具有一定的连续性
对于第一项我们可以理解，因为只有一帧可能具有误差嘛，而对于第二项来说，如果这三帧没有一定程度的连续性，那么也有可能是因为无人车发生了较为严重的轨迹偏移而导致的这三帧按照顺序输入缓冲队列，所以我们需要用历史场景中的三帧连续性来进行约束。

对于第一项，我们可以直接使用词袋模型来进行匹配闭环候选帧，而对于第二项，在源代码中引入了一个叫闭环候选关键帧组的变量，每一个关键帧组都是由闭环候选帧及其共识关键帧组成（关键帧组存在有多个的可能）。然后源代码中也有一个前一关键帧的闭环候选关键帧组这一变量，对于这两个关键帧组，如果共同包含某一关键帧的话，那么我们认为这两个关键帧组是连续的，ex : 前一关键帧的关键帧组的连续长度为1，当前关键帧的关键帧组如果和该关键帧组有“交集”，那么当前关键帧的关键帧组的连续长度为2。那么当当前关键帧的闭环候选关键帧组的连续长度到达3，则认为发生闭环

计算sim3变换

ORB-SLAM2系统在LoopClosing线程中，当检测到闭环的时候，就需要对当前关键帧和对应的闭环候选帧之间计算其变换关系。这时需要用当前关键帧和其对应的闭环候选帧进行sim3求解，这里的sim3求解是对当前关键帧和闭环候选帧之间匹配的MapPoint进行sim3求解。通过sim3变换解出当前关键帧和闭环候选帧的匹配MapPoint之间的旋转矩阵R、平移向量t、尺度变换s，也就得到了当前关键帧到闭环关键帧之间的sim3变换gScm。使用这个sim3变换gScm乘上闭环关键帧的sim3位姿gSmw，mg2oScw=gScm*gSmw的乘积mg2oScw就是当前关键帧的sim3位姿，之后在闭环校正中就可以使用这个sim3位姿转换为SE3位姿后对当前关键帧进行位姿校正（当然也要对关键帧对应的MapPoints以及其共视的关键帧进行校正）

对于单目相机获得的帧，在系统中运算时间足够长的情况下，可能会出现基线丢失，因此尺度会发生变化，所以我们要进行相似变换，而对于双目、RGB-D相机，可获得深度，因此不存在尺度问题，可以进行（欧式变换）刚体变换，因此Sim3中的尺度s=1。

具体流程如下：
（1）通过词袋加速算法实现当前帧、闭环帧的特征点的匹配，建立闭环帧的路标点和当前帧的特征点间的联系。
（2）使用RANSAC法，随机采取3对点（根据特征点的索引，获得当前帧中的路标点（局部建图时获得）及对应的闭环帧中的路标点（步骤（1）中获得）），计算两帧间的Sim3变换。一共迭代5次，如果有一次，获得的内点数大于20，就认为成功。
（3）根据Sim3变换，将闭环帧的路标点投影至当前帧中进行匹配。随后优化Sim3矩阵。
（4）将闭环帧及其共视帧的路标点投影至当前帧中，如果匹配上的路标点大于40，就认为该闭环帧可靠。
（5）随后就可以进行闭环校正，根据当前帧和闭环帧的Sim3矩阵，去校正当前帧及其共视帧的位姿、路标点坐标。更新共视关系，更新本质图。