【论文阅读】ORB-SLAM: a Versatile and Accurate Monocular SLAM System_orbslam论文

论文翻译
论文讲解

一、系统总览

在这部分中简单介绍了一下整个系统的流程。

A、特征选择

论文的一个主要思想，就是在制图和追踪过程中的特征点在重定位和回环检测的时候也会使用，从而实现一个更加高效的系统。在ORBSLAM中使用的特征描述子是ORB描述子，这种描述子计算和匹配的速度很快，同时具有旋转不变性，从而让BA优化的结果精度更高。

B、三个线程：追踪、局部建图和回环检测

图中所示的是整个系统的流程，可以看见整个系统包括三个线程。
跟踪线程主要负责随着每一帧的增加，一帧一帧地计算相机的位姿，并且根据计算出来的位姿，来确定什么时候将一个帧作为新的关键帧。如果跟踪由于光照或者大幅度的运动而丢失，就转而通过全局重定位进行恢复。获取相机位姿的初始位姿之后，就可以利用系统一直维护的共视图等工具，将局部地图中的点进行一个投影，利用这些投影关系再对位姿进行一个优化，最后由该线程根据位姿的变化幅度来确定是否要作为一个新的关键帧。

局部地图线程是负责进行新的关键帧的处理，并且要进行局部的BA优化。同时这个线程也会维护关键帧的数目，从而起到一个限制计算量的作用。可以看出，跟踪线程是对全部帧都进行处理，而到了局部建图线程，就变成了只对关键帧做处理，也就是说经过tracking之后，就只有关键帧流传到了后续的mapping中。在这个线程中会对未匹配过的ORB特征点在共视图中临近的关键帧内寻找匹配关系，从而完成三角化

最后的回环检测线程看名字就能知道意思了，就是对新来的关键帧进行检测，看是否出现了回环的信息，如果出现就利用回环信息去校正累积误差，从而对全局的地图进行一次优化。优化时使用g2o库进行Levenverg-Marquardt算法执行全局优化。

C、地图点、关键帧以及他们的选择

地图点是恢复了3d信息的点，对应的是真实场景下的一个点的坐标，每个地图点要存储下面的一系列信息：

而关键帧没什么好说的，需要保存下面的信息：

地图点和关键帧都通过一些选择和删除机制去维持，从而保证维护的局部场景是有限大小的，不至于过大导致计算过于复杂。在这一步中，通过这种严格的筛选机制，会让点的数目减少很多，但也让误匹配或者噪声点的数目明显减少。

D、共视图和本质图

共视信息指的是一个点可以被两个帧同时看见，而共视图是基于这个共视关系产生的一种无向有权图工具，共视图用节点表示关键帧，关键帧之间如果存在至少十五个点满足共视关系，就给这两个点增加一条边，并用共视关系的点的数目来表示边的权重。可见共视图描述了帧与帧之间特征的联系，在后面的优化过程中会用到这个图。

本质图则是用于回环检测的相关操作，一般在校正回环的时候，会通过图优化的方法将回环的偏差均匀到图中的每一部分，但是共视图是一个局部的关系，并不会维护很持久的关系，这时候就需要用本质图来记录持久的关系进而用于回环检测。本质图保留了所有的关键帧，每个关键帧都存储为一个节点，但是在本质图中节点的数目会更少，这主要是为了保持一个比较强的网络，从而获得精准的结果。它的边包含生成树的连接关系、形成闭环的连接关系、共视关系非常好的关键帧连接关系

此外，系统还增量式地维护一个生成树，从第一个关键帧开始，它连接了边缘数量最少的共视图的子图，也就是关键帧与附近共视关系最好的关键帧，也是一种父子关系。
共视图、本质图和生成树讲解
也就是说，共视图是内容最丰富的，只要满足十五个点的共视关系就有一条边，而生成树是内容最简单的，一个节点只选择共视关系最大的点作为父节点，本质图介于二者之间，只保留了共视关系很好的边以及一些额外的信息。生成树是本质图的一部分，而本质图又可以看作是共视图的子集。

共视图、本质图和生成树是ORBSLAM的一套十分重要的工具。如果对比VINS，ORBSLAM里面使用的共视图就相当于VINS里的滑动窗口，本质上还是用于局部的优化，但是换了一种方法。论文上画的共视图表达效果并不是很好，下面这张图表示的共视图更方便理解：

