ORBSLAM三大线程之Tracking部分。

1. 概述

程序分为两种模式：SLAM模式和Localization模式，由变量mbOnlyTracking标记。SLAM模式中，三个线程全部都在工作，即在定位也在建图。而Localization模式中，只有Tracking线程在工作，即只定位，输出追踪结果（姿态），不会更新地图和关键帧。Localization模式主要用于已经有场景地图的情况下（在SLAM模式下完成建图后可以无缝切换到Localization模式）。Localization模式下追踪方法涉及到的关键函数是一样的，只是策略有所不同。

tracking中包含了5种状态：

  • SYSTEM_NOT_READY 系统没有准备好的状态（启动后加载配置文件和词典时）
  • NO_IMAGES_YET 当前无图像（图像复位、或者第一次运行时）
  • NOT_INITIALIZED 有图像但是没有完成初始化
  • OK 正常时候的工作状态
  • LOST 系统已经跟丢了的状态（TrackLocalMap时匹配成功的MapPoint太少）

tracking线程的目的有三个：获取精确的位姿，设置地图点，设置关键帧。程序中所有的内容都紧紧围绕这三个目的展开。

从流程上说，整个程序分为四大部分：初始化，初始追踪，精确追踪(TrackLocalMap)，加入关键帧。

整体流程如下图所示（深色表示后面有详细分析）：

2. 初始化

2.1 双目初始化

双目初始化包括了双目相机和RGBD相机（都把他们认为双目），双目初始化的目的是：设定初始位姿，获得初始关键帧，构造初始3D地图点。整个模块都是调用void Tracking::StereoInitialization()初始化。

  1. 设定初始位姿

    需要先加一个判断if(mCurrentFrame.N>500)整个函数只有在当前帧的特征点超过500的时候才会进行。初始位姿由Mat::eye构造，是单位变换矩阵。

  2. 构造初始关键帧

    构造的时候需要用参数mCurrentFrame，mpMap，mpKeyFrameDB（关键帧数据库用于重定位和回环）。构造完成后需要将关键帧添加到mpMap地图中。

  3. 特征点构造地图点

    1. 判断是否具有正深度

    2. 反投影获得特征点三维坐标

    3. 将3D点构造成地图点

      3D点是Mat类需要转化为MapPoint*

    4. 为构造的地图点MapPoint添加属性

      观测到该MapPoint的关键帧；该MapPoint的最佳描述子；该MapPoint的平均观测方向和深度范围

    5. 向地图中添加MapPoint

    6. 为关键帧添加特征点和地图点的对应关系

      将地图点和特征点序号添加到关键帧，并构建对应关系（哪个特征点可以观测到哪个3D点），f和e使用了同一个函数AddMapPoint但使用对象不同，e是对地图使用，f是对关键帧使用

  4. 在局部地图中添加该初始关键帧

    除了使用InsertKeyFrame函数插入关键帧以外，每次添加关键帧还需做一些额外操作。

    1. 首先需要将当前帧变上一帧（上一帧=当前帧，上一关键帧ID=当前帧ID，上一关键帧=当前帧）
    2. 然后需要添加到局部关键帧集合和局部地图点集合，并设置参考关键帧（关键帧的参考关键帧就是自己）
    3. 把当前（最新的）局部MapPoints作为ReferenceMapPoints（画图用）

2.2 单目初始化

单目的SLAM系统需要进行初始化，因为单帧图像数据并不能获取深度信息，也不能生成初始的地图。

早期的MonoSLAM，系统初始化利用一个已知尺寸的平面矩形实现，将相机摆放在该矩形前已知距离的地方，利用平面矩形的四个角点计算初始位姿。

单目SLAM地图初始化的目标是构建初始的三维点云。由于不能仅仅从单帧得到深度信息，因此需要从图像序列中选取两帧以上的图像，估计摄像机姿态并重建出初始的三维点云。

