ORB-SLAM2 论文&代码学习 —— LoopClosing 线程
转载请注明出处，谢谢
原创作者：Mingrui
原创链接：https://www.cnblogs.com/MingruiYu/p/12369339.html
本文要点：
- ORB-SLAM2 LoopClosing 线程 论文内容介绍
- ORB-SLAM2 LoopClosing 线程 代码结构介绍
写在前面
之前的 ORB-SLAM2 系列文章中，我们已经对 Tracking 线程和 LocalMapping 进行了介绍。我们将在本文中，对 ORB-SLAM2 系统的 LoopClosing 线程进行介绍。
依旧祭出该图，方便查看：
也再次献上我绘制的程序导图全图：ORB-SLAM2 程序导图
老规矩，还是分两部分：以 ORB-SLAM 论文为参考 和 以 ORB-SLAM2 代码（程序导图）为参考。
以 ORB-SLAM 论文为参考
LoopClosing 线程的大致步骤如下：
- 接收 LoopClosing 送来的筛选处理后的 KF
- 检测出一批 Candidate KFs
- 计算 Sim3，确定最终的 Loop KF
- 进行回环融合
- 优化 Essential Graph
这些步骤可以归为两类：
- 回环检测 Loop Dectection
- 回环校正 Loop Correction
下面我们对每一个步骤进行详细的介绍。
接收 LoopClosing 送来的筛选处理后的 KF
在之前的 LocalMapping 线程中，会对 KFs 进行筛选，最终在 Map 中保留必要的 KFs。经过筛选后的 KFs，会送入 LoopClosing 线程，每一个新送入的 Current KF，都会检测在 KF Database 中，有没有以前路过的 KF，与 Current KF 相匹配，从而可以进行回环校正。
检测出一批 Candidate KFs
这一步的目的是初步筛选出一批 Candidate KFs。
首先，会依据 KF 的 BoW 计算 Current KF 与其每一个 Covisible KF 之间的相似分数，取最低分作为 \(s_{min}\)，Candidate KFs 与 Current KF 之间的相似分数至少要大于 \(s_{min}\)（动态计算选择 Candidate KFs 的阈值）。
另外，Current KF 的 Covisible KFs 是不能成为 Candidate KFs 的。
再之后，还要进行连续性检测,进一步筛选 Candidate KFs：该 Candidate KF 需要在 Covisibility Graph 中与三个以上 Candidate KFs 以 Group 形式相连。
(以上部分论文中说的比较含糊，看代码会相对清晰，下文会详述)
计算 Sim3，确定最终的 Loop KF
在回环校正的时候，我们需要对 Current KF 和 Loop KF 之间的相对误差进行描述。但是注意，在单目 SLAM 中，是有7个自由度的：6 + 尺度。也就是说，对于单目 SLAM，在运行过程中的累计漂移，除了平移漂移，旋转漂移之外，还会有尺度漂移。可能跟着跟着轨迹和 Map 的尺度都整体缩小了。如果仅计算 SE3 的话，没法很好的对回环进行校正，所以提出了 Sim3（相似变换群）：
\(\operatorname{sim}(3)=\left\{\left[\begin{array}{cc} {S=} & {s \boldsymbol{R}} & {t} \\ {\mathbf{0}^{T}} & {1} \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}\)
其中 \(S\) 就是相似变换，其在欧式变换的基础上加上了一个尺度因子 \(s\)。
ORB-SLAM 会计算 Current KF 与 Candidiate KF 之间的相似变换，并据此确定最终的 Loop KF，其步骤如下：
- 对 Current KF 与 Candidate KF 的 FeaturePoints 进行 BoW 匹配（要求是与 MapPoints 链接的 FeaturePoints），从而的到 MapPoints 与 MapPoints 之间的匹配（3D - 3D）。
- RANSAC 迭代计算每一个 Candidate KF 与 Current KF 之间的相似变换。
- 当找到一个有足够多 inliers 的相似变换后，进行优化；根据优化得到的相似变换，可以找到更多的 Current KF 与 该 Candidate KF 的 3D - 3D 匹配。
- 再进行优化，如果又有足够多 inliers，则认为该相似变换满足要求，也就找到一个满足要求的 Loop KF。
至此，就为 Current KF 确定了一个 Loop KF，Loop Detection 部分就完成了，之后就进入 Loop Correction。
回环融合
回环融合分以下几步：
- 校正 Current KF 及其 Convisible KFs 的 SE3 位姿
- 根据 Loop KF 的位姿 和 Loop KF 与 Current KF 之间的相似变换，对 Current KF 的位姿进行校正。
- 同时，也对 Current KF 的 Convisible KFs 的位姿进行校正（在 Current KF 的校正位姿基础上，叠加 Current KF 与 Convisible KFs 之间的相对位姿）。
- 将 Loop KF 及其 Convisible KFs 与 Current KF 及其 Convisible KFs 进行 MapPoints 融合
- 将 Loop KF 及其 Convisible KFs 所含的 MapPoints 与 Current KF 及其 Convisible KFs 的 FeaturePoints 进行匹配（通过投影找匹配），匹配上了就进行 MapPoints 融合（此处的融合和 LocalMapping 中的 MapPoinits 融合采取同样方式）。
- 更新 Convisibility Graph
- 相关 KFs 更新其 Convisible KFs
- 添加回环边
优化 Essential Graph
上述回环校正只对 Current KF 附近的 KFs 进行了校正，但是累计误差是慢慢积累的，一路上每个 KF 都应该接受校正。
ORB-SLAM 采用了对 Essential Graph 进行位姿图优化的方式。该优化是 Sim3 优化，以便校正尺度漂移。
以 ORB-SLAM2 代码（程序导图）为参考
请点击 ORB-SLAM2 程序导图链接（文首）查看清晰全图
与 LocalMapping 线程一样，LoopClosing 线程干的第一件事，也是检查新 KF 队列中有没有未处理的 KF，有的话，取出队首元素作为 Current KF。
回环检测 —— 检测 Candidate KFs
LoopClosing::DetectLoop()，首先，之前刚进行完回环检测的10帧 KFs 内，不进行回环检测，且这10帧 KFs 不会添加进 KF Database。
BoW 检索
首先，计算 \(s_{min}\)。
之后，KeyFrameDatabase::DetectLoopCandidates() 筛选，除了论文中提到的根据 \(s_{min}\) 筛选，还要求 Candidate KF 与 Current KF 的共同 word 数 > 0.8 * 任意 KF 与 Current KF 的最多共同 word 数（但不包括 Current KF 的 Covisible KFs）。
之后，还要再筛选一次：对上述所有符合要求的 KFs，还要将它和其所有 Covisible KFs 组成一组，计算该组的相似分数之和，并且找出该组内相似分数最高的 KF。将所有 (总分数 > 0.75 * 最高组分数) 的组的最高分 KF 选入 Candidate KFs，送入下一步（这一步的意义是，如果单蹦出一 KF 相似分数很高，但它附近的 KFs 的相似分数都很低，那么可能这 KF 分高是不可靠的，需要筛掉）。
连续性检测
进一步筛选 Candidate KFs：进行连续性检测，该 Candidate KF 需要在 Covisibility Graph 中与三个以上 Candidate KFs 以 Group 形式相连。
什么是以 Group 形式相连呢？假设 KF1 及其 Covisible KFs 组成 Group1，KF2 及其 Covisible KFs 组成 Group2；如果 Group1 与 Group2 含有相同的 KF，则两个 Group 相连。
所以如果一个 Candidate KF 的 Group 与 另外 3 个 Candidate KFs 的 Groups 相连，则通过筛选，进入下一环节。
回环检测 —— 计算 Sim3，确定最终的 Loop KF
LoopClosing::ComputeSim3()，上一步中，会得到好几个 Candidate KF，这一步的目的之一是的到最终的 Loop KF，另外，还要计算的到 Current KF 与 Loop KF 之间的相似变换。
注： 从上述回环检测的步骤可以看出，ORB-SLAM2 在检测回环时，分层地进行了大量的筛选步骤。这样，一是为了保证 Loop KF 的准确性，二是避免出现大量相似回环：一般来说，因为场景是连续的，所以出现回环的地方，一般会出现一连串差不多的回环，这些筛选的步骤避免了添加重复的回环，也是减轻 LoopClosing 线程的负担。
回环校正
LoopClosing::CorrectLoop()，首先让通知 LocalMapping 暂停，防止插入新的 KF，并等待 LocalMapping 线程停止。期间还要停止正在进行的 Global BA（如果有的话）。
回环融合
与论文中描述的一致。
注意其中 MapPoints 的融合：将 Loop KF 所含的 MapPoints 与 Current KF 的 FeaturePoints 进行匹配，匹配上的话就将该 MapPoint 与 那个 FeaturePoint 链接上，但如果 Current KF 的该 FeaturePoint 已经链接上了某个 MapPoint，则用 Loop KF 的 MapPoint 替换掉原来链接的 MapPoint。
之后，再将 Loop KF 的 Covisible KFs 与 Current KF 的 Covisible KFs 两部分的 MapPoints 做匹配，使用 ORBmatcher::Fuse()。
最后，更新这局部的 Convisibility Graph。
优化 Essential Graph
Optimizer::OptimizeEssentialGraph()
注意，这之后的 Loop KF -> AddLoopEdge(Current KF) 和 Current KF -> AddLoopEdge(Loop KF) 只是记录回环边。Convisibility Graph 在之前已经更新过了。
Global BA
上述步骤全部进行完了之后，精度已经很高了，但 ORB-SLAM2 还是选择在最后再进行一次 Global BA 锦上添花（ORB-SLAM1 中似乎没有这步）。注意，为了不影响主要3个线程的工作，这里创建了第4个线程，专门进行 Global BA。但该 Global BA 随时可能被打断，只有在系统特别闲的时候才会运行。