虽然这张图和共视图的定义有所偏差，但是在理解上显然是这张图更方便。共视图描述了对于地图中的一部分点的共同观察关系，简单理解的话，不考虑回环的情况下，对于共视关系，其内部其实隐含一些观测上的限制关系，而这部分限制关系，就是进行局部优化所需要的限制条件，其实VINS的滑动窗口也可以这样去理解，滑动窗口本质上就是当前关键帧的周围的一些帧，相邻的帧的共视关系显然是很强的，也就是说滑动窗口是用一个临近范围代替了维护共视图，但这个窗口的划定显然不如共视图灵活，就好比十人核酸里有一个阳性就把十个人全拉去红码隔离，在滑动窗口内肯定有非共视关系的点，这部分在优化时按道理是没什么大用的，此外，由于窗口只限制在周围一定范围内，对于超过这个范围的共视关系就不纳入考虑了，所以一些约束关系被漏掉了，这种情况显然要劣与ORBSLAM的共视图。虽然还没有看过代码实现，但是个人理解ORBSLAM在这里使用共视图，就是为了利用共视关系来进行局部优化，从而代替VINS中的滑动窗口计算残差的机制。

共视图用于局部优化，那么本质图对应的就是全局优化和回环优化，前面也提到了，回环的时候会将偏差均摊到很多帧中，在这种情况下，规模不是很大的共视图就不够用了，就需要一个范围更大的工具，这时候就出现了本质图，本质图的作用就是在矫正闭环的时候，用相似变换来矫正尺度的漂移，从而把闭环的误差均摊在本质图中，相较于共视图，本质图会更加稀疏，原本在共视图中，共视点的数量超过15个就添加一条边，而在本质图，这个阈值被提高到了100，也就是只保留共视关系特别好的帧之间的连接关系，所以边的数目显著变少。对比一般的全局BA优化，本质图的优化可以快速收敛而且结果更加准确，但是耗时会较多，个人感觉收敛快速可能与前面使用共视图速度快是相通的，也是借用共视关系来增强了关键帧之间的联系。

最后生成树的部分，网上介绍这个的资料相比于共视图和本质图少了很多。可以明确的是生成树的关键在于关键帧之间的父子关系，查看生成树的建立过程，生成树的连接的更新是放在共视图生成之后的，个人感觉生成树的连接过程，像是在根据共视关系的强弱，对节点的父子关系进行调整，生成树（Spanning tree）用来管理各关键帧之间的关系，每个帧都有一个父节点和子节点，节点为关键帧，每个节点只跟各自的父节点和子节点相连，与其他关键帧不连接。一个节点只可以有一个父节点，但可以有多个子节点，生成树可以看作是一个父节点组成的连接。每个帧会将自己共视关系最强的帧作为自己的父节点，在关键帧一生成的时候就会连接上去，根据博客的描述可以看出，在与生成树相关的代码中，有更换父节点的函数，那么在这部分，应该是有一个在优化或者更新的过程中，根据共视关系的强度变化对节点的一个迁移。生成树会在更新局部关键帧以及闭环矫正的时候用到。

最近在看ORBSLAM代码讲解的时候听大佬讲解了一下生成树的思路，抛开ORBSLAM的背景不看，这里的生成树本质上就是图形式的共视图的一个最小生成树，在优化的过程中，我们必然希望开销最小，也就是说在保证每个节点都被优化到的前提下，尽可能减小优化的参数内容，放回到ORBSLAM的背景之下，就是让每个节点的位姿都被优化到，但是要让边也就是帧之间的共视关系尽可能少，因为一个节点在共视图里可以和多个节点产生联系，所以在这里采用的做法就是只保留联系最强的边，也就是保留权值最大的节点或者说共视关系最强的节点，由此就构成了生成树，N个节点的共视图需要N-1条边构成最小生成树，也就是这里的生成树。

还有一点是关于这个生成树的图示，乍一看这属于图而不是树，但个人感觉是一个画法的问题，本身树和图就是相通的，这种画法是考虑了可读性的结果，在实际代码中，应该就是一个树的方法，每次找到共视关系最近的一个节点然后挂靠下去，如果按照一般的关键帧延伸方法，这棵生成树的增长应该是不均匀的，或者说是向着生成树单侧不断生长的，所以会演变成一个图，那么出现回环也就表现为这棵树出现了成环的现象。

E、基于图像词袋模型的位置识别

