ORB-SLAM3论文详细解读
最近在搞ORB-SLAM3，程序已经跑了一遍，现在研读对应论文——ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
论文和代码都在此处，欢迎浏览我的SLAM专栏，包括slam安装运行、代码注释、原理详解，一起加油淦穿SLAM。
本博客加粗字体是笔记，普通字体是原文翻译，建议加粗总结字体细看。
文章目录
- 一、先认识框架图
- 二、摘要
- 三、介绍
- 四、相关工作
-
- 1. Visual SLAM
- 2.Visual-Inertial SLAM
- 3.Multi-Map SLAM
- 五、系统概述
- 六、相机模型
-
- 1.Relocalization（重定位问题）
- 2.Non-rectified Stereo SLAM
- 七、VISUAL-INERTIAL SLAM
-
- 1.Fundamentals（VI SLAM的基础部分）
- 2.IMU Initialization（IMU初始化）
- 3.Tracking and Mapping（跟踪和映射）
- 4.Robustness to tracking loss（跟踪损耗的鲁棒性）
- 七、MAP MERGING AND LOOP CLOSING（地图融合及回环）
-
- 1.Place Recognition（位置识别）
- 2.Visual Map Merging（视觉地图合并）
- 3.Visual-Inertial Map Merging
- 4.Loop Closing（循环结束）
- 八、EXPERIMENTAL RESULTS（实验结果）
-
- 1.Single-session SLAM on EuRoC
- 2.Visual-Inertial SLAM on TUM-VI Benchmark
- 3.Multi-session SLAM（多端SLAM）
- 4. Computing Time
- 九、CONCLUSIONS（结论）
- 十、REFERENCES（致谢）
一、先认识框架图
认识框架图可以帮助认识一些文章中的术语，此处和ORB-SLAM2的框架对照。想看懂slam3需要对slam2有基础掌握。
下图为ORB-SLAM2：
下图为ORB-SLAM3：
ORB-SLAM2的框架包括跟踪、局部建图、闭环这三个模块，中间是位置识别，只有一个地图，是单地图，ORB-SLAM3中是摞在一起的多个地图。
ORB-SLAM3输入的是Frame，是图像帧，可以是单目双目或者RGB-D，还有一个数据流是IMU，是惯性测量单元。Frame进来后提取orb特征点，IMU需要做一个积分。进入到跟踪线程，在slam2中提到过，有三种跟踪模式，包括重定位跟踪等，在slam3里多了一个MU可以预测速度，slam2只是利用两个帧之间的差距来做速度预测。slam3会考虑到地图是否重新创建，后边会讲到。这是第一阶段，是一个初始的跟踪，跟踪完以后做一个位姿优化，优化完后有一个粗糙的位姿。然后做第二阶段的跟踪，叫做跟踪局部地图，下图中在slam2中的红色的点就叫做局部地图。
当前帧只能看到一小部分，局部地图是共时关键帧能看到的所有红色地图点，当前帧只能看到小的梯形区域，当前帧和关键帧的中间部分圆圈是共时关键帧，共时帧能看到大的梯形区域（此处我也不很明白）。slam3中把这些红色的点投影到当前帧，再去匹配更多的地图点去跟踪，优化出来的位姿相当于第一阶段更准确。这是跟踪的两个阶段，中间是怎么去判断关键帧，是否建立关键帧，这个关键帧的建立和选择也是一个需要操作讲究的地方，后边有具体细节。
拿到关键帧后就进入LOCAL MAPPING，这个是局部建图，是一个单独的线程。拿到关键帧后也是和slam2一样，也是关键帧插入，剔除地图点，新增地图点，然后做局部BA，还有一个关键帧的剔除。slam3里新增了一个IMU的初始化，IMU尺度精细化，此处在局部建图中体现。在局部建图的这个线程里面，他的关键帧又送到闭环以及合并线程。以前slam2里只有一个LOOP CLOSING，在slam里比较复杂。进来之后的位置识别还是用磁带做的，去计算sim3或者se3，候选关键帧的检测都叫做识别optimize essential graph和slam2一样做一个本质图优化、闭环融合，在slam3里多了一个地图合并，论文里有讲述是怎么合并的，也是先做位姿优化。最后出来就是一个全局的BA。中间这一块和2类似，slam3叫做地图集，有活跃地图，包括特征点、关键帧、正视图、生成树，当前地图就是活跃地图。如果跟着跟着跟丢了，就把当前地图视为非活跃地图，再把最新的地图作为活跃地图。
slam3这块对整个算法提升很明显，以上为算法框架。
二、摘要
本文介绍了ORB-SLAM3系统，它是第一个使用单眼、立体和RGB-D相机，使用针孔和鱼眼镜头模型，实现视觉、视觉惯性和多地图SLAM的系统。虽然说是VI系统，但可以把I关掉，用纯视觉。
第一个主要的创新是一个基于特征的紧密集成的视觉-惯性SLAM系统 （基于特征点的紧耦合系统） ，它完全依赖于Maximum-a-Posteriori (MAP最大后延概率)估计，即使在IMU初始化阶段。其结果是，无论在小型还是大型、室内还是室外环境中，该系统都能稳健地实时运行，并且比以前的方法精确2到5倍。（特地强调IMU初始化也用MAP，说明IMU初始化很重要）
第二个主要的新奇之处是一个多地图系统，它依赖于一种新的位置识别方法，可以提高记忆 (召回率高，准确率更高更快，能识别到两个重复的路线并合并) 。多亏了它，ORB-SLAM3能够在较差的视觉信息下运行很长一段时间:当它丢失时，它将开始一个新的地图，当重新访问已映射的区域时，它将与之前的地图无缝合并。与只使用最后几秒信息的视觉里程计系统相比，ORB-SLAM3是第一个能够在所有算法阶段重用所有之前信息的系统。这允许在bundle调整中包含共同可见的关键帧，这提供了高视差观测提高了准确性，即使它们在时间上被广泛地分开，或者如果它们来自于之前的映射会话。
我们的实验表明，在所有传感器配置中，ORBSLAM3与文献中可用的最佳系统一样健壮，而且显著地更加精确。值得注意的是，我们的立体惯性SLAM在EuRoC无人机上实现了3.5 cm的平均精度，在TUM-VI数据集房间(AR/VR场景的设置代表)的快速手持动作下实现了9 mm的平均精度。
三、介绍
对视觉同步定位与测绘系统(SLAM)和视觉里程测量(VO)的深入研究，无论是单独使用相机还是与惯性传感器结合使用，在过去20年里产生了精度和鲁棒性不断提高的优秀系统。现代系统依赖于最大后验估计(MAP)，在视觉传感器的情况下对应于束调整(BA)，要么是在基于特征的方法中使特征重投影误差最小化的几何校正，要么是在直接方法中使一组选中像素的光度误差最小化的光度校正。
近年来，随着集成闭环技术的VO系统的出现，VO和SLAM之间的边界更加分散。
视觉SLAM的目标是利用移动代理上的传感器来构建环境地图，并实时计算该地图中代理的姿态。
相比之下，虚拟视觉系统将他们的重点放在计算代理的自我运动，而不是建立一个地图。SLAM地图的最大优势在于，它允许在BA中匹配和使用之前的观测数据进行三种类型的数据关联：
- 短期数据关联：匹配最近几秒内获得的地图元素。这是大多数VO系统使用的唯一数据关联类型，一旦环境元素离开视图，就会忘记它们，导致即使系统在同一区域移动，也会导致持续的估计漂移。 可以对应到orb里的跟踪线程，视觉里程计里都有跟踪，仅仅和最近几秒内的元素进行匹配，不管是参考关键帧还是重定位，都是和最近一小段时间内的地图做一个匹配。如果匹配失败或者地图跟不到了就是匹配失败，数据关联就是失效。
- 中期数据关联：匹配距离相机较近但累计漂移仍较小的地图元素。与短期观测相比，这些可以在BA中匹配和使用，并允许系统在地图区域移动时达到零漂移。与环路检测的VO系统相比，它们是我们的系统获得更好精度的关键。slam3的精度很高就和这个中期数据关联有很大关系。对应到local mapping线程，不止和周围的一点做匹配，他是用共视的观点去做的，只要两个关键帧之间有共视关系，就可以进行关联，进行地图点的互相匹配生成更多的地图点。
- 长期数据关联：使用位置识别技术将观测结果与之前访问过的区域的元素进行匹配，而不考虑累积漂移(环路检测)、当前区域之前在断开的地图中被映射(地图合并)或跟踪丢失(重新定位)。长期匹配允许重置漂移和使用位姿图(PG)优化，或更准确地使用BA校正地图。这是在大中型网络环境下实现SLAM精度的关键。长期数据关联就是在整个过程中都有，其实就是闭环线程，用磁带的方法实现，从头到尾都在用这个磁带做匹配，闭环的时候用磁带去找，比如俩个路线能重合就是依靠磁带。
- 此处应该有第四个数据关联，叫做多地图的数据关联。在认识框架中有见过当前地图是活跃地图，与非活跃地图之间是可以建立连接的。一个地图在跑，跑着跑着跟丢了就重建一个地图，假如依次重建了四个地图，地图之间可以建立联系的
本研究在ORB-SLAM[2]、[3]和ORBSLAM visual-inertial[4]基础上构建了首个能够充分利用短期、中期和长期数据关联的视觉和视觉-惯性系统，实现了测绘区域零漂移。在这里，我们进一步提供了多地图数据关联，这允许我们匹配和使用来自之前的映射会话的BA地图元素，实现了SLAM系统的真正目标:构建一个可以稍后用于提供精确定位的地图。
这基本上是一篇系统论文，其最重要的贡献是ORB-SLAM3库本身[5]，它是迄今为止最完整、最精确的视觉、视觉-惯性和多地图SLAM系统(见表一)。ORB-SLAM3的主要新颖之处是:
- 一种单目和立体视觉-惯性SLAM系统：即使在IMU(惯性测量单元)初始阶段也完全依赖于最大后置估计(MAP)。所提出的初始化方法在之前的[6]中给出。在这里，我们添加了它与ORB-SLAMvisual-inertial的集成，对立体-惯性SLAM的扩展，以及对公共数据集的彻底评估。我们的研究结果表明，单目和立体视惯系统具有非常强的鲁棒性，并且明显比其他视惯方法更精确，即使是没有闭环的时候中也是如此。
- 高召回率的位置识别算法：近年来，许多视觉SLAM和VO系统都使用词库中的DBoW2包来解决位置识别问题。DBoW2要求时间一致性，将三个连续的关键帧匹配到同一个区域，然后检查几何一致性，以牺牲召回率为代价提高了精度。因此，系统在闭合循环和重用之前映射的区域时速度太慢。我们提出了一种新的位置识别算法，该算法首先检查候选关键帧的几何一致性，然后检查三个共可见关键帧的局部一致性，在大多数情况下，这些关键帧已经在地图上。这种策略增加了召回率和数据关联的密度，提高了地图的准确性，但代价是略微增加了计算成本。是把以前的闭环算法做了一个改进，具体在代码中有体现，在增加计算量的前提下有了很大提升。
- ORB-SLAM Atlas （多地图） ：第一个完整的多地图SLAM系统，能够处理视觉和视觉惯性系统，在单目和立体配置。Atlas可以表示一组不相连的地图，并将所有的映射操作流畅地应用到这些地图上:位置识别、相机重新定位、循环闭合和精确无缝的地图合并。这允许自动使用和组合在不同时间构建的地图，执行增量多会话SLAM。在[10]中提出了用于视觉传感器的ORBSLAM Atlas的初步版本。本文提出了一种新的地点识别系统——视觉惯性多地图系统及其公共数据集评价方法。
- 一个抽象的相机表示，使SLAM代码不知道所使用的相机模型，并允许通过提供它们的投影、非投影和雅可比函数来添加新的模型。我们提供针孔[11]和鱼眼[12]模型的实现。
所有这些新特性，加上一些代码改进，使ORB-SLAM3成为新的参考visual和visualinertial开源SLAM库，与文献中可用的最好的系统一样健壮，并且显著地更加准确，正如我们在第七节中的实验结果所示。我们还提供了单眼、立体、单眼-惯性和立体-惯性SLAM结果的比较，这些结果可能对从业者感兴趣。
四、相关工作
表一概述了最具代表性的视觉和视觉-惯性系统，显示了用于估计和数据关联的主要技术。表中包含的定性准确性和稳健性评级是基于第七节给出的结果，以及报告的PTAM、LSD-SLAM和ORB-SLAM的比较。
上图中，目前的视觉和视觉惯性slam的对比，左侧都是流行且好用的算法，不是量化效果对比是功能对比。
第一列：SLAM or VO，包括SLAM和VO
第二列：像素怎么用的，第一个Shi Tomasi ，是哈尔斯角点的改进算法，作者是华人史建波，这个算法以他和零一人的名字命名。
第三列：数据关联，包括图中几个方法。
第四列：估计方法，有EKF、BA、Local BA等。第四列是重定位
第五列：重定位，基本用DBoW2
第六列：闭环，也是大部分为DBoW2 PG+BA，位姿优化加BA
第七列：是否包含多地图系统，VINS-Fusion和slam3有。
第八列后五列：单目、双目、单目IMU、双目IMU和鱼眼是否支持，只有slam3全支持，VINS-Fusion纯单目不支持
第十三列：精度上有分三个等级
第十四列：鲁棒性上等级
1. Visual SLAM
首先在MonoSLAM中使用扩展卡尔曼滤波(EKF)和在后续图像中跟踪的ShiTomasi点进行相关引导搜索来解决单目SLAM。利用保证特征匹配一致性的技术，显著改善了中期数据关联，实现了手持视觉SLAM。
相比之下，基于关键帧的方法只使用少数选定的帧来估计地图，丢弃来自中间帧的信息。这允许在关键帧速率下执行更昂贵但更准确的BA优化。最具代表性的系统是PTAM，该系统将摄像机跟踪和映射工作分为两个并行线程。在相同的计算代价下，基于关键帧的技术比过滤技术更精确，成为视觉SLAM和VO的黄金标准。在[56]中使用滑动窗口BA实现大尺度单目SLAM，在中使用双窗口优化和共视图实现大尺度单目SLAM。
在这些思想的基础上，ORB- slam[2]、[3]使用ORB特性(其描述符提供短期和中期数据关联)，构建一个可协作图来限制跟踪和映射的复杂性，并使用bag-of-words库DBoW2执行循环关闭和重新定位，实现长期数据关联。是迄今为止唯一集成了三种数据关联的可视化SLAM系统，我们认为这是其卓越准确性的关键。本研究利用新的Atlas系统在丢失跟踪时启动新的地图，提高了其在纯视觉SLAM中的鲁棒性，并利用具有改进回忆率的新位置识别方法提高了其在复杂场景中的准确性。
直接方法不提取特征，而是直接使用图像中的像素强度，并通过最小化光度误差估计运动和结构。LSD-SLAM能够使用高梯度像素构建大规模的半密集地图。然而，地图估计被简化为posegraph (PG)优化，其精度低于PTAM和ORB-SLAM的。该混合系统SVO提取FAST特征，采用直接跟踪方法跟踪特征和帧与帧之间具有非零强度梯度的任意像素，利用重投影误差优化摄像机轨迹和三维结构。SVO非常高效，但作为一种纯粹的VO方法，它只能进行短期数据关联，这限制了其准确性。Direct Sparse Odometry DSO能够在点检测器性能较差的情况下计算精确的相机姿态，增强了在低纹理区域或对模糊图像的鲁棒性。它引入了局部光度BA，同时优化了一个窗口的七个最近的关键帧和反深度的点。这项工作的扩展包括立体声，环路闭合使用特征和DBoW2，以及视觉惯性里程计[46]。直接稀疏映射DSM在直接方法中引入了映射重用的思想，说明了中期数据关联的重要性。在所有的情况下，缺乏短期、中期和长期数据关联的整合，导致准确度低于我们的建议(见第七节)。DSO在特征点缺失的时候，弱纹理以及图像模糊的时候效果更好，这是直接法的优势。
2.Visual-Inertial SLAM
视觉和惯性传感器的组合为不良纹理、运动模糊和遮挡提供了鲁棒性，在单目系统的情况下，使标度可观察。
紧耦合方法的研究可以追溯到MSCKF，该方法通过特征边缘化避免了EKF在特征数量上的二次代价。最初的系统在[34]中得到完善，在[35]，[36]中扩展到立体。第一个基于关键帧和束平差的紧耦合视觉里程计系统是OKVIS，它同时可以使用单目和立体视觉。虽然这些系统依赖于功能，但ROVIO通过直接数据关联为EFK提供光度误差。
ORB-SLAM-VI[4]首次提出了一种视觉惯性SLAM系统，该系统能够重用具有短期、中期和长期数据关联的地图，并在基于IMU预集成[60]的精确局部视觉惯性BA中使用它们。但其IMU初始化技术太慢，耗时15秒，影响了鲁棒性和准确性。更快的初始化技术在中提出，该方法基于一种封闭的解决方案，联合检索比例尺、重力、加速度计偏差和初始速度以及视觉特征深度。关键的是，它们忽略了IMU的噪声特性，并将空间中点的3D误差最小化，而不是它们的重投影误差，这是基于特征的计算机视觉的黄金标准。我们之前的工作表明，这将导致巨大的不可预测的错误。
VINS-Mono[7]是一种非常精确和鲁棒的单目惯性里程测量系统，采用DBoW2和4自由度位图优化和地图合并的闭环。特征跟踪是用Lucas-Kanade跟踪器执行的，比描述符匹配略强一些。在VINS-Fusion中，它已经扩展到立体和立体惯性。
VI-DSO将DSO扩展到视觉-惯性里程测量，提出了一种将惯性观测与所选高梯度像素的光度误差结合起来的束调整，具有非常好的精度。由于成功地利用了高梯度像素的信息，提高了对纹理较差场景区域的鲁棒性。它们的初始化方法依赖于视惯性BA，在1%的尺度误差范围内收敛需要2030秒。
最近的BASALT 是一种立体惯性测程系统，它从视觉惯性测程中提取非线性因子用于BA，并匹配ORB特性闭合回路，实现了非常好的精度。 Kimera [8]是一种新型的、优秀的度量语义映射系统，但它的度量部分包含了立体惯性里程计、DBoW2闭环和位图优化，实现了与VINS-Fusion相似的精度。
在这项工作中，我们建立了ORB-SLAM-VI，并将其扩展到立体惯性SLAM。本文提出了一种基于最大后验估计(Maximum-a-Posteriori, MAP)的快速初始化方法，该方法适当考虑了视觉和惯性传感器的不确定性，在2秒内估计出5%的真尺度误差，在15秒内收敛到1%的尺度误差。以上讨论的所有其他系统都是视觉-惯性里程计方法，其中一些采用环路闭合的扩展方法，缺乏使用中期数据关联的能力。我们相信，这一点，连同我们的快速和精确的初始化，是我们的系统获得更好的精度的关键，即使在没有循环的序列中。
3.Multi-Map SLAM
在一种滤波方法中首次提出了通过地图创建和融合的方式为探测过程中的跟踪损耗增加鲁棒性的想法。最早的基于关键帧的多地图系统之一，但地图初始化是手动的，系统不能合并或关联不同的子地图。多地图功能作为协作地图系统的一个组成部分被研究，该系统有几个地图代理和一个中央服务器，只能接收信息，或者像C2TAM中那样具有双向信息流。MOARSLAM为协同多设备SLAM提出了一个健壮的无状态客户端-服务器架构，但主要关注的是软件架构，没有报告准确性结果。
最近，CCM-SLAM提出了一种基于ORB-SLAM的分布式多地图系统，可用于多架具有双向信息流的无人机。他们的重点是克服有限带宽和分布式处理的挑战，而我们的是准确性和鲁棒性，在EuRoC数据集上取得了显著更好的结果。SLAMM也提出了ORB-SLAM2的多地图扩展，但将子地图保持为独立的实体，同时我们执行无缝地图合并，构建更精确的全球地图。
VINS-Mono[7]是一种依赖于位置识别库DBoW2[9]的具有闭环和多地图功能的视觉里程计系统。我们的实验表明，由于使用中期数据关联的能力，ORB-SLAM3在EuRoc数据集上的单目-惯性单会话操作的准确性是VINS-Mono的2.6倍。我们的Atlas系统也建立在DBoW2的基础上，但提出了一种新的高召回率的位置识别技术，并利用局部BA实现了更详细、更准确的地图合并，在EuRoC上的多会话操作中，其精度比vns - mono提高了3.2倍。
在此处做一个ORB-SLAM2和ORB-SLAM3的总结对照，如下图：
五、系统概述
ORB-SLAM3构建在ORB-SLAM2[3]和ORBSLAM-VI[4]上。它是一个完整的多地图和多会话系统，能够在纯视觉或视觉惯性模式下使用单目，立体或RGB-D传感器，使用针孔和鱼眼相机型号。图1显示了主要的系统组件，它们与ORB-SLAM2的组件并行，具有一些重要的新奇之处，下面总结一下:
- Atlas是由一组断开连接的映射组成的多映射表示。有一个活动映射，其中跟踪线程本地化传入的帧，并通过本地映射线程不断优化和增长新的关键帧。我们将Atlas中的其他地图称为非活动地图。系统建立了一个唯一的关键帧DBoW2数据库，用于重定位、循环关闭和地图合并。
- 跟踪线程实时处理传感器信息并计算当前帧相对于活动地图的姿态，使匹配的地图特征的重投影误差最小化。它还决定当前帧是否成为关键帧。在视惯性模式下，通过在优化过程中考虑惯性残差来估计体速度和IMU偏差。当跟踪丢失时，跟踪线程试图重新定位所有Atlas地图中的当前帧。如果重新定位，跟踪将恢复，如果需要，将切换活动地图。否则，在一段时间之后，活动映射将被存储为非活动的，并从头开始初始化一个新的活动映射。
- 本地映射线程添加关键帧并指向活动的地图，删除冗余的，并使用视觉或视觉惯性束调整来细化地图，操作在靠近当前帧的关键帧的本地窗口中。此外，在惯性情况下，利用我们的映射估计技术，由映射线程初始化和细化IMU参数。
- 循环和映射合并线程检测常见区域在活动地图和关键帧处的整个地图集之间率。如果公共区域属于活动地图，则执行循环校正;如果它属于不同的地图，两个地图无缝合并为一个地图，成为活动地图。循环校正后，将在独立线程中启动完整的 BA，以在不影响实时性能的情况下进一步优化映射。
六、相机模型
ORB-SLAM假设所有系统组件都采用针孔相机模型。我们的目标是通过提取与相机模型相关的所有属性和函数(投影和非投影函数、雅可比矩阵等)，将相机模型从整个SLAM管道中抽象出来。这允许我们的系统通过提供相应的相机模块来使用任何相机模型。在ORB-SLAM3库中，除了针孔模型，我们还提供了Kannala-Brandt鱼眼模型。论文目标是想抽象相机模型，把这个模型和slam系统进行一个解耦合，怎么解耦合呢？是把相机模型的关键部分（投影矩阵、反投影矩阵、雅可比）提出来。
由于大多数流行的计算机视觉算法都采用针孔摄像机模型，许多SLAM系统要么对整个图像进行校正，要么对特征坐标进行校正，以在理想的平面视网膜上工作。然而，这种方法对于鱼眼镜头来说是有问题的，因为鱼眼镜头可以达到或超过180度的视场(FOV)。图像矫正是不可取的，因为在周边的对象扩大，在中心的对象松散的分辨率，阻碍了特征匹配。校正特征坐标需要使用小于180°的视场，这给许多计算机视觉算法带来了麻烦，这些算法假设图像沿图像方向有均匀的重投影误差，这与校正后的鱼眼图像的情况相去很远。这迫使图像的外部部分突出，失去了大视场的优势:更快的环境映射和更好的遮挡鲁棒性。接下来，我们讨论如何克服这些困难。slam3里提供了针孔模型和Kannala-Brandt鱼眼模型，如果是其他模型的话也可以提供对应的参数也可以去用。即使把相机模型做了抽象，但还是有很多问题。
1.Relocalization（重定位问题）
一个鲁棒的SLAM系统需要在跟踪失败时重新定位摄像机的能力。ORB-SLAM通过设置基于ePnP算法的Perspective-n-Points求解器来解决重新定位问题，该算法在所有公式中都假设有一个校准过的针孔摄像机。为了跟进我们的方法，我们需要一个PnP算法，它的工作独立于相机模型使用。为此，我们采用了极大似然透视n- point算法(Maximum Likelihood Perspective-n-Point algorithm, MLPnP)，该算法使用射影射线作为输入，因此与相机模型完全解耦。相机模型只需要提供一个从像素传递到投影光线的非投影函数，就可以使用重新定位。重定位在slam2里用的是ePnP算法，这个方法假设了一个相机已经标定了，是一个针孔相机模型，只能是针孔模型。重定位在slam3中用了MLPnP算法，最大似然PnP算法，他彻底和相机模型解耦合，这个是更通用的方法，相比slam2用什么模型都可以。
2.Non-rectified Stereo SLAM
大多数立体大满贯系统假定立体框架纠正,即图像都转化为针孔预测使用相同的焦距,与图像平面平面与水平纵向线条,这样一个特性在一个图像可以很容易地匹配通过观察其他的同一行图像。然而，矫正立体图像的假设是非常限制性的，在许多应用中，既不适合也不可行。例如，矫正一个发散的立体声对，或立体鱼眼相机将需要严重的图像裁剪，失去大视场的优势。
因此，我们的系统不依赖于图像校正，考虑到立体装置为两个单目相机，具有:它们之间的一个常数相对SE(3)变换，和可选地，观察场景的相同部分的公共图像区域。这些约束条件允许我们通过在三角化新地标和束调整优化时引入这些信息来有效地估计地图的比例尺。接下来，我们的SLAM管道估计了一个6自由度刚体位姿，其参考系统可以位于其中一个摄像机或IMU传感器中，并表示摄像机相对于刚体位姿。
如果两个相机都有一个重叠的区域，在这个区域我们可以进行立体观察，我们可以在第一次看到它们的时候三角测量真实尺度的地标。这两幅图像的其余部分仍然有大量的相关信息，这些信息在SLAM管道中被用作单目信息。在这些区域首先看到的特征是从多个视图三角化的，就像在单目情况下。
在双目模式下，slam2是假设提前矫正好了，用了同样的焦距，矫正完图像位于同一个平面上，而且特征点是在水平极限上是对齐的，这样在匹配时只要在极限附近搜索就可以了，比较简单但有些情况不适用，有时我们没有办法提供这种矫正好的双目，而且麻烦，实际上双目两个相机也可能不是水平的。另外就是鱼眼模式先去去畸变，然后裁剪，这样市场角很小，失去了鱼眼大市场角下对遮挡鲁棒的优势。在slam3中不依赖于图像的矫正，他是把图像的双目变成两个单目相机去看，双目的两个相机的相对位姿是固定的，是一个固定的矩阵，若两个相机有共视区域就可以做三角化，就可以得到一些地图点，若没有共视区域就当成单目用就可以了。
整个相机模型这部分的目的就是为了让这个算法更通用，不需要必须是针孔相机模型或者双目相机必须水平放置，焦距一样等条件都不需要。
七、VISUAL-INERTIAL SLAM
ORB-SLAM-VI[4]是第一个真正的视觉-惯性SLAM系统，能够实现地图复用。但是，该算法局限于针孔单目相机，初始化速度太慢，在一些具有挑战性的场景中失败。在这项工作中，我们建立在ORB-SLAM-VI上，提供了一种快速准确的IMU初始化技术，以及一个开放源代码的SLAM库，能够使用针孔和鱼眼相机进行单目-惯性和立体-惯性SLAM。
这一章开始是重点部分了，提出slam3提出了一个更快更好的IMU初始化技术，总之比之前的好很多
1.Fundamentals（VI SLAM的基础部分）
而在纯视觉SLAM中，估计的状态只包含摄像机当前的位姿，在视觉-惯性SLAM中，需要计算额外的变量。这些是世界坐标系中的身体姿势Ti = [Ri, pi]∈SE(3)和速度vi，以及陀螺仪和加速度计偏差bg i和bai，假设它们是按照布朗运动演化的。这就得到了状态向量:（主要讲了都有哪些状态参与了计算，slam2里是纯视觉的，所以没有太多的状态，只有旋转和平移。现在加入了一个IMU，一般来说VIO或者多传感器融合等涉及到IMU的部分都会加另外一些状态量。Ti表示第i时刻的旋转和平移，Vi是第i时刻的速度，big是表示第i时刻的陀螺仪的偏执，bia表示加速度计的偏执，一共五个状态量，每个状态量是三维，可以理解为整个状态量是一个15×1的列向量）
对于视觉惯性SLAM，我们按照[60]中开发的理论，在连续的视觉帧i和i + 1之间预先集成IMU测量，并在流形上公式化。我们获得预先集成的旋转，速度和位置测量值，表示为 ∆Ri，i+1，∆vi，i+1 和 ∆pi，i+1，以及整个测量向量的协方差矩阵 ΣIi，i+1。给定这些预积分项和态Si和Si+1，我们采用[61]中惯性残差rIi,i+1的定义:（IMU离不开预积分和残差，第一个式子是把Δ设为IMU残差，表示第i时刻到i+1时刻的旋转。残差定义为三个变量，包括速度∆Ri,i+1、旋转∆vi,i+1、平移∆pi,i+1。第二个式子中表示旋转，比较简单，其中∆RTi,i+1是IMU的预积分，RTi Ri+1是从第i时刻到i+1时刻旋转的改变量
预积分转置✖改变量可以理解为求了两个改变量之间的改变量，加一个log，把旋转矩阵改为向量，所以关于旋转的残差是一个3×1的向量。第三个式子是速度，其中g是在算预积分时，算速度和位置时，我们会拿到加速度计的数据，并且加速度计数据里包含重力，在计算时并没有把重力去掉，就导致得到的预积分里的∆v是受到重力的影响的。vi+1 表示第i+1时刻速度，这个速度是由slam来算出来的，在VIO里当前帧是由IMU来提供的，但是当前帧的状态最后是由匹配点的误差结合预积分来定的，所以这里根据当前状态算出的这个∆v跟IMU的∆v有偏差，所以把他定义为残差。第四个是位置，和初中物理公式很像，pj=pi+平均速度×∆t+1/2∆t方）
其中Log: SO(3)→R3从李群映射到向量空间。结合惯性残差，我们还使用坐标系i和位置为xj的3D点j之间的重投影误差rij:
RnΠ:R3→是相应的摄像机的投影函数模型,uij j指向图像的观察我,在协方差矩阵Σij, TCB∈SE(3)代表刚性变换从body-IMU相机(左或右),已知的校准、和⊕SE(3)组的转换操作R3元素。
（IMU里还有重投影误差，上图公式（3）中，rij里i表示第i帧，j表示第j个三维点，uij表示第i帧的图像中的第j个特征点对应的像素点，对应的像素坐标，是我们提取特征点时特征点的像素坐标。xj表示第j个点在世界坐标系下的坐标，首先根据计算的位姿Ti-1把在世界坐标系下的坐标转换成在IMU坐标系下的坐标，再根据相机外参TCB把这个坐标转换到视觉下，TCBT−1i ⊕ xj连起来就表示相机坐标系下这个点的三维坐标，Π连乘的符号定义为投影的方程，通过相机的内参畸变投到图像平面上，这样一来两个点就可以求出像素误差）
结合惯性和视觉残差项，视觉-惯性SLAM可以作为一个基于关键帧的最小化问题。给定一组k + 1的关键帧及其状态¯Sk .= {S0…Sk}，以及l个三维点的集合及其状态X .= {x0…视惯性优化问题Xl−1}表示为:下面公式（4）这里定义了在整个系统中，都有哪些误差参与了优化。+号前半部分是上面讲述的关于IMU的残差的，+号后半部分是视觉重投影的误差，ρHub是鲁棒和函数主要作用为，匹配时有误匹配的情况，重投影误差很大，若起很大，则在整个误差里占的比例就很大，影响最后精度。ρHub鲁棒和函数会在当这个误差很大时会降低这个误差。
其中Kj是观察3D点j的关键帧的集合。这种优化可以概括为图2a所示的因子图。注意，对于重投影误差，我们使用鲁棒Huber kernel ρHub来减少伪匹配的影响，而对于惯性残差，则不需要这样做，因为不存在错联。这种优化需要在跟踪和映射过程中进行调整，以提高效率，但更重要的是，它需要良好的初始种子收敛到精确的解。图2a表示整个BA，视觉加惯性的BA，红色方框表示重投影的误差，黄色方框表示惯性误差（imu的残差），红色是位姿向量平移，绿色的是速度，紫色的是偏执，蓝色是地图特征点。其中有些约束，在连续两帧之间对应偏执对应的约束，虽然这个偏执在运行的过程中会发生改变，在非常小的范围内这个偏执不会产生很大的变化。所以比如两个紫色之间会产生一个约束，比如b0和b1之间有一个紫色小方块，改变量是0。
图2c表示，在第二阶段优化的时候，他会把窗口内从开始关键帧到开始之内的所有的关键帧的位姿是固定住的。红圈T有个横线表示他现在的尺度是没有的，旋转和位姿是固定的，但是速度是不固定的。黄点是残差。于此同时，我们假定在这个时间内偏置是同一个数，偏置是不变的，这仅仅是在初始化时做的假设，还有这个过程中对应的尺度和重力方向，最后下边浅蓝色的方框是先前残差,表示的质量，在这个阶段他假设了偏置是0.
2.IMU Initialization（IMU初始化）
这一步的目标是获得惯性变量的良好初始值:体速度、重力方向和IMU偏差。一些系统，如VI-DSO，尝试从零开始求解视觉-惯性BA，避开特定的初始化过程，获得缓慢的惯性参数收敛(高达30秒)。
在本工作中，我们基于三个关键观点提出了一种快速准确的初始化方法:
- 纯单目SLAM可以提供非常精确的初始图，但主要问题是比例尺未知。首先解决视觉问题将增强IMU的初始化
- 当尺度显式表示为优化变量时，比使用BA的隐式表示收敛更快。我们在初始化优化时，不止优化15a那个量，另外还要优化一个尺度的东西，把这个尺度显示的优化出来。
- 在IMU初始化过程中忽略传感器的不确定性会产生巨大的不可预知的错误。在别的VIO系统里，视觉还没初始化的时候，都会忽略一些IMU偏置或误差带来的影响。
因此，适当考虑到传感器的不确定性，我们将IMU初始化描述为一个MAP估计问题，分为三个步骤:（正式的IMU初始化部分）
- Vision-only MAP （第一步是纯视觉的估计）
Estimation:我们初始化纯单目SLAM[2]，运行时间为2秒，以4Hz插入关键帧。在这段时间之后，我们得到了一个由k=10个相机姿势和数百个点组成的比例最新的地图，这是使用纯视觉BA进行优化的(图2b)。将这些位姿转换为body reference，得到轨迹 ¯T0:k = [R, ¯ p]0:k，其中bar表示单目情况下的最新变量。在VI模式下头十帧，不进行IMU初始化，在这段时间内进行纯视觉的估计，进行纯视觉的BA - 只有惯性的MAP估计 （纯惯性最大后延的优化） :在这一步中，我们的目标是获得最优的惯性变量估计，在MAP估计的意义上，只使用这些关键帧之间的T0:k和惯性测量。这些惯性变量可以叠加在仅含惯性的状态向量中
上式（5）定义了优化哪些量，第一个量s就是尺度，第二个量Rwg表示的是重力方向到世界坐标系的旋转，因为一开始是纯视觉初始化，他会把第一帧的位姿作为世界坐标系，就导致了第一帧他是单位矩阵，一般情况下我们在vio里希望z轴和重力方向重合，纯视觉没有这个关系，所以我们要找到这个关系。前两个量很重要。第三个量b是这段时间的偏置。第四个量V0k是速度，公式中没有加位姿但加了速度，经过测试这段时间速度作为优化比位姿好。
其中s∈R+是仅视觉解的比例因子;Rwg∈SO(3)是一个旋转矩阵，用于计算参考世界中的重力矢量g = RwggI，其中gI =(0,0, g)T, g为重力大小;b = (ba,bg)∈R6是初始化时假定恒定的加速度计和陀螺仪偏差;而¯v0:k∈R3是第一个到最后一个关键帧的最新速度，最初是由¯T0:k估计的。在这一点上，我们只考虑惯性测量集I0:k .= {i0,1…本土知识−1 k}。因此，我们可以表述一个MAP估计问题，其中需要最大化的后验分布为:
其中p(I0:k|Yk)表示似然，p(Yk)表示先验。考虑到测量的独立性，单惯性MAP估计问题可以写成:
对IMU的前积分和先验分布取负对数并假设高斯误差，最终得到优化问题:
**去看上边图2c的解释。**图2c所示的优化与方程4的不同之处在于不包含视觉残差，因为采用视觉SLAM估计的大尺度轨迹为常数，并添加一个先验残差，使得IMU偏差接近于零。协方差矩阵Σb表示关于IMU偏差取值范围的先验知识。IMU协方差预集成的详细信息ΣIi−1，请参见[61]。
当我们在流形中进行优化时，我们需要定义一个缩回[61]来在优化过程中更新Rwg。由于绕重力方向旋转并不假设重力发生变化，因此该更新用两个角度(δαg，
δβg)参数化:
公式（9）是表示尺度和重力方向是怎么更新的，括号有个0，因为只有在视觉和IMU的时候我们可以得到重力方向，但是我们不能得到航偏角是多少，所以维度是少了一位，所以把少的这一位去掉，防止他带来误差。
Exp(.)从R3到SO(3)的指数映射。为了保证在优化过程中比例因子保持为正，我们将其更新定义为: 下式（10）我们可以看尺度更新时并不是加减，而是×exp（），是让尺度不为负数.这个尺度最开始的初始化值为1.
一旦完成了惯性优化，框架的位姿和速度以及3D地图点就会按照估计的比例因子缩放，并旋转z轴，使其与估计的重力方向对齐。对误差进行更新，并重复IMU预集成，以减少未来的线性化误差。 - 视觉-惯性MAP估计:一旦我们对惯性和视觉参数有了很好的估计，我们就可以执行视觉-惯性联合优化，以进一步细化解决方案。这种优化可以用图2a表示，但对所有关键帧都有共同的偏差，并且比仅在惯性步骤中包含相同的偏差先验信息。看上图2a第三阶段的更新方式就是把点，把之前的残差还有视觉重定位误差的点都加进来。这次我们不假设整个重定位误差是不变的了，下边紫色圆圈在第三阶段更新时偏置不是一个数了，只不过偏置与偏置之间加了约束保证他们的改变量不是很大
我们在EuRoC数据集[6]上进行的详尽初始化实验表明，该初始化非常高效，在2秒的轨迹下达到5%的缩放误差。为了改进初始估计，在初始化后5秒和15秒执行视觉-惯性BA，收敛到1%的尺度误差，如第七节所示。在这些BAs之后，我们说地图是成熟的，意味着比例尺，IMU参数和重力方向已经准确估计。
我们的初始化方法比求解一组代数方程[62]-[64]的联合初始化方法精确得多，也比ORB-SLAMVI[4]中使用的初始化方法快得多，后者需要15秒才能得到第一个尺度估计，或VI-DSO[46]中使用的初始化方法快得多，这从一个巨大的尺度误差开始，需要20-30秒才能收敛到1%的误差。不同初始化方法之间的比较可以在[6]中找到。
在某些特定情况下，当慢速运动不能提供良好的惯性参数可观测性时，初始化可能无法在15秒内收敛到精确的解。为了在这种情况下获得鲁棒性，我们提出了一种新的尺度细化技术，该技术基于一种改进的纯惯性优化，其中包括所有插入的关键帧，但尺度和重力方向是唯一需要估计的参数(图2d)。注意，在这种情况下，恒定偏差的假设是不正确的。相反，我们使用从映射中估计的值，并修复它们。这种优化在计算上非常高效，每10秒在Local Mapping线程中执行一次，直到映射拥有超过100个关键帧，或者自初始化以来超过75秒。
最后，通过将比例因子固定为1，并将其从仅含惯性的优化变量中取出，可以很容易地将单目惯性初始化扩展为立体惯性初始化，增强其收敛性。（上三段主要说他们做的精度好等，没有重点内容）
3.Tracking and Mapping（跟踪和映射）
在跟踪和映射方面，我们采用了[4]中提出的方案。跟踪解决了一种简化的视觉-惯性优化，即只优化最后两帧的状态，而地图点保持固定.
对于制图来说，试图从方程4解决整个优化问题对于大型地图来说是很难的。我们使用关键帧及其点的滑动窗口作为可优化变量，包括从共同可见关键帧对这些点的观察，但保持它们的姿态不变。因为纯单目模式下，他在tracking里有基于单帧的位姿优化，在imu模式下不是很大的窗口来整体优化的，他只优化了当前帧和上一帧，就是vi模式下只优化了当前帧和上一帧，这两个帧之间的残差和重定位误差，来做的单帧优化。这里提到在初始化时如果移动的比较缓慢，就不能很好的提供初始化时的客观性，所以在slam3里提出了一个新的技术，即尺度更新技术，他的用法是imu更新成功后，他会在local mapping线程里每十秒钟执行一次尺度更新的方法，如果当前地图超过了75秒之后就不再进行了，具体做法参照图2d。图2d过程比较简单，计算量的东西比较少，没有加入重投影误差相关的东西，把他好多东西都固定住了，其中不固定的只有一个s和重力方向，仅仅优化这个残差。效果明显。
4.Robustness to tracking loss（跟踪损耗的鲁棒性）
在纯视觉SLAM或VO系统中，时间摄像机遮挡和快速运动会导致视觉元素丢失，从而导致系统丢失。ORB-SLAM率先使用了基于词袋位置识别的快速重定位技术，但事实证明它们不足以解决EuRoC数据集[3]中的困难序列。当跟踪小于15点地图时，我们的视觉惯性系统进入视觉丢失状态，并通过两个阶段实现鲁棒性:
- 短期丢失:当前的身体状态是由IMU读数估计的，地图点被投影在估计的相机姿势和搜索匹配在一个大的图像窗口。结果匹配包括在视觉-惯性优化。在大多数情况下，这允许恢复视觉跟踪。否则，5秒后，我们进入下一阶段。
- 长期丢失:一个新的视觉-惯性地图被初始化，并成为活动地图。
IMU初始化后15秒内系统丢失，地图将被丢弃。这可以防止积累不准确和无意义的地图。
slam2里有一个匀速跟踪模型，当前帧会根据这个模型算出位姿给的先验，根据先验把点投到当前帧，再根据当前帧的点去寻找匹配点。在IMU里就不需要这个匀速模型了，这段时间的先验可以通过预积分来提供，但是这种模式有跟丢的情况。如果5秒内没有跟踪上，他进行下一个阶段，下一个阶段到多地图系统了，跟踪定位失败后我们就不进行重定位了，不同于slam2，在slam2失败后就一直重定位，slam3在5秒后会重开一个新地图。
七、MAP MERGING AND LOOP CLOSING（地图融合及回环）
一帧和活动地图之间的短期和中期数据关联通常由跟踪和映射线程发现，方法是将地图点投射到估计的相机姿态中，并在只有几个像素的图像窗口中搜索匹配。ORB-SLAM采用DBoW2词袋位置识别系统[9]，实现对重定位和环路检测的长期数据关联[75]。这种方法也被最近大多数实现环路闭包的VO和SLAM系统所采用(表一)。
与跟踪不同的是，位置识别并不是从相机姿态的初始猜测开始的。相反，DBoW2用关键帧的词袋向量构建了一个数据库，并且给定一个查询图像能够根据它们的词袋有效地提供最相似的关键帧。只使用第一个候选项，原始DBoW2查询可以达到50-80%[9]的精度和召回率。为了避免误报破坏地图，DBoW2实现了时间和几何一致性检查，将工作点移动到100%的精度和30-40%的召回。重要的是，时间一致性检查至少在3个关键帧期间延迟识别。当我们尝试在我们的Atlas系统中使用它时，我们发现这种延迟和低召回率往往导致相同或不同地图中的重复区域。
在这项工作中，我们提出了一种新的地方识别算法，提高召回长期和多地图数据关联。每当映射线程创建一个新的关键帧时，就会启动位置识别，试图检测与Atlas中已经存在的任何关键帧的匹配。如果找到的匹配关键帧属于活动映射，则执行一个循环闭包。否则，它是一个多地图数据关联，然后，活动的和匹配的地图合并。作为我们方法的第二个新奇之处，一旦估计出新的关键帧和匹配映射之间的相对姿态，我们定义一个局部窗口与匹配的关键帧和它的邻居在共同可见图中。在这个窗口中，我们集中搜索中期数据关联，提高了循环闭合和地图合并的准确性。这两个新特性解释了在EuRoC实验中，ORB-SLAM3比ORB-SLAM2获得了更好的准确性。下面将解释不同操作的详细信息。（本节忽略性看一看即可）
1.Place Recognition（位置识别）
为了获得更高的召回率，对于每个新的活动关键帧，我们在DBoW2数据库中查询Atlas中的几个类似关键帧。为了达到100%的精度，每个候选对象都要经过几何验证的几个步骤。所有几何验证步骤的基本操作都包括检查图像窗口中是否有一个ORB关键点，该关键点的描述符与映射点的ORB描述符相匹配，并为它们之间的Hamming距离使用一个阈值。如果搜索窗口中有多个候选对象，为了消除模糊匹配，我们检查与第二最接近匹配的距离比[76]。我们的位置识别算法的步骤为:
-
DBoW2 candidate keyframes.我们使用活动关键帧Ka查询Atlas DBoW2数据库，以检索三个最相似的关键帧，不包括与Ka共可见的关键帧。我们将用于位置识别的每个匹配候选对象称为Km。从slam模型找到一个候选关键帧，这和slam2一样，会弄一个自己的keyframe database的一个类，这里边是好多单词，哪些单词在keyframe出现过他都会记录下来，当前帧提取完特征点后也会计算一个磁带，记录当前帧有什么单词去数据结构寻找共有单词数量最多的候选关键帧。当前关键帧定义为Ka，候选关键帧定义为Km。第一步是找关键帧。
-
Local window.。对于每一个Km，我们定义一个包含Km、其最佳共可见关键帧以及所有这些关键帧所观察到的地图点的局部窗口。DBoW2直接索引提供了一组Ka中的关键点和本地窗口关键帧中的关键点之间的假定匹配。对于这些2D-2D匹配，我们也可以在它们对应的地图点之间获得3D-3D匹配。对于每一个候选关键帧又定义一个局部地图，里边又包含一个候选关键帧，以及这个候选关键帧他的共识程度非常好的关键帧，又组成了一个小的窗口。第二部是局部窗口。
-
3D aligning transformation.
我们利用RANSAC计算了变换Tam，使Km局部窗口中的地图点与Ka局部窗口中的地图点更好地对齐。在纯单目中，或者在单目-惯性时，当图还不成熟时，我们计算Tam∈Sim(3)，否则计算Tam∈SE(3)。在这两种情况下，我们都使用Horn算法，使用三个3D-3D匹配的最小集合来找到Tam的每个假设。假设的匹配是，通过Tam对Ka中的地图点进行转换后，在Ka中得到一个低于阈值的重投影误差，对假设投了肯定的一票。如果票数超过阈值，则选择票数较多的假设。因为有好多后延关键帧组成的好多小窗口，有了这个局部窗口之后就开始算，就可以算当前关键帧到候选关键帧的旋转平移矩阵我们可以得到内点的数量，最后根据内点的数量做一个投票的机制，最后哪个内点最多就被取出来。 -
Guided matching refinement.
本地窗口中的所有地图点都用Tam进行转换，以找到更多与Ka中的关键点匹配的点。搜索也反过来，在本地窗口的所有关键帧中寻找匹配的Ka映射点。利用找到的所有匹配，通过非线性优化来优化Tam，其中目标函数是双向重投影误差，使用Huber影响函数来提供对虚假匹配的鲁棒性。如果优化后的内层数超过一个阈值，则启动引导匹配和非线性细化的第二次迭代，使用较小的图像搜索窗口。取出来后加了一个引导匹配更新，算的都是单帧与单帧之间的，这一步会把每一个候选关键帧代表一个小窗口，每个小窗口都是相关联的，共视程度比较高的关键帧。这些关键帧里边所对应的main point都取出来，都投影到当前关键帧，也就是给当前关键帧的特征点多一些匹配关系，这样算的时候增加精度。 -
V erification in three covisible keyframes.
为了避免误报，DBoW2在连续三个关键帧中等待位置识别，延迟或错过位置识别。我们的关键观点是，大多数时候，验证所需的信息已经在地图上了。为了验证位置识别，我们在地图的活动部分搜索与Ka共可见的两个关键帧，其中与本地窗口中的点匹配的数量超过一个阈值。如果没有找到这些关键帧，就会对新的关键帧进行进一步的验证，而不需要再次触发这个词袋。验证继续进行，直到三个关键帧验证Tam，或连续两个新的关键帧验证失败。验证三个共视的关键帧，这做法是为了避免错误的识别，宁可识别不出来也不要错误的识别。他是这么做到的，如果当前关键帧成功找到了地图融合或者回环，成功找到候选关键帧，先不着急，当前关键帧过去了，下个关键帧还会算是否和这个环境关键帧匹配成功，依次类推，当有三个连续的关键帧都找到同一个候选关键帧匹配成功后，才开始进行，这样有效避免了错误 -
VI Gravity direction
verification.在视觉-惯性情况下，如果活动地图是成熟的，我们估计Tam∈SE(3)。我们进一步检查俯仰角和滚转角是否低于阈值，以确定接受位置识别假设。最后利用了IMU的性质，如果算的TAM有问题的情况下，就会导致当前的关键帧的坐标系的z轴方向重合，此时我们算一下这个俯仰角以及滚转角，看看角度是否过大，若过大则表示此时坐标的重力方向与实际偏差过大，表面TAM算的不对。
2.Visual Map Merging（视觉地图合并）
第2部分是融合部分，这一部分也是拆开了两块来讲解，第一种是不带IMU，第二种是带IMU。
当一个成功的地点识别在活动地图Ma中的关键帧Ka和存储在Atlas Mm中的不同地图的匹配关键帧Km之间产生多地图数据关联时，通过对齐转换Tam，我们启动了一个地图合并操作。在此过程中，必须特别注意确保Mm中的信息可以被跟踪线程及时重用，以避免地图重复。为此，我们建议将Ma映射引入Mm参考。为了执行过程中保证候选地图中的信息，可以被迅速使用，因为候选地图中的信息是在后台的，属于非激活状态的地图，为了让其快速使用，防止重复，建议将当前激活的地图给他融合到候选关键帧地图，而不是把候选关键帧地图融合到当前帧地图，这里可能是由很多实验得出的结论。 由于地图可能包含很多元素（包括两种keyframe、map point，但数量比较多），合并它们可能需要很长时间，所以合并分为两个步骤。首先，合并在可视性图中Ka和Km的邻居定义的焊接窗口中进行，然后在第二阶段，通过姿态图优化将修正传播到合并图的其余部分。合并算法的具体步骤如下:
- Welding window assembly. 。焊接窗口包括Ka及其共可见关键帧、Km及其共可见关键帧以及它们所观测到的所有映射点。在它们被包含到焊接窗口之前，属于Ma的关键帧和映射点通过Tma进行变换，使它们相对于Mm对齐。地图融合就好比将两块板子用焊接的方式和到一起，焊接的位置就可以理解为两个地图中共视的位置，也就是当前的关键帧以及候选的关键帧，除此之外还有他们两个关键帧共视程度很高的其他关键帧，这些关键帧组合出来的这个窗口就叫做焊接窗口
- Merging maps.
地图Ma和Mm融合在一起成为新的活动地图。为了去除重复点，在Mm关键帧中主动搜索Ma点。对于每一个匹配，去除Ma中的点，继续累加Mm中的点的所有观测值。由于发现了新的中期点关联，通过添加连接Mm和Ma关键帧的边，可共性和基本图[2]得到了更新。融合地图提出了在融合中要删除一些重复点，代码中有体现，除此外之外在融合两个map point时会保留之间的共视关系 - Welding bundle adjustment.
局部BA对焊接窗口中Ma和Mm的所有关键帧以及它们所观察到的映射点进行优化(图3a)。为了固定量规自由，Mm的关键帧不属于焊接窗口，但观察任何局部地图点包含在BA中，它们的姿态固定。一旦优化完成，焊接区域包含的所有关键帧都可以用于相机跟踪，实现地图Mm的快速、准确重用。 - Essential-graph optimization.
在保持焊接区域关键帧不变的情况下，利用整个合并图的基本图进行姿态图优化。这种优化将修正从焊接窗口传播到地图的其余部分。第3、4段都是讲在确定了关系后就可以开始优化了，详细看图3a
在图3a中，右侧的大椭圆和周围的两排红色小圆圈就是焊接窗口，Ka表示当前的关键帧，Km表示候选关键帧，其他都是与他们共视程度比较高的关键帧。以及对应的map point 都是在焊接窗口里。优化阶段的主要目标就是优化焊接窗口里的这些元素。为了保证不让优化飞了，要固定一些帧，固定的帧是共视里的，因为很多map point ，找一些除了焊接窗口以外的其他一些共视高的关键帧。这里只是从候选地图取出来的帧，并没有从当前地图取关键帧。
3.Visual-Inertial Map Merging
以上都是纯视觉模式下，加入IMU就会发生一个改变。加入IMU后有两个地方需要改变。
视觉-惯性合并算法的步骤与纯视觉情况相似。修改步骤1)和步骤3)，以更好地利用惯性信息:
- VI welding window
assembly:如果活动映射是成熟的，我们应用可用的Tma∈SE(3)在Ma包含在焊接窗口之前对其进行映射。如果活跃地图还不成熟，我们使用可用的Tma∈Sim(3)对Ma进行对齐。 - VI welding bundle
adjustment:姿态，速度和偏差的关键帧Ka和Km和他们的五个最后的时间关键帧包括可优化。这些变量与IMU前整合项相关，如图3b所示。对于Mm，在本地窗口之前的关键帧被包含，但是固定的，而对于Ma，类似的关键帧被包含，但它的姿态仍然是可优化的。所有由上述关键帧看到的地图点，以及Km和Ka共可见关键帧的姿态都得到了优化。所有关键帧和点通过重投影误差进行关联。
这段我们直接看图3b，除去左侧的五个小红圈T的右侧大部分是焊接窗口，焊接窗口定义的和纯视觉的不一样，纯视觉的里边是当前窗口的关键帧和其他关键帧的共视度很高，但是imu模式下存在预积分，我们没办法一个个跳着找，找的方法是把当前关键帧往前推，找上上一个关键帧，一直找五个，就是图3b左侧的五个小红圈T，他们不固定，除此之外再找一个关键帧，优化时我们把速度和偏置固定，这个时关于当前帧。候选关键帧时一样的，他左右来找，算上自己是六个，再加一个最近的关键帧，一个七个，如图中的并排七个小圈，然后把位姿状态都固定然后做优化。除此之外还会优化其他的，因为焊接窗口包括很多点，根据这些点，从两个地图分别找出若干个共视程度高的关键帧。vi模式是从两个地图开始找，而纯视觉模式只是从候选模式地图里找。
4.Loop Closing（循环结束）
环路闭合校正算法类似于地图合并，但是在位置识别匹配的两个关键帧都属于活动地图的情况下。焊接窗口由匹配的关键帧组装而成，并检测和融合点副本，在可同构性和基本图形中创建新的链接。下一步是posegraph优化，将环路修正传播到地图的其余部分。最后一步是在考虑循环的中期和长期匹配后，通过全局BA来寻找MAP的估计。在视觉惯性情况下，仅当关键帧的数量低于阈值时，才会执行全局 BA，以避免巨大的计算成本。回环算法和融合算法一样，只不过不需要把两个地图来回导，但是回环的过程也是有map point的去重。位姿图优化也是一样，根据不同的情况去采用不同的优化方式，最后进行全局的重定位。
八、EXPERIMENTAL RESULTS（实验结果）
对整个系统的评价分为:
- EuRoC中的单会话实验:11个序列中的每一个都被处理以产生一幅地图，使用四种传感器配置:Monocular、MonocularInertial、Stereo和Stereo-
inertial。 - 单目和立体视觉惯性SLAM与鱼眼相机的性能，在具有挑战性的TUM VI基准测试中。
- 两个数据集的多会话实验。
在现场，我们通常使用RMS ATE测量精度[81]，在纯单眼情况下使用Sim(3)变换将估计的轨迹与地面真实情况对齐，在其他传感器配置中使用SE(3)变换。使用Sim(3)对准的s计算尺度误差，如|1−s|。所有的实验都是在Intel Core i7-7700 CPU上进行的，3.6GHz, 32gb内存，只使用CPU。
1.Single-session SLAM on EuRoC
表二比较了ORB-SLAM3使用四种传感器配置与最先进的相关系统的性能。我们报告的值是执行10次后的中位数。如表所示，ORB-SLAM3在所有传感器配置中都比文献中可用的最佳系统实现了更精确的结果，在大多数情况下都有很大的差距。
在单目和立体配置中，我们的系统比ORB-SLAM2更精确，这是因为我们的位置识别算法更早地关闭循环，并提供更多的中期匹配。有趣的是，下一个最好的结果是由同样使用中期匹配的DSM获得的，尽管它没有闭合循环。
在单目惯性构型下，与MCSKF、OKVIS和ROVIO相比，ORB-SLAM3的精度提高了5 - 10倍，比VI-DSO和VINSMono的精度提高了一倍以上，再次显示了中长期数据关联的优势。与ORB-SLAM VI相比，我们的新型快速IMU初始化允许ORB-SLAM3在几秒钟内校准惯性传感器，并从一开始就使用它，能够完成所有EuRoC序列，并获得更好的精度。
在立体惯性构型中，ORB-SLAM3比Kimera和VINS-Fusion精确3到4倍。它的精度只有最近的玄武岩，作为一个原生的立体惯性系统，不能完成V203序列，其中一个相机的一些帧缺失。对比我们的单目惯性和立体惯性系统，后者在大多数情况下表现更好。只有对两个Machine Hall (MH)序列的精度较低。我们假设，更大的深度场景MH序列可能导致不准确的立体三角测量，因此不精确的比例尺。
为了总结性能，我们给出了每种传感器配置执行10次的中值。对于一个鲁棒系统，中位数准确地表示了系统的行为。但非鲁棒系统将显示其结果的高方差。这可以用图4来分析，图4用颜色显示了10次执行中每一次获得的错误。与发表的DSO、ROVIO和VI-DSO的数据进行比较，证实了我们方法的优越性。
在纯粹的视觉配置中，多地图系统通过在跟踪丢失时创建一个新的地图来增加对快速运动的鲁棒性，这个新地图稍后会与全局地图合并。这可以在序列V103单眼和V203立体中看到，ORB-SLAM2无法解决，我们的系统在大多数执行中都成功解决了。正如预期的那样，由于其更快的特征初始化，立体视觉比单眼视觉更具鲁棒性，另外还有一个优势，即可以估算出真实的尺度。
然而，我们的新视觉-惯性SLAM系统在单目和立体构型上都实现了鲁棒性的巨大飞跃。立体惯性系统比单目惯性系统有非常小的优势，特别是在最具挑战性的V203序列中。
我们可以得出结论，与纯视觉方案相比，惯性集成不仅提高了精度，降低了平均ATE误差，而且使系统具有良好的鲁棒性，性能更加稳定。
在这种视觉惯性的模式下和纯视觉相比不仅增加了精度，降低了终值的ATE误差，并且系统更加鲁棒，得到更稳定性能。
2.Visual-Inertial SLAM on TUM-VI Benchmark
tumi - vi数据集包含6种不同环境下的28个序列，使用手持鱼眼立体惯性钻机记录。轨迹的地面真相只在序列的开始和结束时可用，对大多数序列来说，这只代表了整个轨迹的很小一部分。数据集中的许多序列不包含循环。即使起始点和结束点在同一房间，视点方向相反，位置识别也无法检测到任何公共区域。利用这一地面真相进行评估相当于测量沿整个轨迹的累积漂移。
上表2中的左侧四列分别是单目、双目、单目惯性、双目惯性。加粗数据的表示最佳，数列为不同数据集，可以看到SLAM3有很多性能最佳。
在应用CLAHE均衡化来解决数据集中发现的曝光不足和过度后，我们在单眼惯性设置中提取了每幅图像1500个ORB点，在立体惯性设置中提取了每幅图像1000个ORB点。对于户外序列，我们的系统与来自多云天空的非常远的点斗争，这是非常明显的鱼眼相机。这些点可能有慢动作，可以引入漂移的相机姿势。为了防止这种情况，我们放弃距离当前相机姿势20米以上的点，只对户外序列。一个更复杂的解决方案是使用图像分割算法来检测和丢弃天空。
得到的结果与表三中文献中最相关的系统进行了比较，这清楚地显示了ORB-SLAM3在单目惯性和立体惯性方面的优势。最接近的系统是VINS-Mono和玄武岩，它们本质上是带有环路闭包的视觉-惯性里程测量系统，缺少中期数据关联。
通过对系统性能的详细分析，该系统在中小型室内环境、房间和走廊序列中误差最小，大多数误差在10 cm以下。在这些轨迹中，系统不断地重新访问和重用以前绘制的区域，这是ORB-SLAM3的主要优势之一。此外，跟踪点通常距离小于5米，这使得估计惯性参数更容易，防止它们发散。
在最长达900米的室内序列中，大多数跟踪点都比较接近，ORB-SLAM3除了一个接近5米的序列外，误差在1米左右。相反，在一些较长的户外序列中，近距离视觉特征的缺乏可能会导致惯性参数的漂移，特别是尺度和加速度计偏差，这将导致10到70米的误差。尽管如此，ORB-SLAM3是户外序列中表现最好的系统。
这个数据集还包含三个真正具有挑战性的幻灯片序列，在这些序列中，用户通过一个几乎完全没有视觉特征的黑色管状幻灯片下降。在这种情况下，一个纯粹的视觉系统会丢失，但我们的视觉-惯性系统能够在竞争误差下处理整个序列，即使不能检测到环路闭包。有趣的是，使用Lukas-Kanade跟踪特征的VINSMono和BASALT在某些序列中获得了比ORB更好的精度。
表四:由ORB-SLAM3在房间序列中使用四种传感器配置获得的RMS A TE (m)，代表AR/VR场景(执行3次的中位数)。
与ORB描述符匹配的SLAM3。
最后，房间序列可以代表典型的AR/VR应用，用户在一个小环境中使用手持或头戴设备移动。对于这些序列，地面真相可用于整个轨迹。表三显示，ORB-SLAM3明显比竞争对手的方法更准确。获得的结果使用我们的四个传感器配置表四、更好的准确性比较纯粹的单眼与音响相比仅仅是明显的:单眼的解决方案是up-to-scale并与7自由度,与真实而立体提供真实的规模,并与6自由度。利用单目惯性，我们进一步降低了平均RMS ATE误差接近1 cm，也获得了真实的尺度。最后，我们的立体惯性SLAM将误差控制在1厘米以下，使其成为AR/VR应用的最佳选择。
3.Multi-session SLAM（多端SLAM）
多段：同一个大场景下不同数据集,代码中有不同数据集同时去跑会效果比单个好，毕竟数据多。
EuRoC数据集包含3个环境中的多个会话:5个在Machine Hall, 3个在Vicon1, 3个在Vicon2。为了测试ORB-SLAM3的多会话性能，我们依次处理每个环境对应的所有会话。在相同的环境中，每条轨迹都具有相同的地面真相和相同的世界参考，这允许执行单一的全球校准来计算a TE。
每个房间的第一个序列提供了一个初始地图。处理下面的序列从创建一个新的活动映射开始，这个活动映射会很快与以前会话的映射合并，从那时起，ORB-SLAM3就可以从重用以前的映射中获益。
表V报道全球multi-session RMS吃四个传感器配置三个房间,比较两个只有multi-session结果发表在EuRoC数据集:CCM-SLAM[71]在MH01-MH03纯单眼财报,和VINS-Mono[7]的五机大厅序列,使用monocular-inertial。在这两种情况下，ORBSLAM3的准确度都比其他方法高出一倍以上。在vin - mono的情况下，ORB-SLAM3在单会话下的精度提高了2.6，在多会话下的精度提高了3.2倍，显示了我们的地图合并操作的优越性。
表V: EuRoC数据集上的多会话RMS A TE (m)。对于CCM-SLAM和VINS，我们展示了每个系统作者报告的结果。我们的值是5次执行的中值，将轨迹与处理过的GT对齐。
将这些多会话性能与表二报告的单会话结果进行比较，最显著的差异是，由于利用了之前的地图，多会话单目和立体SLAM能够稳健地处理难度序列V103和V203。我们还在tumv - vi数据集上进行了多会话实验。图5显示了在TUM大楼内处理几个序列后的结果。在本例中，小房间序列提供了较长序列中所缺少的循环闭包，将所有错误降到厘米级别。虽然在房间外无法获得地面真相，但将该数据与[82]中公布的数据进行比较，清楚地表明了我们的观点:我们multi-session slam系统比现有的视觉惯性里程计系统具有更高的精度。图6进一步说明了这一点。虽然ORB-SLAM3在户外s1的立体惯性单次振动处理中等级较高，但仍然存在显著的漂移(≈60 m)，而如果在magistrale2之后以多session的方式处理户外s1，这种漂移明显减少，最终得到的图也更加精确。
4. Computing Time
表六总结了跟踪和映射线程中主要操作的运行时间，显示我们的系统能够以30-40帧和3-6关键帧/秒的速度实时运行。惯性部分在跟踪过程中花费的时间可以忽略不计，实际上可以使系统更有效，因为可以安全地降低帧率。在映射线程中，在惯性局部BA中，用较小的关键帧数补偿了每关键帧较多的变量数，实现了较好的精度，但运行时间相似。由于跟踪和映射线程总是在活动映射中工作，所以多映射不会引入显著的开销。
表VII总结了循环关闭和映射合并的主要步骤的运行时间。这种新的位置识别方法每关键帧只需要10毫秒。合并和循环关闭的时间保持在1秒以下，只运行一个姿势图优化。对于循环的关闭，执行完整的bundle调整可能会将时间增加到数秒，这取决于所涉及的地图的大小。在任何情况下，由于两个操作都在一个单独的线程中执行(图1)，它们不会干扰系统其余部分的实时性能。视觉-惯性系统只执行两次地图合并以连接三个序列，而视觉系统执行一些额外的合并以从跟踪损失中恢复。由于其较低的漂移，视觉惯性系统也执行更少的闭环操作相比纯视觉系统。
虽然这很有趣，但是我们没有将运行时间与其他系统进行比较，因为这将需要超出本工作范围的大量工作。
九、CONCLUSIONS（结论）
在[2]-[4]的基础上，我们推出了ORB-SLAM3，这是用于视觉、视觉惯性和多回合SLAM的最完整的开源库，具有单眼、立体、RGB-D、pin-hole和鱼眼相机。除了我们的主要贡献从集成库本身，是快速和准确的IMU初始化技术，以及多会话地图合并功能，依赖于一种新的地方识别技术，提高回忆。
实验结果表明，ORB-SLAM3是首个能够有效利用短期、中期、长期和多地图数据关联的视觉和视惯性系统，达到了现有系统无法达到的精度水平。我们的结果还表明，就准确性而言，使用所有这些类型的数据关联的能力胜过其他选择，例如使用直接方法而不是特征，或为局部BA执行关键帧边缘化，而不是像我们所做的那样假设一个静态关键帧的外部集。双目惯性效果最好
《ORB-SLAM3》的主要失败案例是低纹理环境。直接方法对低纹理更有鲁棒性，但仅限于短期和中期数据关联。另一方面，匹配特征描述符成功地解决了长期和多地图数据的关联，但在跟踪方面似乎不如使用光度信息的Lucas-Kanade稳健。一个有趣的研究方向可能是开发足以解决这四个数据关联问题的光度测定技术。我们目前正在探索利用人体内部的内窥镜图像构建地图的想法。
关于四种不同的传感器配置，毫无疑问，立体惯性SLAM提供了最稳健、最精确的解决方案。此外，惯性传感器允许在IMU速率下估计姿态，这是高于帧速率的数量级，这是一些用例的关键特征。对于立体相机由于体积、成本或处理要求较高而不受欢迎的应用程序，您可以使用单目惯性，而不会丢失很多鲁棒性和准确性。只需要记住，在勘探过程中单纯的旋转是不允许估计深度的。纯旋转无法估测深度
在运动缓慢或没有滚动和俯仰旋转的应用程序中，例如在平坦区域的汽车，IMU传感器可能很难初始化。在这些情况下，如果可能，使用立体SLAM。此外，最近利用CNN对单个图像进行深度估计的进展为可靠和真实尺度的单目SLAM提供了良好的前景，至少在训练CNN的同一类型环境中是如此。深度学习方向可以探究
十、REFERENCES（致谢）
此处为大佬照片合集
