ORB-SLAM2中Tracking线程主要流程梳理

Tracking线程中首先加载图片帧数据集，接着从图片帧中提取ORB特征点，后边就是初始化部分，初始化完成后就是跟踪部分。本篇笔记对初始化部分和初始化完成后的部分进行如下简单梳理。

 

一、初始化部分：

1.首次进入初始化
  对于当前帧中关键点数>100的帧，用该帧来构造初始化帧和上一个帧；
  并记录当前帧中关键点的坐标值，用于下一帧进来的时候进行特征点匹配；
  使用当前帧构造初始化器；

2.第二次进入初始化
  如果当前传入的帧的关键点数<=100的帧，删除初始化器，直接返回；
  对当前传入的帧和初始化帧之间进行特征点匹配，调用matcher.SearchForInitialization函数；
         这里如果匹配的特征点个数<100,则释放初始化器并直接返回；
  使用初始化器进行初始化（调用Tracking.cc:MonocularInitialization-->mpInitializer->Initialize函数）：
         1）这里是2d-2d根据对极几何求解基础矩阵和单应矩阵；

         2）进而从基础矩阵或者单应矩阵中分解出旋转矩阵和平移变量，并进行三角化计算出了特征点的3D世界坐标；
  对于Initialize函数初始化完成后不能三角化的匹配特征点都进行删除；
  为当前帧设置位姿（也就是上边Initialize中求出的旋转矩阵和平移变量组成的变换矩阵）；

  初始化Map相关操作
  在CreateInitialMapMonocular中为关键帧初始化创建了地图点，并将关键帧插入到地图当中，并更新了地图中关键帧和地图点的观测关系。之后对地图做了全局BA优化GlobalBundleAdjustemnt-->BundleAdjustment：
      添加顶点：
         1）BundleAdjustment当中进行g2o优化，遍历关键帧并将关键帧位姿（变换矩阵）作为图的顶点；
         2）遍历地图点并将地图点作为图的顶点，估计量为地图点的3d坐标；
      添加优化边：通过遍历每一个地图点对应的关键帧，将关键帧的位姿和地图点的3d坐标构成的点作为边的两个顶点来构造优化边。
  获取优化后关键帧的位姿和地图点对应的3d坐标，更新关键帧和地图点中的相关值。

二、初始化完成：

跟踪参考关键帧TrackReferenceKeyFrame：
  1.用当前帧追踪参考帧（通过特征匹配）：Tracking::TrackReferenceKeyFrame()-->matcher.SearchByBoW：将当前传入的帧中特征点的描述子和参考关键帧中地图点的描述子进行比较，找到匹配关系；
  2.用和当前帧中特征点的描述子匹配的参考关键帧的地图点更新当前帧的地图点列表；
  3.进行当前帧位姿的优化：Optimizer::PoseOptimization,当前帧的初始位姿为参考关键帧的位姿，然后根据1当中的匹配关系进行优化，那么对应关系就变成了3D-2D，使用PnP(这里是P3P)来优化当前帧的位姿;
  4.在2中优化完当前帧位姿之后，对于当前帧对应的地图点属于outlier的，进行剔除不在地图当中显示。

使用运动模型进行跟踪TrackWithMotionModel：
  1.更新上一帧信息UpdateLastFrame：
  2.设置当前帧的位姿；
  3.matcher.SearchByProjection：
    1）获取当前帧和上一帧的旋转和平移矩阵；
    2）遍历上一帧的地图点，对于非outlier的地图点将其三维坐标转换为当前帧中的像素坐标（三维坐标-->相机坐标-->相机归一化坐标-->像素坐标），并判断所得的像素坐标是否在当前帧的有效范围内；
    3）根据像素坐标值(u,v)在当前帧中的有效半径内进行特征点匹配；
    4) 遍历3）中的匹配列表，用上一帧地图点的描述子和匹配的特征点的描述子计算距离，获取描述子距离最小的特征点为和该地图点匹配的特征点，并计算匹配的特征点数量；
  4.如果3中匹配的特征点数量<20，则扩大匹配的有效半径为原来的2倍，再次调用matcher.SearchByProjection进行投影匹配，如果匹配的特征点数量还是小于20则跟踪失败做返回处理；
  5.优化当前帧的位姿Optimizer::PoseOptimization(&mCurrentFrame)；
  6.优化位姿后剔除outlier的mvpMapPoints抛弃杂点，并记录匹配成功的特征点个数。在仅进行跟踪的情况下匹配点数大于20则跟踪成功；在跟踪和定位同时进行的情况下匹配点数大于等于10则成功。

