ORB-SLAM2--Tracking线程

tracking线程主要完成的是帧之间的跟踪，单目初始化、重定位、关键帧的插入、位姿的优化。

单目初始化

详细请看MonocularInitialization()
基本问题就是：怎么通过两张图像确定相机自身运动，并且确定像素点的距离

初始化的运动是通过对极几何来求解的，结构是由三角测量得到的
通过并行的计算基础矩阵和单应矩阵，实际上这两个矩阵都可以分解出初始两帧的运动，但是由于平面与非平面的场景会选择采用不同的初始化方法，平面场景视差小会用单应矩阵，非平面场景视差大会用基本矩阵。因此会有一个评分标准。

特征匹配：首先取帧1中的所有特征点，根据给定参数搜索窗口确定搜索半径，根据该特征点在帧1中的2D位置估计其在帧2中的搜索中心，从而根据搜索中心和半径确定搜索范围，在帧2中搜索当前特征点的匹配点。最后进行匹配筛选（剔除误匹配）
GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);，这进行初始帧和当前帧的特征匹配，由同一层的金字塔层数和给定的搜索边长100创建出来的搜索窗口进行特征点的筛选与匹配。

R_H>0.45 表示二维平面和低视差的情况，使用单应矩阵。其他的情况，我们选择基本矩阵

尺度不确定性

那么在对极约束中t无论变化多少整数倍都是满足的，把运动和场景同时放大任意倍数，单目相机仍会观察到同样的图像，这就是尺度不确定性。比如我们对一个高楼在一定上进行拍照，把一个高楼缩小n倍变成一个模型，那我们靠近这个模型与之前近n倍距离进行拍照，得到的效果是一样的。
前面初始化运动估计的单位是不确定的，因此要进行单位的确定，但是这个单位是多少我们是不知道的，每次都不会不一样，但是orbslam的规则是选择地图点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化。

全局BA

全局的BA GlobalBundleAdjustemnt只发生在单目初始化和回环检测，这里的全局BA的目的是为了对地图点和初始位姿进行一个优化，只有初始参考精确了，后面的估计才会更精确。

跟踪模型

首先会先考虑运动模型的跟踪，次之是参考关键帧的跟踪。如果前面跟踪稳定的话，之后会再进行局部地图的跟踪（这里为什么是前面跟踪稳定了才会进行局部地图的跟踪？？）。完成前面的跟踪会进行跟踪是否成功的判断，如果失败，会把当前状态位设置为lose。之后进行重定位操作。

运动模型跟踪

上面图像给出了3个时刻的帧，k、k-1、k-2三个时刻，根据前面运动的估计可以计算出k-2到k-1帧的相对变化△Rk-1，那么令∆R≈△Rk-1这样就可以知道当前帧和上一帧的相对变化了，同时也可以由上一帧位姿,$\Delta R_{k-1}\cdot T_{c(k-1)w}=T_{c(k)w}$,得到当前帧的初始估计位姿，这个位姿是不准确的，后续完成点的匹配后还会进行优化。

RGBD、双目下：
这两种传感器都可以直接提供特征点的深度信息，并且在每次模型跟踪中都会进行位姿的优化，对于产生的属于外点的地图点都会进行剔除，为了保障能够这次顺利跟踪，会对上一帧添加一些额外的地图点，这些点只进行跟踪，因此创建后不会为它们添加任何属性（计算平均观测方向、最优描述子、观测次数、与关键帧哪个特征点配对等），因此会对它们进行标记，之后进行删除。
通过函数 UpdateLastFrame() 来进行地图点的添加

• mlRelativeFramePoses是在上帧完成跟踪后计算出它与最新参考关键帧的相对变换，往这容器尾部压入这个相对变换，他其实就是存储每一成功跟踪的帧与最新关键帧的相对变换的一个容器。
• mlpTemporalPoints是一个存储了在tracking线程中通过这种方式为了跟踪额外添加的地图点，之后在CreateNewKeyFrame之前会全部删除
• 对于额外生成的地图点数量+已有地图点数量>100，那么就停止添加。

详细请看源码，比较简单

匹配：这里因为是与上一帧k-1帧的地图点进行跟踪，所以匹配也是和这些地图点进行匹配。
1、地图点投影到当前帧，中间会进行深度值是否有效、该地图点是否在当前帧像平面内的检测
2、由该地图点在k-1帧下特征点提取时所在的金字塔层知道该层的尺度因子结合给定的参数计算出搜索半径[详细请看]
3、对位姿进行优化

参考关键帧的跟踪

同样的给出了三个帧k、k-1、KF，这里还是同运动模型跟踪一样需要把已经存在的地图点往当前帧进行投影，完成跟踪；同样的当前帧k的位姿未知，需要进行估计，之后再优化

• 由于前面的跟踪，k-1帧的位姿是已经知道的，我们把SE3k≈SE3k-1(注意上面图片出错把下标KF改成k-1)，这样赋值的目的是为了后续的优化中可以更快的收敛，不是用来进行投影。
• 这里并没有地图点投影过程，而是直接对KF和K帧用词袋向量加速特征点的匹配，对与两张图中处于同一个node的特征点进行两两比较（这里是KF中该节点上的地图点与K帧该节点上的特征点两两比较）
• [详细看SearchByBoW]
• 进行位姿的优化，由于第一步的初值估计，可以得到更快的收敛。优化后对在当前帧中属于外点的地图点进行清除

