一起学ORBSLAM2（4）tracking主线程
转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125
跌跌宕宕距离上次更博已经数月，其实对于ORB-SLAM的学习早就完成，只是由于时间原因没能够及时更新。关于orbslam的中文注释的源码，可以在csdn博客中下载，网址：https://download.csdn.net/download/qq_30356613/10460918，但貌似只能积分下载已经设到积分最低了，之后将上传到github，后面会更新网址，因此在博客中我们就尽量只讲述理论知识以及梳理工程思路，至于具体代码实现以及代码的注释可以下载注释版源码查看。
接着上一篇对于ORB-SLAM的系统介绍继续ORB-SLAM的主线程tracking。
ORB-SLAM的tracking线程作为系统的主线程，也是SLAM前端视觉里程计的主要内容，实现的主要内容就是计算地图点以及相机位姿交给后端。具体实现流程为：
在进行追踪线程之前，首先对新的数据（单目相机下为rgb图片，双目相机下为两张rgb图片，rgbd相机下为rgb图片和深度图）进行处理，将数据转化为灰度图，并根据数据建立新帧数据（在构建新帧数据时对新帧数据进行一些预处理，比如提取图片特征点并计算关键点（位置方向）和描述子构建金字塔模型，对新帧的特征点位置根据矫正矩阵进行矫正）
一．Tracking的两个模式
视觉里程计分为两种模式：1.仅追踪模式 2.同步定位与建图模式
1. 仅追踪模式下：不插入新的关键帧，不添加新的地图点，局部地图线程不工作，而且回环检测线程也不会工作，只会追踪地图中现有的地图点。
2. 同步定位与建图模式：在追踪线程的同时有局部建图和回环检测
二．Tracking的四种状态
追踪线程有四种状态：1.NO_IMAGES_YET 2.NOT_INITIALIZED 3.OK 4.LOST
其中NO_IMAGES_YET表示当前没有图片，NOT_INITIALIZED表示当前没有初始化追踪线程，OK证明当前追踪线程完好，LOST证明当前追踪线程丢失——注意这里的线程状态都是指当前帧处理之前的状态。
根据这四种追踪状态，做出不同的处理：
1.处于NO_IMAGES_YET，当新的一帧来临时，将线程状态改变为NOT_INITIALIZED。
2.处于NOT_INITIALIZED，则针对单目相机和双目相机/RGBD相机进行不同的初始化
1>针对单目相机：单目相机的初始化是一个比较棘手的问题，会在另一篇文章（ORBSLAM的单目视觉处理方式）中单独介绍，这里主要介绍思路和流程。单目相机的初始化至少需要两帧，第一帧建立初始化器，设定该帧作为初始化参考帧。第二帧作为匹配帧，通过这两帧之间进行匹配，进而通过单应性矩阵和基础矩阵计算两帧之间的位姿以及匹配点的深度信息。初始化成功之后初始化地图（建立关键帧并设置关键帧属性（a添加关键帧中的地图点，b计算关键帧的BOW向量）将其加入全局地图map，建立地图点并设置地图点属性（a设置观察到该地图点的关键帧，b计算地图点的最优描述子，c计算地图点的平均观测方向和深度范围）将其加入全局地图map，更新关键帧之间的共视关系，将关键帧添加进局部地图线程中，更新追踪线程的局部关键帧集以及局部地图点集，更新当前参考帧，设置全局地图map的参考地图点（用于建图），在地图显示器中设置当前帧位姿，更新追踪线程状态为OK。）注意单目初始化阶段得到的深度信息同样具有不确定性，所以我们在初始化结束最后需要将得到的地图点的深度进行归一化，并且将位移向量也进行归一化，在此后的追踪线程中计算的地图点和相机位姿，都是相对于初始化时的距离来展开的。
2>针对双目相机和RGBD相机：由于双目相机和RGBD相机与单目相机相比可以计算出地图点确定的深度信息，所以初始化过程只需要一帧就可以，设置初始帧位姿,初始化地图（建立关键帧并设置关键帧属性（a添加关键帧中的地图点，b计算关键帧的BOW向量）将其加入全局地图map，建立地图点并设置地图点属性（a设置观察到该地图点的关键帧，b计算地图点的最优描述子，c计算地图点的平均观测方向和深度范围）将其加入全局地图map，更新关键帧之间的共视关系，将关键帧添加进局部地图线程中，更新追踪线程的局部关键帧集以及局部地图点集，更新当前参考帧，设置全局地图map的参考地图点（用于建图），在地图显示器中设置当前帧位姿，更新追踪线程状态为OK。）注意这里的初始化地图与单目相机的不同之处在于这里只需要一帧初始化帧，所以构建地图只需要一帧就好，而单目相机中构建地图中的关键帧地图点等都是构建两帧。
3.处于OK，经过初始化的系统追踪线程就转为OK状态，在没有丢帧或者是复位的情况下系统将一直处于OK状态。处于OK状态的系统就可以进行位姿估计，地图点追踪了，以同步定位与地图构建模式为例，讲解是怎么进行追踪的（仅定位模式只能追踪现有的地图点，所以如果在大场景下很容易丢失，比如说道路上，无人车上，这些情况下场景是不断变化的，所以仅定位模式是不能满足这些情况的）。
（1）首先检测上一帧中的所有地图点中有没有可以代替该地图点的其他地图点，如果有则将两地图点融合为一个地图点（用替代点替代当前帧中的地图点）
（2）用两种方式进行相机位姿的求解，并确定当前帧中所有特征点的内点和外点。一种是根据参考关键帧与当前帧匹配得到匹配地图点，将当前帧的初始位姿设为上一帧的位姿，然后将匹配点和位姿同时进行图优化，得到当前帧的位姿以及匹配内点。另一种是根据上一帧映射到当前帧来匹配得到匹配地图点，并根据上一帧的位姿和位姿变化速度得到当前相机的初始位姿，同样将匹配地图点和位姿加入到图优化中得到当前帧的位姿以及匹配内点。两种匹配方式我们将在另一篇文章中讲解（ORBSLAM中的特征匹配）。
（3）进行地图点追踪以及局部地图的更新TrackLocalMap()。更新局部地图，包括局部关键帧和局部地图点（局部关键帧中所有的地图点），在局部地图点中寻找当前帧的视野范围内的点，匹配局部地图视野范围内的点与当前帧，匹配点存储到当前帧的地图点容器中。根据新得到的匹配点重新优化相机位姿（位姿优化见另一篇文章（ORBSLAM中的优化问题））并得到匹配内点, 通过匹配内点的数量来判断当前局部地图是否追踪成功
（4）如果跟踪局部地图成功，则根据在追踪局部地图中得到的新的相机位姿更新运动模型以及清空临时地图点，删除地图点中是外点的地图点，更新参考关键帧（参考关键帧是所有关键帧中公式地图点最多的关键帧）。检测此时是否需要添加关键帧，这里添加关键帧是给局部地图线程添加关键帧，然后在局部地图中处理完成后将关键帧传递给回环检测。ORB中是怎么判定是否此时需要添加新的关键帧呢，基本是根据如下流程进行判断：
1>仅线程追踪模式不需要添加新的关键帧
2>局部地图模式被暂停或者被发起暂停请求(回环检测冻结局部地图)则不添加关键帧
3>上次重定位后没有足够的帧通过则不添加关键帧
4>追踪线程是比较弱的
5>此帧距离上次插入关键帧已经超过了最大的帧数(很久没插入关键帧了)
6>局部地图处于空闲状态并且没有超过最大关键帧数
7>局内点数量比较少,跟踪地图中地图点比较少
上述情况下时需要加入关键帧的。在需要加入关键帧的情况下执行CreateNewKeyFrame()函数，这个函数主要做的工作是创建关键帧，将关键帧中的近点加入地图中（注意只有在添加关键帧时才将地图点添加进全局地图mpMap中），然后将关键帧传给局部建图线程，更新上一关键帧id。
（5）根据上面的结果更新最终的相机位姿，相机参考点，当前帧的时间，以及当前追踪线程状态，这一步是为了存储完整的帧姿态信息以获取完整的相机轨迹。
4.处于LOST状态，上一帧追踪失败，当前帧进行重定位。重定位实现过程：
（1）计算当前帧的BoW映射
（2）找到与当前帧相似的候选关键帧
（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw（RANSAC+PNP算法）
（4）优化相机位姿Tcw
首先进行优化，将优化的结果存到nGood中，
如果优化结果不理想进行映射匹配，然后再进行优化
如果优化结果还不理想缩小映射窗口后在进行匹配并优化
此时若还不理想就判定该候选关键帧不能与本帧形成匹配，继续进行下一关键帧的匹配;如果可以，则证明已经进行重定位或者找到回环，退出循环
经过上述重定位之后如果失败，那么进行下一帧
如果成功则，追踪局部地图并添加当前关键帧，与上面的OK状态下的局部地图追踪类似。
重定位的进一步讲解请见文章（ORBSLAM的loopclosing）。
三．补充说明
上述为完整的tracking线程，针对上述进行如下补充说明：
1.应当注意维护的全局map和tracking线程的局部关键帧集和局部地图点集，以及localmapping线程中的关键帧集和地图点集存在区别，他们的更新在不同的时候进行。一新帧加入系统之后，首先是交给tracking线程进行处理，然后检测是否是关键帧（初始化帧或者是经过函数NeedNewKeyFrame()检验之后的帧），如果是关键帧，才将他交给localmapping线程处理，localmapping线程处理之后才交给回环检测线程。
2.而且只有当加入关键帧的情况下才进行全局map的更新，其余帧不会加入全局map，而tracking线程的局部关键帧集和局部地图点集则不同，他们两者的更新是在只要线程不LOST的情况下就会一直更新。更新是在：初始化帧和每一帧位姿计算之后的TrackLocalMap()函数中调用UpdateLocalMap()进行，其中更新先更新局部关键帧（a.当前帧中所有地图点在其余关键帧中观测到，将其加入 b.将所有看到地图点的所有关键帧的共视关键帧添加到局部地图 c.所有看到该地图点的所有父关键帧和子关键帧加入），更新地图点（所有局部关键帧的地图点））。
3.更新全局地图map，会做如下操作：建立地图点并设置地图点属性（a设置观察到该地图点的关键帧，b计算地图点的最优描述子，c计算地图点的平均观测方向和深度范围），建立关键帧并设置地图点关键帧属性（a设置该关键帧中的地图点 b.计算关键帧的BOW向量）。其中将关键帧加入地图map是在Tracking::CreateInitialMapMonocular()（单目初始化帧），Tracking::StereoInitialization()（双目及rgbd相机初始化帧），LocalMapping::ProcessNewKeyFrame()（其他关键帧插入）；地图点插入全局地图map是在Tracking::CreateInitialMapMonocular()（单目初始化帧），Tracking::StereoInitialization()（双目及rgbd相机初始化帧），Tracking::CreateNewKeyFrame()和LocalMapping::CreateNewMapPoints() (非单目其他关键帧地图点插入)，LocalMapping::CreateNewMapPoints()（单目其他关键帧地图点）。非单目地图点的插入可以直接在tracking中进行，因为他们可以直接得到深度信息，从而直接得到地图点的三维点坐标，而单目地图点求其深度信息，需要对极约束和三角化，而对于地图点的三角化和对极约束我们在localmapping线程LocalMapping::CreateNewMapPoints()函数中进行的，所以我们需要在局部地图线程中进行。
4．KeyFrameDatabase的维护
关键帧集是用来做回环检测和重定位的，通过关键帧集中的BOW向量来找候选关键帧，关键帧集中仅仅维护了mvInvertedFile成员变量来存储每个叶子节点下的所有关键帧。将关键帧添加进其中是在回环检测完成时，无论是否发现回环都将其加入关键帧集KeyFrameDatabase；如果一个关键帧发现是坏的，则将其从关键帧集中删除，在KeyFrame::SetBadFlag()中实现，至于什么时候认为该关键帧是坏的，参考（6）去除冗余的关键帧。
5. 应注意关键帧，地图点和地图三者之间的关系。我们在为全局地图添加关键帧和地图点的同时，需要更新关键帧与地图点的联系，为关键帧添加地图点同时为地图点添加可视关键帧，而且每个关键帧之间是有联系的，为了方便我们回环检测和重定位，我们将每个关键帧都添加共视关键帧（能看到相同的地图点，看到地图点的数量为两共视关键帧的权重）和父关键帧（权重最大的共视关键帧），其中共视关键帧更新是在将关键帧加入全局map之前调用UpdateConnections()函数进行的，其中初始化关键帧的共视关键帧在初始化函数Tracking::CreateInitialMapMonocular()和Tracking::StereoInitialization()中加入，其余关键帧的共视关键帧加入是在Tracking::StereoInitialization()函数中进行。
6.去除冗余关键帧和冗余地图点
冗余关键帧的删除是在localmapping线程中，当一组关键帧运行完成之后（当前新关键帧集中没有新的关键帧）时进行关键帧的融合，融合的是当前关键帧（当前组关键帧的最后一帧）及其共视关键帧（有相同地图点的关键帧）。融合的判定条件是：当前关键帧中有90%以上的地图点在其他关键帧中能够找到，则认为该关键帧是冗余的。去除冗余关键帧的方法时将该帧设为bad帧，KeyFrame::SetBadFlag()函数，做如下操作：（1）验证该帧是否可以被擦除（2）擦除所有本关键帧与关联关键帧之间的联系（3）擦除所有地图点与本关键帧之间的关联，标志本关键帧已经不能看到这些地图点,这些地图点也不会存在这些关键帧 （4）清空存储与本关键帧关联的其他关键帧变量，清空排序之后的关联关键帧序列（5）清空子关键帧 并找每个子关键帧的新的父关键帧（6）在地图点和关键帧数据集中剔除本关键帧
冗余地图点的删除是在localmapping线程中SearchInNeighbors()函数下进行的，对当前关键帧中的所有地图点与所有一级二级相邻关键帧（一级关键帧是指当前关键帧的共视关键帧，二级关键帧是指当前关键帧共视关键帧的共视关键帧）中的地图点进行检测融合，通过函数ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)将地图点投影到关键帧的方式进行特征匹配选取最优的匹配点，如果匹配点与当前点的描述子距离小于阈值的情况下进行融合。融合的方式是用被观察次数大的地图点代替被观察次数少的地图点，用函数MapPoint::Replace(MapPoint* pMP)融合，融合地图点作如下操作：（1）将被替换地图点的被观察次数,被查找次数,以及观察到该地图点的关键帧都清空,坏地图点标志置位（2）将被替换地图点的被观察次数,被查找次数都加到替换地图点pMP中,并将当前地图点在关键帧中的位置用代替地图点代替（3）最后将本地图点从全局地图map中删除。
7.tracking线程的位姿计算
Tracking线程中的位姿计算分为三个部分。
（1）根据关键帧追踪（TrackReferenceKeyFrame()）进行位姿计算。匹配方式使用关键帧BOW向量与当前帧进行匹配（见另一篇文章（ORBSLAM中的特征匹配问题）），通过该匹配方式得到地图点得到的地图点作为图优化节点，然后将上一帧的位姿作为初始位姿，用位姿图优化进行位姿优化（见另一篇文章（ORBSLAM中的优化问题））得到当前帧位姿。
（2）根据运动模型追踪（TrackWithMotionModel()）进行位姿计算。匹配方式使用上一帧特征点投影到当前帧的方式（见另一篇文章（ORBSLAM中的特征匹配问题））进行匹配，得到的匹配地图点作为图优化节点，然后根据上一帧的位姿和上一帧位姿的变换速度得到当前帧的初始位姿，用位姿图优化进行位姿优化（见另一篇文章（ORBSLAM中的优化问题））得到当前帧位姿。
（3）根据局部地图点的追踪（TrackLocalMap()）进行位姿计算。匹配方式是将局部地图点（除去（1）（2）已经匹配过的地图点剩下的局部地图点！！！）投影到到当前帧下（见另一篇文章（ORBSLAM中的优化问题））进行匹配，得到的匹配地图点作为图优化节点，然后根据上面（1）（2）得到帧位姿作为当前帧的初始位姿，用位姿图优化进行位姿优化（见另一篇文章（ORBSLAM中的优化问题））得到当前帧位姿。
