ORBSLAM2之Tracking线程

序言

  为加深自己的理解，同时督促自己回顾ORBSLAM2框架，决定从今天起开始重读ORBSLAM2，并按照框架逻辑做一些笔记。目前打算分三个线程写三篇文章，把整个框架概述一遍。若有错误，欢迎指出。

Tracking 线程概述

  此线程主要用于完成数据关联任务，实现帧间跟踪，位姿初步计算，生成关键帧与MapPoint。
该线程可高度简化为如下流程。

  Track()中的流程为:

本文将围绕这两个流程图进行介绍。

一、读取图像 GrabImageStereo（）

1. 提取ORB特征点

1.1 构造图像金字塔，把把提取到的图像层层降采样
1.2 将每一层的图像分成一个个cell进行FAST角点检测
1.3 把这些特征点存入四叉树保证特征点均匀
1.4 计算每个特征点的描述子
四叉树原理：
a) 只有1个node

b) 1个分裂为4个node，4<25，还要接着分裂

c) 4node分为15个node，因为有一个node里面没有点，所以不是16个。15<25接着分

d) 第三次分裂为30个node，30>25。分裂结束

e) 从每个node中挑选质量最好的一个点

2. 双目匹配

1. 根据特征点尺度确定带状搜索域，尺度大则带宽大

2. 根据SAD滑窗计算匹配坐标修正值bestincR

3. 抛物线拟合计算bestincR修正值deltaR

4. 最终匹配点的坐标为ur = ur+bestincR+deltaR

5. 根据视差ul-ur计算深度并存储

二、 双目初始化 StereoInitialization（）

1. 若特征点足够多则开始初始化
2. 设定当前帧位姿，在地图中插入该帧
3. 根据双目匹配深度构造特征点对应的MP，当前关键帧作为该MP的参考关键帧
4. MP和该帧之间互相建立关联、计算MP最具区分性描述子、计算MP平均观测方向和观测距离范围
   a) 计算MP最具区分性描述子ComputeDistinctiveDescriptors()
     i. MP可能会被多帧观测到，因此具有很多描述子
     ii. 找出一个描述子距离其他描述子距离中值最小的作为该MP的描述子（中值一定程    度上反应了到其他描述子的平均距离）
   b) 更新平均观测方向和最远最大距离UpdateNormalAndDepth（）
     i. 获得观测到该MP的光心到MP的单位方向向量
     ii. 计算这些单位方向向量的平均值即为平均观测方向

    c) 计算观测到该MP的最远最近距离

     若在参考关键帧的第m层观测到该MP，则
      最远观测距离 $d_{max}=|OMP|\ast 1.2^m$（m层尺度因子）
      大于这个距离则在整个金字塔上都检测不到该特征点，因为太小了，此时特征点在0层
      最近观测距离 $d_{min}=\frac{|OMP|}{1.2^{n-1-m}}$
      在最近观测距离时，该特征点在金字塔顶部

5. 局部地图插入该关键帧，把当前关键帧设为参考关键帧，mState=OK，初始化成功

三、跟踪 Track()

注意本文中不考虑OnlyTracking

1. 检查MapPoint

  检查上一帧的MP是否被LocalMapping或LoopClosure线程替换过。若替换过，则更新上一帧对应的MP

2. 以运动模型进行跟踪TrackWithMotionModel()

a) 对上一帧生成一些临时MP UpdateLastFrame（）
    i. 根据上一帧与上一帧的参考关键帧相对位姿（Tlr）设定上一帧位姿(Tlw)
    ii. 在初始关键帧时会生成一些MP存入，后面的帧用MP进行跟踪。若后 面的帧不及时补充一些MP，会导致可跟踪的MP越来越少。

     iii. 真正的MP都是通过KF在CreateNewKeyFrame（）函数中生成
     iv. 这些临时生成的MP都存在mLastFrame.mvpMapPoints中，也存在mlpTemporalPoints中，这些临时MP将会在CreateNewKeyFrame（）全部删除