匹配：这里的匹配用到了词袋的正向索引，对两帧图像处于同一节点下的特征点两两比较（而这里是同一节点下地图点与特征点的比较)

局部地图跟踪

进行当前帧局部地图的搭建，先向局部地图中加入关键帧（ UpdateLocalKeyFrames()），这些关键帧是和当前帧有共视关系或者间接关系的关键帧，之后再把这些关键帧的地图点放如局部地图中。
对局部地图中的点进行筛选那些在当前帧视角下的点通过函数 isInFrustum（），在此之前因为前面的参考关键帧和运动模型的跟踪已经成功的跟踪了部分地图点了，那这些点就不需要重复的进行配对，所以需要对已有的地图点进行不需要配对的标记。
匹配：通过函数**SearchByProjection**进行投影匹配，那这里也同运动模型匹配一样照样需要计算出一个搜索半径，但是它是通过当前地图点的视角进行推算的，之后根据地图点预测（在 isInFrustum函数中进行了预测）出来的金字塔提取的层数来进行搜索的。

位姿的优化

在上面的每一个跟踪模型中都会进行位姿的优化，但是这里的MapPoint是不参与优化的，知道进入了LocalMapping线程才会参与局部的优化，因此就是构造一个一元边优化模型

该图就是重投影误差模型，理论上P的投影会与P2重合实际上会存在一个误差e，优化的目的也就是调整优化变量使得这个误差尽可能的小。
因为会存在很多的投影点，那么要综合所有的约束进行优化，所以可以得到下面的公式：

要加上一个求和符号，表示就把所有的约束给综合起来，这是实际就是一个最小二乘问题，目标就是求解这个最小二乘,exp(ξ∧)就是待优化变量。
那么考虑其中一条约束并简化就是：

上面是进行了一阶泰勒展开，x就是待优化变量，J是雅克比矩阵，也就是误差函数e对位姿的导数。当 e 为像素坐标误差(2 维),x 为相机位姿(6 维)时,J 将是一个 2 × 6 的矩阵。
考虑 e 的变化关于扰动量的导数。利用链式法则,可以列写如下:

P’是地图点在当前帧相机坐标系下的坐标。
分别进行求导详情请看


最终
误差函数关于待优化变量的导数一求出来，就可以知道每次迭代步长和方向运用LM或者高斯牛顿求解最小二乘。
但是这里只是其中的一条约束，我们需要把所有的约束考虑进来，就需要用到G2O的优化库搭建一个优化模型。

3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw)

只优化Frame的Tcw，不优化MapPoints的坐标

构建一个一元边

1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw

2. Edge:
   g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge
   + Vertex：待优化当前帧的Tcw
   + measurement：MapPoint在当前帧中的二维位置(u,v)
   + InfoMatrix: invSigma2(与特征点所在的尺度有关)

详细看到PoseOptimization(Frame pFrame);

统计跟踪局部地图效果
mnMatchesInliers变量记录的是当前帧进行局部地图跟踪和位姿优化后属于内点地图点的数量，同时会对这些点增添观测值。在后面的插入关键帧判断也会用到这个参数

重定位

Relocalization()

作用： 相机的快速移动，会造成图像的模糊，这样会影响特征的提取与匹配；相机视角小的缘故，快速移动也会使前后两张图像的重叠度低，这也会影响特征的匹配；这样跟踪就很可能会失败，在每一帧的跟踪结束后都会进行一个判断，如果当前帧的跟踪失败了（没有太多的匹配点），会把状态位变为lose状态，那么在下一帧到来时直接启动重定位 。
重定位可以在丢失的状态下，根据之前的跟踪信息，可以重新的找回当前相机的位姿，继续接下来的跟踪。

程序流程图

小结

• 首先整个过程需要通过 DetectRelocalizationCandidates(&mCurrentFrame) 函数来求解基于当前帧的候选关键帧，之后才是对这些所有的候选关键帧进行遍历和剔除，直到所有的候选关键帧剔除完毕或者先找到了一个符合要求的候选关键帧。
• 每一个候选选关键帧都为它创建一个Epnp求解器，因为每个求解器的参数不一样，也就使得每个求解器都不一样，比如每个求解器可用迭代的特征点数量都是由每个候选帧的地图点数量决定的
• 在每一次Epnp的迭代求解位姿后pSolver->iterate都会对外点进行清除，同样在PoseOptimization 后也会进行外点清除
• 由于不断的进行外点的清除，就很可能导致保存下来的地图点太少，在进行了Epnp的迭代求解位姿后会进行位姿的优化也都会进行内点数量的判断，如果是不够或者在一定范围内会通过**SearchByProjection**进行地图点的再添加

运动模型创建