在进行回环检测过程中，使用DBoW2词袋模型进行回环的检测，关于词袋模型的内容前面SLAM十四讲里面有，这里不多赘述了。

四、自动地图初始化

地图初始化是一个很关键的问题，因为地图初始化是缺少之前信息的，所以不能直接用后面跟踪建图的方法进行。这篇论文提出的时候，一种初始化方法是在初始化的时候人工选择两个效果比较好的帧去做初始化，所以这篇论文主要是对这一点做了改进，因此可以看见论文用了automatic全自动这个词。
初始化的主要目的是在SLAM过程一开始的时候计算两帧之间的位姿变化，之后再三角化得到一组初始的地图点，前面在slam的前端部分也提到过，对于一些存在平面的场景，可以通过计算关键矩阵或者基本矩阵来恢复旋转平移，但是会存在不小的误差，这种情况可以通过计算单应矩阵来得到更加准确的旋转平移。而在ORBSLAM中，作者提出的自动化计算，就是计算单应矩阵和基本矩阵，根据效果做一个启发式的选择。

在初始化的过程中，首先提取帧之间的对应关系，即提取ORB特征点，之后在参考帧和当前帧之间做匹配，找出匹配的特征点，如果特征点对不足就换一个参考帧。之后用平行的两个线程，去计算单应矩阵H和基本矩阵F：

这两个式子是根据对极几何得到的，在SLAM十四讲前端的部分里面也讲到过，这个计算过程是一个迭代的过程，两种模型的迭代次数设置为一样的，每次迭代都对一个模型计算一个Sm值，用于判断哪一个更优秀：

对于这两个式子，Sm值计算过程中是对每个特征都计算一次，式子中的M表示两种模型，也就是用单应矩阵和基本矩阵都去带入计算一遍，得到两个分值。Tm是一个分类阈值，用于确定是不是无效数据，超过了阈值就设置为0。d表示的是对称转移误差，常用的是重投影误差，这里的对称转移误差可以看作是扩展版的重投影误差，讲解的链接如下：
重投影误差与对称转移误差

关于这个链接里面放的图，个人不确定是老外的ppt写错了还是个人理解错了，里面的重投影误差是两部分组成的，而SLAM十四讲里面重投影误差就是投影点到检测点的距离，顺着ppt我查到了维基百科，链接里关于重投影误差的公式就来自维基百科，其叙述如下：

可以看出，在维基百科中定义的重投影误差确实是两部分组成的，但是其依赖于perfectly matched points和imperfect correspondences，而且在优化的式子也可以看出，优化的量不仅包含了位姿T：

所以这里就不要太纠结链接中的重投影误差了，里面的对称转移误差是没错的，匹配点互相投影然后距离相加，或者可以看作两个重投影误差的和。

回到得分本身，得到了两个模型的分值之后，我们就用这个分值去计算一个Rh值，根据Rh值的大小去确定使用哪一个模型：

如果Rh大于0.45就使用单应矩阵，否则使用基本矩阵。确定使用哪一个矩阵后，就利用矩阵恢复出旋转和平移，从而得到位姿。最后使用一个BA优化对初始化的结果进行优化。
采用这种分开处理的策略，主要还是考虑了场景的多样性，对于有许多平面的场景或者视差比较小的场景，显然计算单应矩阵会更好，反过来，对于一般的场景，使用基本矩阵就足够了，所以用这种启发式的方法来判断哪一种更合适。

对于这个初始化的过程做一个小总结，首先提取ORB特征点，找出两帧之间的特征点匹配关系，之后计算单应矩阵和基本矩阵，根据RH值来确定使用哪一个模型，恢复出旋转平移，得到位姿，最后使用一个BA优化对结果进行优化。与之相对，VINS采用的初始化策略是利用滑动窗口，找出窗口内的两帧效果比较好的帧，利用五点法恢复位姿变换，从而三角化出地图点的信息，利用这些3d信息使用pnp恢复出窗口内剩下的位姿，最后补一个BA优化去优化这些位姿信息。

五、跟踪

跟踪这一部分，主要是为了在运行过程中获得每一帧的位姿信息，同时还要确定什么时候将当前帧作为一个新的关键帧。

A、ORB特征提取

在跟踪过程中使用的特征提取利用了八层的图像金字塔，在每一层提取FAST角点，同时为了保证分布的均匀，每一层都划分为网格，每个网格至少提取5个角点，这种均匀分布的策略称为均匀化，如果角点数量不够就提高阈值。均匀化之后对保留下来的特征点，计算其旋转和ORB描述子，保存后用于后续的帧间关键点匹配。