b) 根据最近一次前后位姿之差计算当前帧初始位姿。

c) 重投影匹配（跟踪上一帧） realized by SearchByProjection（）
      i. 将上一帧MP重投影到当前帧中，根据MP尺度确定搜索半径，在搜索半径内找到最佳匹配点

  ii. 将MP设为当前帧匹配上的特征点对应的MP
  iii. 利用特征点角度变化剔除误匹配。

  iv. 若匹配点较少，则返回false，bOK置为false，进入TrackReferenceKeyFrame模式

d) 位姿优化
  关于协方差矩阵以及卡方分布在ORBSLAM中的应用请参照另一篇博文
如何理解ORB-SLAM g2o优化中的卡方分布

1. 构造g2o优化器，线性方程求解器，块求解器，迭代算法
2. 将当前帧位姿设为顶点
3. 添加1元边，将当前帧MP设为边的观测值。根据卡方检验值设置鲁棒核函数参数。将${\Sigma }^{-1}$设为信息矩阵，其中
   $$\mathbf{\Sigma }={\left(s^n\times p\right)}^2\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$
4. 优化4轮，每轮10次。在每轮结束后利用卡方检验将MP分为inlier和outlier，outlier不参与下次优化。每轮结束后原有的inlier可能会变为outlier，原有的outlier可能会变为inlier。
5. 根据优化结果设定当前帧位姿，返回inlier数量

3. 以参考关键帧进行跟踪TrackReferenceKeyFrame()

当TrackWithMotionModel()中重投影匹配点较少时进入该模式
a) 计算当前帧的BoW向量
  i. 将当前帧描述子转化为BoW向量，正向索引，用于加速特征点匹配。格式为
$$[(w1,特征点序号),(w2,特征点序号),\dots \dots ,(wn,特征点序号)]$$

b) 利用BoW加速参考关键帧与当前帧的特征点匹配
  i. 同一个node下的word进行匹配
  ii. 若匹配上则将参考关键帧特征点MP赋值给匹配点对应MP
  iii. 根据旋转角删除误匹配，参照三、2、c)、iii

c) 若BoW匹配较少，TrackReferenceKeyFrame()返回false，bOK置为false

d) 位姿优化

4. 模式切换

  在TrackReferenceKeyFrame()和TrackWithMotionModel()都跟踪失败后（bOK连续两次是false），mState=LOST，将会进入重定位模式。若bOK是True, 则进行局部地图跟踪TrackLocalMap()。

5. 重定位模式Relocalization()

a) 计算当前帧BoW向量
b) 找到与当前帧相似的候选关键帧，用Bow加速匹配候选帧与当前帧的3d-2d匹配，参照三、3、b）
c) 不断地对每帧至多迭代5次利用RANSAC和EPnP计算位姿
  i. RANSAC请参照博客：RANSAC算法理解
  ii. EPnP原理请参照[PnP]PnP问题之EPnP解法
  iii. RANSAC + EPnP + 优化 计算位姿
伪代码：

w = 内点数/外点数，ORBSLAM中设为0.5
N 当前关键帧3D-2D匹配点对数
Foreach(候选关键帧)
	Iteration = 0
	While(iteration<5 || 该帧迭代次数<最大迭代次数k)
		Inlier.clear()
		随机选取4对3D-2D匹配点作为Inlier
		用Inlier EPnP求解Tcw0
		Foreach(数据集中其他点)
			If(在Tcw0下重投影误差小于阈值)
				将该点加入Inlier
		If(Inlier数大于阈值w*N)
			用Inlier EPnP计算Tcw1
			If(Tcw1下Inlier数比之前都多)
				Tcw’ = Tcw1
			返回Tcw1
	If（该帧总共迭代次数>k）
		删除该候选帧
		返回Tcw’
	以返回值为初始值进行pose-only BA优化
	If(优化后内点数>阈值)
		重定位成功
		break
	else
		进入下一个候选帧
If(所有候选帧都不满足条件)
	 mState = LOST
	 进入Reset模式