重定位Relocalization（有三种情况需要进行重定位：1.初始化未成功；2.跟踪丢失；3.仅跟踪的情况下，跟踪匹配的特征点数小于10；）：
  1.计算当前帧中特征点的BOW；
  2.在关键帧数据库中检测当前帧的候选关键帧mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
  3.遍历2中检测的候选关键帧列表，调用matcher.SearchByBoW将候选关键帧和当前帧进行BoW匹配。
    匹配特征点数小于15则继续匹配下一个候选关键帧；
    匹配点数>=15则构造PnP(调用PnPsolver)求解器，并设置Ransac(调用pSolver->SetRansacParameters)参数；
    注意：这个PnP求解器中的3D point为候选关键帧中对应的MapPoint，2D点为mCurrentFrame中的关键点
         因为是重定位，所以就是求重定位的候选帧对应的MapPoint到当前帧的关键点之间的投影关系，通过投影关系确定当前帧的位姿，也就进行了重定位。
  4.遍历和当前帧匹配点数>=15的候选关键帧，通过3中构造的PnP求解器进行迭代优化求解；
  5.对于4中优化求解得到的当前帧的位姿进行优化，（调用Optimizer::PoseOptimization(&mCurrentFrame)进行优化），优化完成后对当前帧中的outlier的地图点设置为NULL;
  6.如果第5步中位姿优化求解获得的内点数少于50个，则在一个更大半径的区域中去进行匹配（调用函数为matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);）；
     6.1.对于第6步中匹配的特征点数加上第5步优化后的内点数总个数大于50的情况下，再次对当前帧的位姿进行优化（Optimizer::PoseOptimization(&mCurrentFrame)）；
     6.2.对于第6.1步位姿优化后的inlier点数>30并且<50的情况，再次在一个更小的窗口中进行投影；
       如果投影后的匹配点数加上第7步位姿优化的inlier点数之和>=50，则再次进行位姿优化（Optimizer::PoseOptimization(&mCurrentFrame)）；
  7.如果第5步或者6.2步中所获得的inlier点数>=50，则停止Ransac操作并更新当前帧id为上一次重定位帧的id。
  至此，当前帧就在历史关键帧中找到了匹配的关键帧，该关键帧的位姿也就是当前帧的位姿，完成了定位。如果遍历完了所有的候选关键帧后，还是没有找到足够的inlier，那么重定位失败。

更新和当前帧进行匹配的参考关键帧为当前帧的参考关键帧；

跟踪局部地图（TrackLocalMap）：
  1.更新局部地图（UpdateLocalMap）:
      1)更新局部关键帧：局部关键帧是指能看到当前帧对应的地图点的所有关键帧组成的关键帧列表；
         在局部关键帧列表中的关键帧数不够80个的时候，这时候需要通过下面3个策略来给局部关键帧列表中添加关键帧：
              a)遍历局部关键帧列表中的关键帧，获取和关键帧共视程度最高的前10个（少于10个的有几个就加入几个）关键帧加入局部关键帧列表当中；
              b)遍历局部关键帧列表中的关键帧，将关键帧的孩子关键帧加入到局部关键帧列表当中；  （什么是孩子关键帧？孩子关键帧就是和当前关键帧有共视关系的关键帧）
              c)遍历局部关键帧列表中的关键帧，将关键帧的父亲关键帧加入到局部关键帧列表当中；  （什么是父亲关键帧呢？父亲关键帧是和当前关键帧共视MapPoint个数最多的关键帧）
      2）更新局部地图点：
           a)遍历局部关键帧，获取每一帧中的地图点列表，将有用的地图点存入局部地图点列表当中，并更新地图点的跟踪参考帧为当前帧的id;
 2.在局部地图中查找与当前帧匹配的MapPoints，并建立关联（SearchLocalPoints）：
      1）遍历局部地图点，对当前帧对应的地图点之外的地图点判断其是否在可视范围内（isInFrustum），对于在可视范围内的地图点的观测帧数目+1操作，并统计可视范围内的地图点总体个数；
      2）对当前帧视野范围内的局部地图点通过投影和当前帧中的特征点进行匹配。
 3.对当前帧的位姿进行优化：Optimizer::PoseOptimization。在更新了局部地图点并将当前帧和局部地图点建立关联后，再次调用优化方法对当前帧的位姿进行优化。
 4.统计当前帧的地图点被局部地图中其他关键帧观测情况（匹配情况），如果匹配成功的数量>=30则为跟踪布局地图成功。

更新绘制帧的窗口：mpFrameDrawer->Update(this)

在绘制地图的窗口中设置当前相机的位置：mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw)；

清理当前帧的地图点中没有被其他关键帧观测到的地图点，将该地图点设置为NULL;

创建新的关键帧CreateNewKeyFrame：
  1.使用当前帧创建新的关键帧，并设置新创建的关键帧为当前帧的参考关键帧；
  2.对于非单目相机的情况处理：
     1）更新当前帧的旋转矩阵、平移和相机中心等变量；
     2）创建地图点，并将地图点插入到地图当中；
  3.往局部地图中插入新创建的关键帧：mpLocalMapper->InsertKeyFrame(pKF)；至此LocalMapping线程有了关键帧插入就运行起来了，关键帧巧妙的将各个线程之间串联了起来。
  4.更新记录的上一个关键帧和关键帧id：
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;

剔除当前帧中为outlier的地图点；

如果初始化完成后很快状态跟丢了，则将系统进行重置；

用当前帧来更新上一个帧；

存储帧的位姿信息，稍后用来重现完整的相机运动轨迹；

---

本篇笔记为笔者梳理的Tracking线程的主要流程，方便在脑海中进行推演，我们可以描述出Tracking线程中跟踪是怎么做的。

上边已经用红色加粗标注出了Tracking线程中进行全局BA优化的地方，也就是在初始化中。

也用红色加粗字体标注除了Tracking线程中进行位姿优化的地方，这样方便进行梳理。