B、以上一帧为基础的位姿估计

如果上一帧能够正确跟踪，这种情况下先使用上一帧作为基础进行位姿估计，具体做法是使用匀速运动模型，用上一帧的速率去估计一个相机位置，利用这个相机位姿，给特征点匹配一定的引导作用，这里的引导作用指的是用匀速运动模型得到一个假定的位姿变换T，用这个T将一些点投影到下一帧，在投影位置附近找点，看是否与之匹配，如果匹配的数量足够多，就认为这个假定的位姿变换T是比较好的，反之则在上一帧中找到更多的3D-2D匹配点，利用BA(Bundle Adjustment)进行位姿的优化。

对于这种跟踪方式，个人的理解就是拿上一帧的位姿变换作为这一帧的，这主要是因为一般来说相机的拍摄频率很大，也就是说两张图之间的时间很短，所以让帧间的位姿变换其实不会很大，可以认为是匀速运动模型，因此采用上一帧的位姿变换作为初值，然后通过检验的方法来判断这个位姿是不是合适，也就是上图中重投影然后检测的方法，只要偏离不大的点的数目在可接受范围内，我们就认为这个估计的位姿是可靠的，之后再用这个值作为初值，进行优化，在优化之后剔除外点，若剩余的匹配依然大于等于十对，就认为跟踪成功。

在这里补充另外的一个位姿估计方法，在论文里面貌似没有体现，这里应该叫做参考关键帧的跟踪。这种跟踪策略可以看作是上面以上一帧为基准的跟踪策略的延伸，主要是用在没有速度信息、刚完成重定位这种恒速模型失效的情况下。

可以看出，与上面方法最大的不同在于，左下角将上一帧换成了参考关键帧，此外帧之间的特征对应关系也变了，从原来的投影变成了借助词袋模型树来实现。借助词袋模型，本质上是想节省暴力匹配的时间，具体来说这里的匹配方法是利用当前帧和参考关键帧在同一节点下的特征点，不是暴力匹配的方法，而是用词袋模型来加速这个过程，具体的过程如下：

C、基于全局重定位的位姿估计

如果上一帧的跟踪失败，就需要使用这种方法，这时上一帧已经不可靠了，就需要利用词袋模型，在图像数据库中去寻找一个最匹配的关键帧（匹配的特征超过15个），利用PnP恢复出当前帧的位姿，利用BA优化对位姿进行优化。需要注意的是，这种情况出现的次数是很少的，其主要的思想是在恒速模型跟踪和关键帧跟踪都失效的时候，就从之前存储的关键帧中，找一个最相似的关键帧，利用这一帧来找出位姿信息，这时候也就没有办法投机取巧了，只能用传统的EPnP来求解初始位姿，之后通过最小化重投影误差的方法，来进行位姿的优化。如果这一步还不成功，就认为彻底跟踪失败，需要重新从初始化那里开始运行。

对于这一步，个人感觉之所以要从过去的关键帧中找最相似的关键帧，主要还是要保证有足够多的匹配关键点。可以确定的是，如果执行到了重定位的位姿估计，那么肯定是前面的帧或者说前面的关键帧相对于这一帧产生了很大的位姿变化，可能是因为光照、运动等因素导致的，出现这一点就代表我们已经没办法用临近信息来估计帧间位姿变化了，这时候我们就寄希望于更旧的信息，因为信息比较旧，所以帧间位姿变换相对会更大，这种情况下我们想要一个比较准确的位姿变化，最好的办法就是有更多的匹配特征点，匹配点数目多，意味着计算重投影误差的时候，提供优化信息的途径也就更多，优化的结果也会更准确，虽然耗费时间，但是比直接重新初始化的开销要小，就是处于这种考虑，所以才有这个重定位的位姿估计。

D、局部地图位姿优化

经过BC两步，我们得到了相机位姿一些特征匹配关系，但是这三步的位姿，或多或少都有偏差，现在就是要利用共视关系，对局部的地图做优化，这里的局部地图，包括两部分，一个是共享地图点的关键帧K1，这部分可以看作是与当前关键帧存在共视关系的关键帧，另一部分则是共视图中与K1中的帧临近的帧，我们也可以将其看作与当前帧二级相邻。也就是说，优化的部分包括下图的红色圈和绿色圈的部分：