在ORB-SLAM中，作者并行计算基本矩阵和单应矩阵（用RANSAC方法），并评估两种方法的对称传输误差来选择合适的模型。完成之后，就会进行适当的分解，恢复出相机的位姿，并三角化生成初始地图点，最后通BA调整优化地图。如果选择的模型导致跟踪质量差，或者图像上的特征匹配较少，初始化就会迅速被系统丢弃，重新进行初始化，这保证了初始化的可靠性。

2.2.1 初始化的基本流程

Track线程中的Tracking::MonocularInitialization()，这个函数完成了单目的初始化，并且初始化了地图。

  1. 构建初始器，并选择初始器的第一关键帧

    如果初始器还未生成，则开始构建初始器。否则进入第二步。构建初始器：选取一帧提取到的特征点大数目于100的图像帧，构建初始器Initializer,并将当前帧图像设为初始化的第一帧，返回等待下一帧图像；

  2. 搜索第二关键帧

    搜索当前帧如果特征点大于100个，就构建为初始化的第二帧，否则需要重新构造初始器(连续两帧大于100才算成功)

  3. 两帧匹配

    调用匹配器matcher，如果初始化的两帧之间的匹配点太少，重新初始化

  4. 求取单应矩阵和基本矩阵

    进入初始化器中的Initialize()的过程(主要在initialize.cpp中实现)，完成H矩阵和F矩阵的构建。构建的过程中会判断是构建H矩阵还是F矩阵，并根据构建的矩阵的质量判断是否初始化成果。如果矩阵的质量不好，那么就判断初始化失败，返回第二步，重新寻找初始化的第二帧。

  5. 删除无法三角化的点

    无法三角化是指三角化的结果差的重投影差的离谱

  6. 位姿和坐标确定

    删除那些无法进行三角化的匹配点。计算变换矩阵，将初始化的第一帧作为世界坐标系。

  7. 生成初始地图

    初始化完成R,T还有三角化完成的3D点。接下来就是先删除无法进行三角化的点，然后将第一帧的位姿设为世界坐标的位姿，最后将将三角化得到的3D点包装成MapPoints，加入到新建立的地图CreateInitialMapMonocular()。

2.2.2 单目三角化点包装成地图点

无论是单目还是双目，将特征点包装成地图点MapPoint都是必不可少的。区别在于双目镜头可以直接得到特征点的深度信息，因此包装的过程比较简单，所以直接放在初始化函数中StereoInitialization()，而单目的包装过程非常复杂，因此单独拿出来作为一个函数Tracking::CreateInitialMapMonocular()

首先就是用3D点构造MapPoint，之后为初始化用到的这两帧构建连接关系(双目只用到了一帧所以没有这一步)，最后需要对MapPoint的深度归一化。

  1. 3D点构造MapPoint

    这一步跟双目构造大同小异

    1. 利用3D点创建一个MapPoint
    2. 为MapPoint添加属性(观测到的关键帧，最佳描述子，平均观测方向，深度范围)
    3. 向地图中添加MapPoint

  2. 更新关键帧间的连接关系

    这里调用KeyFrame::UpdateConnections。在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数。

  3. BA优化

    优化的对象是当前地图mpMap，使用的是全局优化函数Optimizer::GlobalBundleAdjustemnt

  4. 深度归一化

    1. 计算归一化系数

      首先计算MapPoint的平均深度，调用KeyFrame::ComputeSceneMedianDepth，然后求倒数

    2. 判断可行条件

      平均深度大于0||当前帧观测到的地图点大于100，否则Reset

    3. 归一化变化矩阵

      提取变换矩阵Tc2w的第三列，乘以归一化系数，将两帧之间的变换归一化到平均深度1的尺度下。

    4. 归一化地图点

      把地图点的尺度也归一化到1，直接将点的世界坐标乘以归一化系数

深度归一化的步骤是：首先求得所有MapPoint的深度（相机坐标系下Z的大小）的中位数，再将所有点的深度除以中值深度。这样最后得到的点所有深度的平均深度（中值深度）就为1。

变换矩阵如下所示，只需要提取$R$的第三行，乘以点的世界坐标就可以得到点在相机坐标系下的深度，再加上平移量$t$，即可求得点的深度。

3. 追踪模型

