ORBSLAM-Altas：多地图SLAM

这篇博客

 最近ORB-SLAM3横空出世，马上跑去GitHub膜拜。然后在项目的相关工作中看到了ORB-SLAM3使用了一个多地图方法。这个方法对应的论文介绍的就是ORBSLAM-Atlas系统的。为了之后更好地理解ORB-SLAM3和代码，抓紧跑去把这个多地图的论文给看一遍，再默默地记下笔记。
【转载声明】本篇文字均为原创，如转载请注明出处

ORBSLAM-Altas

这个系统

 系统摘要：
 ORBSLAM-Atlas系统是ORB-SLAM向多地图领域的一个扩展研究，它在机器跟踪丢失时不会直接停止地图更新（ORB-SLAM会停止更新地图，然后等着机器回来以后进行重定位），而是马上重新构建一个新的子地图，之前的地图先保存起来。
 如果当前在使用的地图与之前的保存的地图存在闭环，则将两个地图进行融合。之后使用融合后的地图继续进行跟踪操作。

系统方法

 图1是ORBSLAM-Atlas系统的结构框图。

 可以看出，系统框图和ORB-SLAM的框图十分相似。区别明显的地方有有：
1、系统中有两类子地图：Active map和Non-active map；
2、跟踪线程要判断是否需要构建新的地图以供跟踪；
3、检测的闭环可以是在同一个子地图中，也可以在不同子地图中。
 下面就针对上述内容进行介绍。

两类子地图

 ORBSLAM-Atlas系统会保留两类子地图：Active map和Non-active map。其中Active map是当前跟踪线程所使用的地图（Active Map >=Local Map），Non-active则是之前跟踪丢失后保留下来的所有地图。如果Active map与Non-active map存在闭环，那么就会将两个存在闭环的地图融合起来，然后系统使用融合后的地图作为新的Active map使用。
 每个子地图都是独立的，它们的第一参考帧也都是独立的。系统使用一个能够适应于所有子地图的DBoW2词袋来描述每一个子地图，以便完成地图融合操作。

 这种做法的好处有：
1、在跟踪丢失后系统通过创建新的地图来继续工作；
2、构建多个子地图可以降低误差的累积；
3、所有子地图融合后获得完整地图。这个融合后的地图相比于ORB-SLAM构建的一整个地图，在精度方面会有所提高（有部分原因是剔除了一些精度较低或是跟踪丢失的轨迹）。

新地图的构建

 ORBSLAM-Atlas系统的跟踪线程需要判断是否跟踪丢失，再决定是否构建新的地图。新地图构建和初始化的过程相同，最重要的是何时构建新地图。论文阐述了两个条件来判断跟踪是否丢失：

1、当前帧跟踪到的地图点数目：设$N$为当前帧跟踪到的局部地图中的地图点数量。如果$N$大于给定阈值，则认为跟踪良好，否则跟踪丢失。
2、相机位姿的可观测性：如果$N$中的地图点的几何条件较差，则认为当前帧的位姿估计值精度较低，认为跟踪丢失。

 第一条很好理解，第二条则是论文提出的一种新的判断丢失的标准（更加严格）。第二条可以通过下图来理解：

 从图中可以看到，当前帧跟踪到的地图点数量$N$很多，肯定能满足第一条标准。但是跟踪到的点大多距离相机很远（远在天边~）。太远的地图点会降低在平移运动上的估计值的精度（如：走一两步几乎不会影响远点在当前图像上的成像）。所以最后的估计的位姿精度较差，如果保留这些较差的估计值，那么就会直接影响到之后图优化的精度。
 所以系统将这些较差的位姿剔除，从这时开始构建一个新的子地图。这样就将那些误差较大的轨迹排除在外了。下图就是这种作法的一个例子。

 图中的P1是一部分精度较差的位姿估计值，P2、P3则是一个闭环。图b因为保留了P1，以至于影响到了最后闭环优化的精度。而下面的图则直接剔除了P1部分，保证了最终轨迹的一个估计精度。论文也给了“相机位姿可观测性”的具体判断公式：

相机位姿的可观测性

 通过相机位姿的误差协方差来估计其位姿的观测性。首先假设 Active map 中的地图点都是精确的，然后用${\Omega }_{i,j}$表示地图点观测$x_{i,j}$的不确定性，其中$x_{i,j}$表示的是地图点 $j$ 在第 $i$ 个相机上的观测值。
 定义${\hat{T}}_{i,w}ϵSE(3)$是当前相机的位姿估计值，用${\epsilon }_i$表示估计值的不确定性（${\epsilon }_i$是六维向量且服从均值为0的高斯分布）。然后有以下式子：

其中$C_i$表示估计位姿的精度，$J_{i,j}$是第 i 个相机的位姿估计值关于第 j 个地图点的雅可比矩阵。然后通过 公式 (1) 就能判断相机的可观测性了：

子地图融合

 如果在两个地图之间检测到了闭环，那么就要进行地图融合。然后用融合后的地图替换掉那两个地图。下面介绍具体的融合方法，以及融合后的优化操作。以下用 $a，s，m$ 下标分别标记：Active map，Non-active map和融合后地图(用 $a地图$表示Active map，$s地图$表示Non-active map，$m地图$表示融合后的地图)。

 1、检测到Active 和Non-active 两地图间的闭环，产生两个匹配的$KFs：Ka，Ks$，以及两个地图间匹配的地图点集合$Ma，Ms$。
 2、估计两个地图之间的对齐变换矩阵。对齐变换矩阵（在双目情况下属于$SE3$，单目则是$Sim3$）用来完成 $s地图$中地图和关键帧到 $a地图$坐标系的转换。具体分为两步：
  1）通过$Ma$和$Ms$这些匹配的地图点来进行初始估计，并用 RANSAC 算法提高估计值精度。
  2）再根据 $Ma$在 $Ks$ 上的匹配点，构建重投影误差。最后由非线性优化来优化重投影误
  差，以获得最终的位姿估计值。
 3、融合子地图：完成地图对齐后，将两个地图中重叠的地图点进行融合、更新，获得精度更高的地图点。两个子地图的可视图也保留在融合后的$m地图$中。
 4、在子地图的闭环部分进行局部 BA 优化。在融合后的$m地图$中，找到$Ka$和$Ks$。根据$a地图$原有的可视图，确定$a地图$中哪些$KFs$是固定的（不再 Local Map 中）。这些固定的$KFs$在$m地图$中仍然固定。然后对$m地图$中剩余的$KFs$进行 BA 优化。
 5、对 $m地图$ 进行完整的位姿图优化。

 如果是在Active map中检测到闭环，那么就和ORB-SLAM中的操作一样，只对Active map进行闭环操作。

系统线程

 论文还提到了各线程之间的一些关系。地图融合会在一个单独的线程中进行。在融合开始前，会将Local Mapping线程暂停，以防止新的$KF$产生。在地图融合的过程中，跟踪线程会继续使用 Active map 来达到实时工作的目的。当融合结束后，跟踪线程会将融合后的地图作为新的 Active map 来使用。

结尾

 至此，这篇博客的主要内容就结束了。ORBSLAM-Atlas系统主要提供一种多地图SLAM的思路。它能够提高系统的鲁棒性和精度。但看完论文后我产生了一下几个疑问：
1、这个系统会剔除掉那些由第二条丢失判断标准找出的$KFs$。这会使最后的地图出现一个缺口，降低地图的完整性。
2、如果有些子地图没有机会进行融合，那最后该怎么处理？是否会被直接抛弃？
 先把这些问题记在小本本上，等学的更加深入后，也许就会有解决疑问的灵感了。