在得到K1和K2之后，就会对其中的特征点进行筛选，筛选过程如下：

最后利用符合条件的共视点去做优化。

E、新关键帧的筛选

符合下面条件的帧会被作为新的关键帧存储起来：

六、局部制图

经过上面一步，关键帧已经被筛选了出来并且得到了一个比较准确的位姿变换，接下来就是利用这些关键帧和位姿的信息去建立局部地图。

A、关键帧的插入

建图就需要使用我们第三部分提到的共视图，新来的一个关键帧会用一个节点的形式插入到共视图中，之后根据共视关系给新加入的节点加上带有权值的边，同时更新生成树上的内容，之后计算当前帧的词袋模型，将计算出来的单词存入数据库中方便后续的查询。

B、旧地图点的筛选

这部分是在筛选恢复出深度的地图点，由于地图点要进行跟踪，所以筛选一些优秀的具有代表性的地图点效果会更好。地图点要符合下面的条件：

只要地图点通过了上面两个条件，就可以进行跟踪，当少于三个共视关系的时候就会被删除。

C、新地图点的插入

对于匹配的特征点，我们可以根据共视关系通过三角化恢复深度。首先利用共视图，找出邻接的一些关键帧，找出匹配的特征点并进行三角化恢复深度，计算地图点的相关属性后，查看这个地图点是不是已经被之前的关键帧查找到过，也就是这个地图点是不是已经出现过了，如果没有出现则直接插入，如果出现过则需要确定以哪一个为准，具体做法如下：

D、局部BA优化

这一部分是优化当前处理的关键帧、共视图中的关键帧以及被其余关键帧观察到的地图点，优化局部的建图结果。共视图中与当前关键帧连接的其他关键帧，以及这些关键帧观测到的地图点进行优化，所有其他能够观测到这些点的关键帧但没有与当前关键帧产生公式关系的会被保留在优化线程中，并不参与优化过程。

E、局部关键帧的剔除

关键帧过多会导致共视图比较复杂，进而导致局部建图的过程计算开销很大，所以这里采用这种剔除策略，在保证共视关系足够的前提下，删除冗余的关键帧，从而限制计算复杂度不至于太离谱。相当于对共视关系的大小做了限制，因为如果按照共视关系的定义，存在整个地图都被一个共视关系传递包含的可能，所以就需要用一些剔除策略来限制大小，比如VINS的边缘化策略，这里使用如下的策略：

继续拿VINS做对比，其实这一部分加上上一部分刚好对应了VINS中完整的滑动窗口机制，ORBSLAM采用的共视图实际上和滑动窗口的思路是一样的，VINS使用边缘化去限制窗口内关键帧的数目并为优化产生先验信息，而在ORBSLAM中则利用共视关系去限制关键帧的数量。此外在优化上二者也存在一些区别，VINS是用三部分优化：重投影误差、IMU误差和先验信息，而ORBSLAM中体现在论文中的就是只使用了一个BA优化，而没有VINS那样再这里大费篇幅。

七、回环检测

回环检测的思路其实vins和ORBSLAM都差不多，都是利用词袋模型去找回环，不同的地方在于由于ORBSLAM使用了共视图和本质图，所以会有一些细节上的区别。

回环检测的过程中，先利用共视图确定所有的相邻的关键帧，利用这些关键帧去寻找一个最小的相似度，之后在关键帧数据库中筛选相似度不小于这个最小相似度的关键帧作为候选的回环帧，全部搜索完之后，再在候选的回环帧中筛选，如果候选帧被连续三个帧的搜索都认为是候选才真正作为回环。
这里有点绕，简单来说就是来一个帧后，先对相邻的关键帧做一次筛选，得到一个最小匹配值，之后筛选之前全部的关键帧，如果有大于这个最小匹配值就设为候选回环帧，如果对于接下来的连续两个帧，都认为这个候选回环帧是候选回环帧，那么就认为确实出现了回环。

检测出回环之后，就利用回环这个信息，去计算相似变换，主要是利用RANSAC来迭代计算变换矩阵，如果匹配的地图点大于40就认为匹配成功。

之后利用回环关系和变换矩阵对回环做优化，同时修正共视图和本质图。

对比vins和ORBSLAM可以看出，二者其实使用的都是图的结构去进行优化，vins使用了位姿图，利用回环边和顺序边去体现约束关系，而ORBSLAM则是用共视图和本质图，个人感觉这两个东西貌似就是一个东西，都是在检测出回环后，利用图去表现约束关系，从而进行全局优化。

