ORB-SLAM3 Tracking类源码分析（三）

2021SC@SDUSC

Tracking类最重要的方法是Track（），今天重点讲一下Track（）函数的流程

1.判断如果imu数据有问题（mbBadImu==true）,那么就重置当前的地图（ResetAvtiveMap()）

2.处理时间戳异常情况：

•      当前帧时间戳大于上一帧的时间戳，则清空imu数，创建新的地图
•      时间戳出现了大跳变（1s），如果imu没有完成BA2，则重置地图；如果完成了BA2，则拆功能键新地图

3.imu模式下设置bias参数，认为当前的帧的bias和上一帧的bias相同

4.imu模式下进行imu的预积分

5.如果还未进行初始化，则先初始化

•    单目/单目imu：MonocularInitialization()
•    双目/RGBD/双目imu：StereoInitialization()

6.已完成初始化，正常定位模式

      6.1    跟踪正常（mState==OK）

•  检查上一帧中被替换的地图点将地图点替换成新的地图点（在局部建图线程中可能会被替换）CheckReplaceInLastFrame()
•   如果运动模型是空的且imu未初始化，或刚刚完成重定位，则跟踪参考关键帧；否则用恒速模型进行跟踪，恒速模型跟踪失败再跟踪参考关键帧（KeyFrame）
• 如果跟踪失败（bOK==false）     1.如果刚刚进行重定位且为imu模式，则直接将mState置为LOST  2.否则如果地图中关键帧数量大于10，则置为recent_lost 3.其他情况直接置为LOST

      6.2  mState==RECENT_LOST

• 如果是imu模式则用纯imu进行跟踪，最多跟5s，还失败就置为LOST
• 纯视觉模式下进行重定位，重地位失败，就置为LOST

     6.3 mState==LOST 

•  如果地图中关键帧的数量小于10，则重置地图；否则新建一个地图，删除  mpLastKeyFrame，从头开始

7.已完成初始化，纯定位模式

• mState==Lost 进行重定位
• 跟踪正常（mbVO==false）恒速模型非空用恒速模型，否则跟踪参考关键帧
• 跟踪失败（mbVO==true）同时用恒速模型和重定位，重定位成功则相信重定位

8.更新参考关键帧，如果当前帧不是新的关键帧，则用上一时刻的参考关键帧，否则参考关键帧就是他自己

9.跟踪成功的话，则跟踪局部地图，寻找更多的匹配，优化当前帧的位姿（TrackLocalMap（））

9.1更新局部地图
          9.1.1更新局部关键帧

•  统计与当前帧有共视关系的关键帧及共视地图点数，找出共视程度最高的关键帧

• 将共视程度最高的关键帧及其共视关键帧、父子关键帧加入mvpLocalKeyFrames,如果是imu模式还要加当前帧之前连续的20帧
• 将与当前共视程度最高的关键帧设为参考关键帧

9.1.2更新局部地图点 将mvpLocalKeyFrames看到的地图点放入mvpLocalMapPoints中}

9.2将局部未匹配的局部地图点投影到当前帧，进行匹配SearchByProjection

9.3位姿优化。imu没有初始化或刚重定位：PoseOptimization
地图未更新（与上一帧距离近，误差小）：PoseInertialOptimizationLastFrame
地图更新了（关键帧被优化了，误差更小）：PoseInertialOptimizationLastKeyFrame

9.4 统计内点数，根据内点数返回跟踪成功

10.局部地图跟踪成功，mState==OK;局部地图跟踪失败但前面位姿跟踪成功，则imu模式置为recent_lost，纯视觉模式置为lost

11.如果跟踪成功或者recent_lost;更新恒速模式，计算上一帧到当前帧的位姿变换