初始化完成后，对于相机获取当前图像mCurrentFrame，通过跟踪匹配上一帧mLastFrame特征点的方式，可以获取一个相机位姿的初始值；为了兼顾计算量和跟踪鲁棒性，处理了三种模型：

  1. TrackWithMotionModel 恒速模型
  2. TrackReferenceKeyFrame 关键帧模型
  3. Relocalization 重定位模型

这三种跟踪模型都是为了获取相机位姿一个粗略的初值，后面会通过跟踪局部地图TrackLocalMap对位姿进行BA优化。在使用三大追踪模型前我们需要更新上一帧的地图点。

3.1 TrackWithMotionModel

该模型根据两帧之间的约束关系来求解估算位姿。假设物体处于匀速运动，那么可以用上一帧的位姿和速度来估计当前帧的位姿（认为这两帧之间的相对运动和之前两帧间相对运动相同）。上一帧的速度可以通过前面几帧的位姿计算得到。这个模型适用于运动速度和方向比较一致、没有大转动的情形。

如果是静止状态或者运动模型匹配效果不佳（运用恒速模型后反投影发现LastFrame的地图点和CurrentFrame的特征点匹配很少），通过增大参考帧的地图点反投影匹配范围，获取较多匹配后，计算当前位姿；而对于运动比较随意的目标，上述操作失效的。

在执行恒速模型前需要先判断一下：速度是否为空。速度为空意味着刚完成重定位，这时候只能采用关键帧模型。

3.1.1 更新地图点 UpdateLastFrame

如果采用关键帧参考模型或重定位模型，参考的帧是关键帧，则其记录了足够丰富的地图点MapPoint信息，运动模型参考的上一帧可能是普通帧，包含的地图点信息非常少(mvpMapPoints包含的对应关系特别少)，这样不利于优化，所以我们需要为上一帧添加一些临时地图点用于优化（这些点之后会被删除）。这一步只针对于双目和RGBD相机（单目为什么不需要目前不清楚）

  1. 更新上一帧位姿

    将上一帧的位姿设为参考关键帧的位姿(与上一帧有最多共视关系的参考帧)

  2. 获取上一帧具有正深度的特征点
    如果特征点的深度为负，说明根本不在视野范围内，无法重投影。

  3. 将特征点按深度从小到大排列

  4. 将距离较近的特征点包装为MapPoint

    如果这个特征点已经是MapPoint了就不管，如果还不是，则需要创建并添加属性。满足两个条件时，更新结束：1.当前的点的深度已经超过了远点的阈值mThDepth(分割远近点的阈值=基线长度*比例系数/焦距)；2.已经拥有100个MapPoint

3.1.2 整体思路

步骤如下：

  1. 更新地图点

  2. 根据前两帧速度计算当前位姿

前两帧算速度，然后将速度乘以当前帧的前一帧计算粗略位姿。

  1. 重投影追踪

    预设一个搜索半径th ，根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围，计算符合要求的特征点数目。实现方法在ORBmatcher.cpp中。如果得到跟踪点太少，则扩大搜索半径再来一次。如果还是不行则认为跟踪失败。

  2. 优化位姿

    调用Optimizer::PoseOptimization优化

  3. 剔除outlier的关键点

    在优化时，将区域分为outliers和inliers，我们将非常不可能的测量值（根据测量模型）称为外点（outlier）。在优化的过程中就有了对这些外点的标记，outlier不参与下次优化。具体检测方法有RANSAC和卡方分布法。

  4. 返回ture、false标志

    如果成功匹配到的地图点数目（剔除外点后）数目大于等于10，则返回true，否则返回false

3.2 TrackReferenceKeyFrame

假如motion model已经失效（返回false），那么首先可以尝试和最近一个关键帧去做匹配。毕竟当前帧和上一个关键帧的距离还不是很远。作者利用了bag of words（BoW）来加速匹配。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配。

添加到地图中的帧称为关键帧(KeyFrame)，它构建在帧(Frame)的基础上，与地图(Map)关联。换句话说关键帧是对建图和定位比较重要的帧