八、整体结构回顾

最近看了大佬讲解代码的视频，顺着视频的思路再理顺一遍整个ORBSLAM的过程，链接地址如下：
https://www.bilibili.com/video/BV1bK4y197kB?p=1&vd_source=848be89944503c2ec88f53924ce8316a

对于整个ORBSLAM而言，可以区分为三大线程：跟踪（Tracking）、局部建图（Local Mapping）以及回环检测（Loop Closing）。在整个程序运行的一开始，按道理应该还有一个初始化线程，这部分被划分到跟踪线程里面了。

Tracking

对于相机传来的一帧图像，首先进行预处理部分，也就是提取ORB特征，之后如果处于SLAM刚运行的情况，就转而进行地图的初始化。在初始化的部分，主要是利用初始的几帧图像，完成特征点的匹配和三角化，这部分单独出来是因为整个SLAM过程刚刚开始运行，没有之前的地图点等内容，也就是说这个时候后续的跟踪等线程都是没法进行的，所以会出现这部分初始化模块。在初始化的过程中，会在传入第一帧的时候创建一个初始化器对象，而在得到符合要求的第二帧的时候，调用初始化器对象中的初始化函数，这里所谓的符合要求主要是帧的特征强度，一方面符合条件的第二帧特征点数目不能少于100个，另一方面第二帧和第一帧的匹配特征点数目不能少于100个，这主要是为了保证初始化过程中能够创建出足够多的地图点，防止后续第一轮跟踪过程中因为地图点不够而出现问题，如果顺序来的帧不符合这个条件，就换用下一帧并重复检测到符合条件为止。
在得到两帧符合条件的初始化帧之后，就在帧之间进行特征点匹配，并利用匹配关系三角化恢复特征点的深度信息，这部分点将作为初始信息，创建为地图点并加入到地图中。

对于已经初始化之后的SLAM过程，就不需要进入上面的地图初始化部分，而是直接进入跟踪线程的核心之一：位姿估计。这一部分本质上就是为了获得每一帧的位姿，注意这里是每一帧而不是每一个关键帧，在跟踪线程中的对象是所有帧而不是关键帧，甚至说在这里都还没有关键帧这个概念，在跟踪线程的最后才有了关键帧。如果按照一般的SLAM流程，这部分应该是对两帧做特征匹配然后对匹配点利用对极几何来恢复位姿，ORBSLAM在这里可以理解为投机取巧来优化了这个耗时的步骤。
具体来说，在这部分用三种估计位姿的策略来得到一个初始值，然后用初始值做优化，从而能够实现一个快速收敛的效果。三种策略分别是：恒速模型、参考关键帧和重定位。在速度信息明确的时候，利用恒速模型估计，如果跟踪失败就转用参考关键帧方式，如果还是跟踪失败，就换用重定位方式，如果三种方式都失败了，就直接挨卵，转而重新初始化。可以看出，ORBSLAM其实是希望程序使用恒速模型来进行这一步的位姿估计的，在实际运行代码的时候也是这样，其实在位姿估计这部分，大多数都是通过恒速模型来计算的，这其实要求运行使用的数据集必须很稳，或者说运动过程不是太复杂，否则这个恒速模型会失效，而一旦失效，采用其它模型估计的开销实际上是不小的。

恒速模型和它的名字一样，是假设相机采用恒定的速度进行运动，用参考关键帧结合速度信息，估计一个当前帧的位姿。参考关键帧，这里就直接看作上一个关键帧吧，差别不是很大。如果使用的是双目或者RGBD相机，在这部分还会有一个临时地图点的生成。现在我们得到了当前帧的一个估计的位姿，就可以利用这个位姿，将上一帧的地图点通过投影的方法，找到当前帧上的一个位置，在这个位置附近搜索，看是否有匹配的特征点，如果匹配点数目太少，就扩大搜索范围的半径再次搜索一次，得到足够数量的匹配点之后，就利用一个BA优化，只优化当前帧的位姿，之后利用优化的结果剔除一些匹配点中的错误结果，即剔除外点，最后只要匹配的特征点只要大于10对，就认为跟踪成功，也就是得到了当前帧的位姿。
对于这里的只优化位姿的BA优化，个人的理解就是用缩小搜索范围的方法，减少了匹配特征点所带来的时间开销，之后的优化，本质上还是利用匹配点来优化位姿，不清楚内部的优化细节是怎样的，但个人感觉应该还是将位姿作为优化对象，最小化类似于重投影误差这样的误差和，从而得到最优位姿。这里的位姿由于是利用恒速模型估计出来，如果特征点数目足够，那么收敛的速度也会很快，因此说采用恒速模型的速度会有提升。