6. 局部地图跟踪TrackLocalMap（）

若TrackReferenceKeyFrame()，TrackWithMotionModel()，Relocalization()有一个成功，则进入该模式，否则Reset
a) 更新局部关键帧（UpdateLocalMap()）
  i. 清空局部关键帧
  ii. 将当前关键帧的1级邻接关键帧、2级邻接关键帧以及1级邻接关键帧的子关键帧和父关键帧加入到局部关键帧中。（注意到这里查找共视关键帧所用到的MP都是真实的MP，不是那些临时的MP）
  iii. 将共视程度最高的1级邻接关键帧作为当前帧的参考关键帧
b) 更新局部地图点（UpdateLocalMap()）
  i. 清空局部地图点
  ii. 将局部关键帧的地图点加入到局部地图点中
c) 跟踪局部地图点（SearchLocalPoints（））
  i. 依次判断局部地图点是否在当前关键帧视野范围内
    1. 判断MP投影到当前帧中是否在图像范围内
    2. 判断光心到MP距离是否在该MP最大最小距离范围内
    3. 判断光心观测MP的角度与该MP的平均观测方向小于60度
    4. 预测MP在当前帧下尺度（此步非常重要，用于下步重投影匹配）
$$nPredictedLevel=log(最大观测距离/当前观测距离)/log(1.2)$$
  上述加粗字都为MP属性，新关键帧观测到MP后会更新这些属性
  ii. 将所有局部地图点投影到当前帧中进行匹配（与跟踪上一帧的重投影匹配有些区别）SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)
    1. 将MP重投影到当前关键帧中
    2. 根据MP的观测角以及该MP预测尺度确定搜索半径
      a) 搜索半径与观测角、尺度成正比
    3. 在搜索半径内确定最佳匹配点
      a) 记录匹配上的特征点对应的MP存入 F.mvpMapPoints
      b) 并没有对MP属性进行操作，也是临时用来Track的

d) 位姿优化
    i. 在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化。这里利用一系列局部地图点进行位姿优化
    ii. 原理参考三、2、d）

e) 更新当前帧MP被观测程度，更新当前关键帧观测到的MP数量mnMatchesInliers。注意这些MP都是真实MP（跟踪局部关键帧而来）

f) 若位姿优化后mnMatchesInliers较少,则跟踪局部地图失败，进入reset模式

7. 局部地图跟踪之后

TrackLocalMap()若成功则
a) 删除TrackWithMotionModel()生成的临时MP，注意到TrackReferenceKeyFrame()利用的都是真实MP，不用删除。

b) 检查是否需要插入新关键帧NeedNewKeyFrame（）
  i. 满足下列条件之一则不插入关键帧，返回false
    1. 若距离上次重定位不超过1s，或者地图中关键帧太多
    2. 若局部地图线程被闭环检测使用（mbStopped || mbStopRequested）
  ii. 满足下列条件之一
    1. 长时间没插入关键帧（c1a）：1s还没有插入关键帧
    2. LocalMapper处于空闲状态(c1b)
    3. 跟踪要跪(c1c)：若上一帧跟踪到的MP小于阈值（该阈值为自适应阈值），即当前帧跟踪到的MP数量比参考关键帧少得多
  iii. 且满足该条件：当前帧跟踪到的MP数量比参考关键帧少一些(C2)
  iv. 若(c1a || c1b || c1c) && c2且LocalMapper空闲则插入参考关键帧。若LocalMapper不空闲，则阻断LocalMapper的BA（mbAbortBA=true）再返回true

c) 若NeedNewKeyFrame()=true则CreateNewKeyFrame()
  i. 将当前帧设为参考关键帧
  ii. 将没有MP对应且有深度的特征点包装为MP，更新MP属性（好像只有创建该MP的时候才更新属性？难不成在LocalMapping线程中有更新？——是的）
  iii. 将创建该MP的帧设为MP的参考关键帧

参考：https://zhuanlan.zhihu.com/p/61738607