标签：database 不能 map center poi 连续 迭代 结构 init
转载请注明出处，谢谢
原创作者：Mingrui
原创链接：https://www.cnblogs.com/MingruiYu/p/12369339.html
本文要点：
之前的 ORB-SLAM2 系列文章中，我们已经对 Tracking 线程和 LocalMapping 进行了介绍。我们将在本文中，对 ORB-SLAM2 系统的 LoopClosing 线程进行介绍。
依旧祭出该图，方便查看：
也再次献上我绘制的程序导图全图：ORB-SLAM2 程序导图
老规矩，还是分两部分：以 ORB-SLAM 论文为参考 和 以 ORB-SLAM2 代码（程序导图）为参考。
LoopClosing 线程的大致步骤如下：
这些步骤可以归为两类：
下面我们对每一个步骤进行详细的介绍。
在之前的 LocalMapping 线程中，会对 KFs 进行筛选，最终在 Map 中保留必要的 KFs。经过筛选后的 KFs，会送入 LoopClosing 线程，每一个新送入的 Current KF，都会检测在 KF Database 中，有没有以前路过的 KF，与 Current KF 相匹配，从而可以进行回环校正。
这一步的目的是初步筛选出一批 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 形式相连。
(以上部分论文中说的比较含糊，看代码会相对清晰，下文会详述)
在回环校正的时候，我们需要对 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 确定了一个 Loop KF，Loop Detection 部分就完成了，之后就进入 Loop Correction。
回环融合分以下几步：
上述回环校正只对 Current KF 附近的 KFs 进行了校正，但是累计误差是慢慢积累的，一路上每个 KF 都应该接受校正。
ORB-SLAM 采用了对 Essential Graph 进行位姿图优化的方式。该优化是 Sim3 优化，以便校正尺度漂移。
请点击 ORB-SLAM2 程序导图链接（文首）查看清晰全图
与 LocalMapping 线程一样，LoopClosing 线程干的第一件事，也是检查新 KF 队列中有没有未处理的 KF，有的话，取出队首元素作为 Current KF。
LoopClosing::DetectLoop()，首先，之前刚进行完回环检测的10帧 KFs 内，不进行回环检测，且这10帧 KFs 不会添加进 KF Database。
首先，计算 \(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 相连，则通过筛选，进入下一环节。
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。
Optimizer::OptimizeEssentialGraph()
注意，这之后的 Loop KF -> AddLoopEdge(Current KF) 和 Current KF -> AddLoopEdge(Loop KF) 只是记录回环边。Convisibility Graph 在之前已经更新过了。
上述步骤全部进行完了之后，精度已经很高了，但 ORB-SLAM2 还是选择在最后再进行一次 Global BA 锦上添花（ORB-SLAM1 中似乎没有这步）。注意，为了不影响主要3个线程的工作，这里创建了第4个线程，专门进行 Global BA。但该 Global BA 随时可能被打断，只有在系统特别闲的时候才会运行。
ORB-SLAM2 论文&代码学习 —— LoopClosing 线程
标签：database 不能 map center poi 连续 迭代 结构 init
原文地址：https://www.cnblogs.com/MingruiYu/p/12369339.html