步骤如下:

  1. 将当前帧的描述子转化为BoW向量

  2. 通过特征点的BoW加快当前帧与参考帧之间的特征点匹配

    由ORBmatcher提供方法，采用SearchByBoW()专门计算，由vpMapPointMatches存储匹配关系。参考关键帧就是里当前帧最近的关键帧。如果匹配数目不够就退出，采用重定位模式。

  3. 将上一帧的位姿态作为当前帧位姿的初始值

    这里需要拷贝两个东西：把上一帧的位姿mLastFrame.mTcw设置为本帧的位姿（不是上一个关键帧）。另外需要将与关键帧匹配得到的路标点vpMapPointMatches复制到mCurrentFrame.mvpMapPoints中。

  4. 优化位姿

    优化3D-2D的重投影误差，依然是Optimizer.cpp内容

  5. 剔除outlier

  6. 返回true、false标志

3.3 Relocalization

假如当前帧与最近邻关键帧的匹配也失败了，意味着此时当前帧已经丢失，无法确定其真实位置。此时，只有去和所有关键帧匹配，看能否找到合适的位置。

重定位的方法是利用词袋模型，在关键帧数据库中找到与当前图像帧相似的候选关键帧(与回环检测过程不同，回环检测使用参考关键帧去寻找闭环候选帧，这里使用普通帧去寻找候选）。

步骤如下:

  1. 将当前帧的描述子转化为BoW向量

  2. 初次筛选找到与当前帧相似的候选关键帧

    通过KeyFrameDB.cc中的DetectRelocalizationCandidates进行候选，存储一个容器中vector，筛选的条件比较复杂

  3. Bow二次筛选候选帧

    这一步想要确定出满足进一步要求的候选关键帧并且为其创建pnp优化器。首先通过BoW进行匹配，筛选条件是单词匹配数，匹配数太小直接放弃此帧，如果合格就初始化PnPsolver。

  4. 遍历关键帧

    通过Bow二次筛选，我们得到了一个小范围的候选帧。接下来对这些候选帧进行分析。后面每次操作都会判断内点数量nGood有没有超过50，超过了就直接bMatch=true，然后跳出，证明匹配成功

    1. EPnP估算姿态

      估算可以得到位姿和内点数，如果RANSAC迭代后发现效果不好，直接踢掉此帧

    2. 存入所有内点

      将上一次筛选得到的关键帧的内点vbInliers存入mCurrentFrame.mvpMapPoints和sFound，这个sFound是一个set类型，表示找到地图点的集合。后续重投影搜索有用。

    3. 优化位姿

      如果优化后内点太少，踢掉

    4. 删除外点更新地图

    5. 如果内点比较少，一系列骚操作

      这里骚操作实在太多，结构见上图。总的来说就是不断重复：投影找额外点，然后让原本的内点加上额外点一起优化，再投影找额外点，再次共同优化。最后看结果内点有没有超过50，超过了就表示顺利匹配上了，否则说明这个候选帧不行，再选一个从a做起。值得注意的是，不管怎样，都要在最后检测nGood>50

4. 局部地图匹配 TrackLocalMap

4.1 总体思路

上面的三个跟踪模型得到的位姿和地图点是粗略的。下面需要进一步优化地图和位姿。我们还需要通过TrackLocalMap判断我们追踪的结果怎么样，有没有跟丢。

姿态优化部分的主要思路是在当前帧和（局部）地图之间寻找尽可能多的对应关系，来优化当前帧的位姿。实际程序中，作者选取了非常多的关键帧和地图点。在跑Euroc数据集MH_01_easy时，几乎有一半以上的关键帧和地图点（后期>3000个）会在这一步被选中。然而，每一帧中只有200~300个地图点可以在当前帧上找到特征匹配点。这一步保证了非关键帧姿态估计的精度和鲁棒性。

匹配的步骤如下:

  1. 更新局部关键帧和局部地图点

    更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

  2. 进一步筛选局部地图点

    投影范围超出相机画面、观测视角和地图点平均观测方向相差60°以上、特征点的尺度和地图点的尺度不匹配。

  3. 再次优化

    通过更新和抛弃，再次调用Optimizer::PoseOptimization优化得到位姿

  4. 更新当前帧的MapPoints被观测程度

    通过优化我们得到了精确的位姿和当前帧对应的地图点。判断mvpMapPoints是不是外点（主要针对单目），如果不是外点，说明能被观测到，被观测统计量Found+1，匹配内点数mnMatchesInliers+1。这些参数用于判别是否跟踪成功。

  5. 判别是否跟踪成功

    如果最近刚刚发生了重定位,那么至少跟踪上了50个点mnMatchesInliers我们才认为是跟踪上了。如果是正常的状态话只要跟踪的地图点大于30个我们就认为成功了。

4.2 更新局部关键帧和局部地图点

首先第一步更新局部关键帧和局部地图点的目的是为了给优化提供样本。

局部地图包括：当前帧POS3，与当前帧有共视关系的关键帧POS2，与POS2有密切关系的关键帧POS1；局部关键帧对应的所有地图点X1X2。

4.2.1 更新局部关键帧

在论文里作者定义局部地图关键帧的方式如下。简单来说局部关键帧包含了两个集合(set)：第一，与当前帧有共视关系(share map points)的关键帧，记作集合K1；第二，与集合K1在共视图中(covisibility graph)有良好共视关系的帧（具体见下），记作K2。

作者原文：This local map contains the set of keyframes K1, that share map points with the current frame, and a set K2 with neighbors to the keyframes K1 in the covisibility graph.

K1比较好理解，凡是和当前帧有共同的MapPoint的关键帧都可以被归为这一集合。K2比较麻烦，总体来说是和K1比较密切的帧的集合。这里的密切有三种条件：

  • 与K1有良好共视关系的子关键帧(作者选取了最佳共视的10帧)
  • K1中元素的子关键帧
  • K1中元素的父关键帧

满足其中一种条件的关键帧都可以被归为K2集合。

在处理完局部关键帧后，还需要添加参考关键帧。与当前帧共视程度最高（有最多share map points）的关键帧作为参考关键帧。

4.2.2 更新局部地图点

比较简单。上一步得到了所有的局部关键帧，这一步只需要把局部关键帧中对应的MapPoints全部添加到mvpLocalMapPoints中即可。注意，在添加之前需要将mvpLocalMapPoints清空才行。

4.3 进一步筛选局部地图点

4.2得到了一大堆局部地图点，这些点有很多是不能用的，所以需要进一步做筛选。

  1. 遍历当前帧mvpMapPoints

    MapPoint一定是没有问题的，是我们可以用来做优化样本的，所以在这一步标记一下，之后不参与判断，默认放行。需要注意的是：MapPoint是地图点，在tracking三大模型中经过层层筛选得到的，一帧有很多特征点，只有少数才能被遴选为MapPoint。因此除了MapPoint，当前帧还有很多特征点和其他帧有共视关系，这就是我们需要在这一步筛选的。

  2. 将所有局部地图点投影到当前帧，判断是否在视野内

    1. 检查这个地图点在当前帧的相机坐标系下，是否有正的深度。如果是负的，就说明它在当前帧下不在相机视野中。
    2. 将MapPoint投影到当前帧, 并判断是否在图像内（即是否在图像边界中）
    3. MapPoint到相机中心的距离是否在范围内。如果里的太远或者太近这个点就不合适。
    4. 计算当前视角和平均视角夹角的余弦值, 需要小于60°才能合格

    经过4个关卡的重重考验后，这些MapPoint被认为能够作为最后优化的样本。然后为他们添加一些信息：点到光心的距离；置位标记（true表示要被投影）；这个点在图像中的投影坐标；当前视角和平均视角夹角的余弦值。

  3. 为合格的地图点确立投影匹配关系

    要先设立一个阈值th，如果匹配关系落在阈值内就表示匹配成功，正式成为优化样本一员。

4.4 优化

这里采用的是g2o优化器优化，顶点是当前位姿和合格地图点，需要进行4次优化。

优化图如上所示，顶点有当前帧位姿和所有的合格地图点，其中地图点固定。这里采用的是EdgeSE3ProjectXYZOnlyPose和EdgeStereoSE3ProjectXYZOnlyPose类型，这是g2o中提供的模板，用于优化位姿，地图点默认固定。

优化结束后就进行信息记录和判别是否追踪成功，具体内容见4.1总体思路部分。

5. 创建新的关键帧

ORB-SLAM中关键帧的加入是比较密集的，这样确保了定位的精度，同时在LocalMapping线程最后会进行关键帧的剔除，确保了关键帧的数量不会无限增加，不会对large scale的场景造成计算负担。

以下条件必须同时满足，才可以加入关键帧：

  • 距离上一次重定位距离至少1S
  • 当前帧跟踪至少50个点，保证精度
  • 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%，确保关键帧之间有明显的视觉变化
  • 局部地图线程空闲 或者 距离上一次加入关键帧过去了20帧(如果需要关键帧插入过了20帧。而LocalMapping线程忙，则发送信号给线程，停止局部地图优化，使得新的关键帧可以被及时处理)

调用函数创建完成后，将关键帧传递到LocalMapping线程。

注意：这里只是判断是否需要将当前帧创建为关键帧，并没有真的加入全局地图，因为Tracking线程的主要功能是局部定位，而处理地图中的关键帧、地图点，包括如何加入、如何删除的工作是在LocalMapping线程完成的，Tracking负责localization，LocalMapping负责Mapping。

5.1 是否需要加入关键帧

首先是NeedNewKeyFrame()判断是否加入关键帧：

  1. 判断是否重定位

    由于插入关键帧过程中会生成MapPoint，因此用户选择重定位后地图上的点云和关键帧都不会再增加。

  2. 判断局部地图是否被闭环检测使用

    如果局部地图被闭环检测使用，则不插入关键帧

  3. 获取参考关键帧跟踪到的MapPoints数量

    在 UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧

  4. 查询localMapper是否繁忙

  5. 统计可以添加和跟踪到地图中的的MapPoint数量

  6. 决策是否插入关键帧（必须满足a-e所有条件）

    1. 长时间没有插入关键帧

    2. localMapper处于空闲状态

    3. 地图点匹配数目和跟踪成功比例很小，即将撑不下去

      radio=被关键帧观测到的mappoints数/总共可以添加的mappoints数(如果是近点,并且这个特征点的深度合法,就可以被添加到地图中)；这个radio比例太小，说明track is weak

    4. 与之前的参考帧重复度不高

      共视的地图点不是很多

    5. 如果localMapper繁忙，等待队列等待数需要小于3

      前面判断localMapper是否繁忙，用的是mpLocalMapper->AcceptKeyFrames()也就是说是否接受关键帧。这里判断的是关键帧等待队列是否阻塞严重(>3)

5.2 创建关键帧

之后利用CreateNewKeyFrame()创建关键帧

  1. 构造关键帧

  2. 当前关键帧设置为当前帧的参考关键帧

    关键帧的参考关键帧就是他自己。

  3. 根据Tcw计算额外矩阵

    普通帧为了节省计算量，只计算了TcwTcw相机坐标到世界坐标的转化（也就是相机变换矩阵或者相机姿态），而关键帧由于在很多地方有特殊用途所以还需要额外计算一些矩阵。

    1. mRcw 旋转矩阵
    2. mRwc 旋转矩阵的逆
    3. mtcw 平移向量
    4. mOw 光心在世界坐标系下的坐标

  4. 获取正深度特征点

    用于重新构建MapPoint

  5. 按照深度从小到大排序

  6. 将距离比较近的点包装成MapPoints

    如果当前帧中无这个地图点，或者是刚刚创立（观测者Observations<1），就在全局地图中创建地图点。每次创建MapPoint都需要添加属性。如果当前已经处理了超过100个点且深度已超过阈值，就停止。

  7. 插入关键帧

    执行插入关键帧的操作,其实也是在列表中等待。同时需要然后现在允许局部建图器停止，并且让当前帧成为新的关键帧。