在恒速模型失败的时候，会转而进行参考关键帧的跟踪，前面的恒速模型提到过，参考关键帧可以简单理解为上一个关键帧。在这里把这个参考关键帧展开说，其来源主要有两个：在跟踪线程中每次创建的新关键帧和跟踪局部建图模块中当前帧共视点最多的局部关键帧。这个参考关键帧是Tracking对象的成员变量，也就是说在整个SLAM过程中，这个参考关键帧只有一个，它所对应的就是与当前帧共视程度最好的一个关键帧，而由于上一次刚刚加入的新关键帧离创建不远，所以共视关系不会太差，进而当作参考关键帧来理解也偏差不大。
在基于参考关键帧的位姿估计这部分，首先要对当前帧和参考关键帧进行一个词袋匹配，也就是利用词袋进行描述子的匹配，这种方法不再需要以往一样去匹配每一对描述子，利用词袋里的树结构找到最相似的描述子，层层匹配，大大提高了匹配速度。如果词袋匹配数目太少，就认为跟踪失败。跟踪成功的情况下，以上一帧的位姿作为初始值，利用词袋匹配的结果，进行一次只优化位姿的BA优化，利用的方法依然是恒速模型那里的方法，之后剔除外点后如果匹配点大于十个认为跟踪成功。
对比参考关键帧的跟踪和恒速模型的跟踪可以看出，二者主要的区别在于特征点对应关系的获取，恒速模型利用估计出来的位姿找投影位置附近的点，可以说将暴力匹配的范围限制在了投影位置的附近，而参考关键帧的方法中，由于这种估计位姿的方法失效了，所以跑不了要进行匹配，但是这里采用词袋模型的方法来匹配，比一般的暴力匹配还要快一些，算是退而求其次了。

在上面两种策略都失败的情况下，就只能进行基于重定位的位姿预测了。一旦进入这种策略，表明上一帧已经不靠谱了，甚至于一个参考关键帧都足矣把定位给拉回来，这种情况采用的策略是利用当前帧的词袋模型，从关键帧数据库中，找出来一系列的候选参考关键帧，对每个候选参考关键帧，都利用上一种策略相同的Bow进行快速匹配，进行一个3D与2D的匹配，之后通过PNP得到一个位姿，用这个位姿作为初始值进行BA优化，优化后剔除外点，如果这样处理之后太少，则最多补充两次匹配+优化的循环，最后如果匹配数目大于50，则认为跟踪成功。
可以看出，一旦到了这第三种策略，复杂程度明显提升，一方面扩大了匹配的来源，从一帧变为了多帧，另一方面还扩大了匹配的范围，补充了最多两次的匹配。毕竟这是跟踪最后的救命稻草，一旦失败就只能重新初始化了。

总得来看，这部分跟踪，可以看作是以一种开销尽可能小的方法，去获得帧的位姿。经过这一步，每一帧的位姿信息就清楚了。进入到下一步跟踪局部地图，说实话这一个模块属实是有些难理解，因为它很容易和后面的局部建图混淆。
进入这部分之后，首先会对局部地图做一个更新，局部地图包括局部关键帧和局部地图点，所以这部分也就是对关键帧和地图点做一个更新。这里的更新是全部清除后重新赋值的策略，更新局部关键帧时会清空之前的局部关键帧列表，之后重新赋值，赋值的新内容包括：当前地图点的所有共视关键帧、共视关键帧的父子关键帧以及共视关键帧中共视关系前十的共视关键帧，简单来说就是凑了100个共视关系比较好的关键帧作为局部关键帧。同时在这里也会更新参考关键帧，也就是将局部关键帧中共视关系最好的帧作为新的参考关键帧。更新地图点则是先清空局部地图点列表，之后将局部关键帧中所有的地图点拿出来放在一起，就成了局部地图点。最后对于这部分的局部地图点，经过一定的筛选之后，使用一个BA优化再次对位姿做一个优化。
这一部分个人理解就是在做一个时间的推移，新的帧加入进来，必然观测结果要向着最新的状态去移动，在这部分中就表现为对局部关键帧和局部地图点做的一个更新。此外，仅仅经过跟踪的话，会出现地图点的减少，因为投影关系必然会让一部分地图点投影失败，如果不加入局部地图跟踪，就会让局部地图点持续减少，利用这一步也是为了将局部地图点再次补充回去，从而让其一直保持在一个稳定的数目上。也就是说，经过跟踪，我们得到了当前帧的位姿和部分地图点信息，而经过局部地图跟踪，我们一方面对位姿再次做了修正，另一方面补充了更详细的地图点信息。