• 运动模型是在初始化之后就会进行尝试的创建，运动模型实际上就是上一帧到当前帧的相对变化
• 因为在每一次帧的读取，不管该帧是成功跟踪了还是失败了都会保存为上一帧，那只要上一帧成功跟踪有位姿信息，那么就会在这一步创建运行模型，否则运动模型就是为空，那下一帧的跟踪就不会启动运动模型跟踪
• 转换公式如下，其中twc是上一帧的光心坐标
  

清除临时添加的MapPoints

• 双目或深度情况下，在进行运动模型跟踪的时候会对在当前帧中会通过UpdateLastFrame() 产生一些属于自己的地图点
• 这写地图点只用来跟踪，因此不会进行观测信息的添加，那么这些点的nObs 被观测数肯定小于1，用此条件来进行清除，并且清空内存

添加关键帧

这里的关键添加，需要进行一个检测当前帧的情况下是否需要添加关键帧，之后添加关键帧后，并没有直接的放入关键帧数据库，也没有对前面跟踪得到的地图点更新任何的属性（计算平均观测方向、最优描述子、观测次数、与关键帧哪个特征点配对等），而是放入一个队列容器中，等待LocalMapping线程进行进一步的处理。同时添加关键帧过程还分单目和非单目情况。

检测是否需要插入关键帧

NeedNewKeyFrame()

关键帧插入这里只是把tracking线程的关键帧放入一个序列，待LocalMapping线程处理，因此这里不是十分的苛刻。
首先会先进行几个步骤的繁忙、模式情况判断，之后再进入到决策判断。

繁忙、运行模式情况判断

• 仅定位跟踪模式不插入关键帧
• 局部地图被闭环线程使用不插入关键帧
• 当前帧距离上一次重定位帧时间很短不插入关键帧（这里是以图像输入频率来作为阈值、30帧）

决策判断

决策判断----参数准备：

1. 计算当前帧跟踪点与当前帧所有可添加点的一个比例ratioMap=当前帧跟踪地图点数量 / 当前帧符合深度信息的所有特征点也就是总的可以添加mappoints数 （该参数意义就是体现当前帧具有合格深度信息的特征点能够成功用来跟踪的数量，ratioMap小说明当前帧特征点利用率不高，和地图的重合度不高，如果单目情况该参数是1，无意义）
2. 设定当前帧地图点和地图关联的比例值 thMapRatio (也就是ratioMap参数的一个阈值)
3. 记录当前帧进行局部地图跟踪并且进行位姿优化后产生的被观测数大于0的内点 weimnMatchesInliers
4. 获取参考关键帧成功跟踪到的地图点数量 nRefMatches（注意这里的点不包括参考关键帧自身生成的点）
5. 设定和当前参考关键帧特征点匹配的inlier比例阈值 theRefRatio（ weimnMatchesInliers / nRefMatches）（表示的是当前帧和参考关键帧的重复度）

决策判断----最终判决：

• cla=距离上一插入已经超过图像传入频率（已过30帧）
• clb=LocalMapping线程空闲（前提是距离上一帧插入必须大于最少帧数否则同样是false）
• clc=非单目情况下 跟踪要跪的时候，也就是当前帧从参考关键帧跟踪到地图点的数量很少（weimnMatchesInliers / nRefMatches<0.25）或者当前帧从地图跟踪到的点很少（ratioMap<0.3）
• c2=当前帧从参考关键帧跟踪到地图点的数量少重复度不是太高（weimnMatchesInliers / nRefMatches
• final=((cla||clb||clc)&&c2) 其中c2的决策权更大

其中这5个都是bool型变量

如果final=ture那么会进行下一步判断LocalMapping的繁忙情况，不繁忙那么同意插入否则会中断LocalMapping线程的BA优化
中断BA后，非单目下进一步检查关键帧序列是否少于3个帧，是则同意插入，不是则不同意插入；单目情况下直接返回不同意插入，只能进行下一帧的跟踪，如果再次符合上面判断才会插入关键帧

创建并插入关键帧

CreateNewKeyFrame()
首先会把当前帧创建成关键帧，之后根据是否非单目情况会进行额外操作

非单目情况（额外操作）：
会生成属于当前帧的地图点，并且为这些地图点添加属性（计算平均观测方向、最优描述子、观测次数、与关键帧哪个特征点配对等）
流程图：

最后把当前生成的关键帧插入序列，待LocalMapping线程进行下一步处理，把当前关键帧设置为最新参考关键帧，以备下一帧的跟踪。

记录跟踪信息

在当前帧完成以上的步骤之后，不管是丢失还是成功，会对当前帧得到的状态进行记录，以备在下一帧跟踪的时候用到上一帧的跟踪信息。

• 如果当前帧的参考关键帧为空，那么进行填充
• 完全复制当前帧信息，保存到mLastFrame变量，以备下一帧的模型跟踪提供信息
• 如果当前帧跟踪成功，把它和参考关键帧的相对位姿Tcr和时间戳、参考关键帧、跟踪状态压入对应的队列中，以备后续跟踪提供信息
• 如果跟踪失败，是没有位姿信息的，那么会把当前帧的上一帧的它和参考关键帧的相对位姿Tcr和时间戳、参考关键帧、跟踪状态再次压入对应的队列中

总结

• 参考连接Tracking线程总结