最后进入到整个Tracking线程的最后一部分，也就是新关键帧的选择，这部分就没什么好说的了，没有哪个SLAM系统不会用到这部分，这里不再赘述。有一个要注意点点就是，这里创建新的关键帧，如果是单目相机，就只存储位姿、前后关系等内容，而对于双目相机和RGBD相机，还会多一个创建新地图点的内容。

对这个线程做一个小总结，在Tracking线程中，主要的任务就是获得每一帧的位姿，为了加速这个过程，ORBSLAM采用了三种策略，为了保证局部地图的时效性，ORBSLAM提出了局部地图跟踪的方法，从而实现了速度与精确度兼顾的效果。

Local Mapping

在建图线程中，其模块之间就不像Tracking线程那样分得一清二楚了，这里就直接对整个线程进行理顺。这个线程会死循环执行一个操作，就是建图，利用多线程的消息队列机制，只要关键帧队列里面出现了新的关键帧，就会取一个执行这个线程的内容。
对于一个新来的关键帧，首先会计算其关键帧的词袋向量，之后对于这个关键帧的每个地图点，如果是在创建关键帧的模块中新创建的，就放入地图点中并加一个待检测的符号，如果是旧的，就只更新一下观测关系。这里待检测的符号主要是为了保证地图点的优质性，一个地图点必须要被至少三个连续关键帧观测到才能被正式保留成为真正的地图点，如果不符合条件，这些地图点会被再次删除。
这里如果我没理解错，对于单目相机来说，这里并没有新创建的地图点，查看局部建图线程的ProcessNewKeyFrame代码可以发现，这里检查是否新创建的地图点的实现，实际上是遍历了一个for循环，遍历的内容实际上是创建关键帧时候加入的，但是在创建时，添加地图点是双目和RGBD的特权，所以在这里按道理是没有地图点的添加的，对于单目相机来说地图点的添加应该是放在局部建图线程中来实现的。所以按道理，对于单目相机来说，在这部分应该只做了一个共视关系的更新。

更新共视关系之后，就向地图内加入新的内容，也就是关键帧和地图点，主要是加入新的地图点，这里加入的方式，是将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配，从而三角化生成地图点。经过这一步之后，新的地图点就被创建出来了，下一步就是将地图点和关键帧做一个融合。这里融合分为正向融合和反向融合，前者是指将当前关键帧的地图点融合到各共视关键帧中，而后者则是将各共视关键帧的地图点融合到当前关键帧中。
在正向融合的过程中，可能会出现不同位置观测造成的偏差，比如一对多这种情况，解决方法是如果之前没有某个观测关系，就直接创建新的，如果已经有了，就删除一个共视关系较弱的观测关系。

融合之后，就对所有局部地图点和位姿做一个BA优化。最后，会对一些冗余的关键帧做一个剔除，冗余的标准是90%以上的地图点可以被其它的关键帧观测到，这样的关键帧就会被去除掉，但是新加入的关键帧不会在这一步中被删除。

地图点与关键帧

这里有一个很关键的问题，就是地图点、关键帧创建和删除的时机。

对于单目相机，地图点的创建也就是点的三角化，所以在初始化的时候一定会有地图点的创建，按照网上的资料，第二个地图点的创建是在创建新的关键帧的时候，但这里个人理解单目相机不会在这里创建新的地图点，代码里体现的创建只是针对非单目相机的，单目相机剩下一个地图点创建时机应该是在局部建图线程中的创建地图点，通过匹配进行三角化从而创建出来。删除地图点则对应局部建图线程中的剔除部分，观测关系不满足时会被删除。

关键帧的创建是在Tracking线程的最后，满足条件会被创建出来送入局部建图线程，删除则是在局部建图线程的最后，一旦检测到时冗余关键帧，即会被删除。

九、代码结构

代码结构的部分内容太多了，这里单独开一篇博客记录，点击