ORBSLAM2学习之Tracking线程

        之前阅读了ORBSLAM2代码的各个类，对整个代码的功能有了了解，接下来整理下几个线程的逻辑，这样才能更清晰，计划是用文字书写，其实原本用流程图更加合适一些，不过简单画了下发现很花时间，还是以后闲了再化成图吧。

ORBSLAM共有四个线程，分别是Tracking主线程，LocalMapping线程，LoopClosing线程和观测窗口线程，其中Tracking线程运行在主循环中，负责跟踪对每一帧计算位姿。当然除此之外在系统初始化时会开辟两个线程分别计算基础矩阵和单应矩阵，检测到闭环进行校正时会开辟线程运行一个全局的BA优化，不过这几个线程运行的次数比较少。

首先是Tracking线程，主要介绍下做了哪些事情，部分函数的功能。以单目为例

Tracking线程即前端了，负责简单的跟踪，类似VO，不过ORBSLAM中即使前端也做了位姿优化。

主函数进入后：

SLAM.TrackMonocular(im,tframe);   检查模式变化，定位还是跟踪

mpTracker->GrabImageMonocular(im,timestamp); 将图像转为灰度图，构造初始帧mCurrentFrame，若系统未初始化则特征提取器会提取更多特征。

Track(); 跟踪

Track()函数按照顺序：

1. 初始化MonocularInitialization()
2. 若系统不是仅跟踪模式(mbOnlyTracking = false)：首先检查并更新上一帧被替换的地图点CheckReplacedInLastFrame()，因为Local Mapping线程可能会改变地图点；之后若速度为空或者不久前刚做过重定位，则跟踪参考关键帧TrackReferenceKeyFrame()，否则根据速度模型进行跟踪TrackWithMotionModel()，若此时跟踪失败，则依然使用跟踪参考关键帧的函数进行跟踪。
3. 定位模式(mbOnlyTracking = true)：此时局部图不工作，若跟丢了则进行重定位Relocalization()；否则，若匹配的特征点数量较多，证明此时跟踪正常，则根据速度是否为空选择跟踪速度或是参考关键帧，若匹配的特征点很少了，少于10个(mbVO = nmatchesMap<10)，则既做跟踪又做重定位。
4. 跟踪局部图TrackLocalMap()
5. 若跟踪没问题，检查是否插入关键帧：首先更新速度模型中的速度，检查是否需要插入关键帧，需要则插入，删除BA中检测到的外点。
6. 若状态丢失，则系统重置(mState==LOST)

其中涉及到的函数及功能：

MonocularInitialization()单目初始化

• 创建单目初始化器，要求必须有连续的两帧的特征点个数都大于100才能进行初始化操作，若两帧之间的匹配点对少于100，则重新创建初始化器
• Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)函数进行初始化，根据匹配的特征点，开启两个线程分别计算基础矩阵和单应矩阵，计算得分比例，根据得分比例选取其中一个模型恢复旋转和平移
• 初始化成功后，删除无法三角化的匹配点，将第一针作为世界坐标系，将三角化得到的3D点包装成地图点（构造地图点，添加关键帧和点、点和关键帧之间的观测属性，计算最佳描述子，更新观测方向和距离范围，插入地图），构造初始地图
• 通过全局BundleAdjustment优化相机位姿和关键点坐标
• 将场景的中间深度值归一化到1，将基线归一化，3D点的尺度也归一化到1

TrackReferenceKeyFrame()跟踪参考关键帧

• 计算当前帧的Bow
• 当前帧存在一个参考关键帧，通过Bow匹配两帧之间的特征点
• 将上一帧的位姿作为当前帧的位姿的初值进行优化Optimizer::PoseOptimization(&mCurrentFrame)，这里仅优化帧的位姿，不优化地图点的坐标，之后剔除优化后的外点

TrackWithMotionModel()根据速度模型进行跟踪

• 根据速度模型计算（预测）当前帧位姿 mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
• 将上一帧的地图点投影到当前帧进行特征点匹配SearchByProjection()，之后和跟踪参考帧一样优化当前帧位姿，剔除外点

TrackLocalMap()跟踪局部图

• 更新局部关键帧(mvpLocalKeyFrames) UpdateLocalKeyFrames()：有以下几种帧作为局部图中的关键帧，一：遍历当前帧的地图点，记录所有能观测到这些点的关键帧；二：与一中的帧共视程度最高的10个关键帧、自己的子关键帧、父关键帧；以上帧全部作为局部图中的关键帧
• 更新局部地图点(mvpLocalMapPoints) UpdateLocalPoints()：遍历局部图中的关键帧，将每一帧的地图点加入局部地图点中，当然要注意防止重复添加
• 匹配当前帧和局部图中的地图点SearchLocalPoints()
• 进行一次位姿优化，之后更新当前帧地图点的观测程度，统计跟踪效果

NeedNewKeyFrame()是否插入关键帧

• 不插入关键帧的情况：选择了重定位模式、局部图被闭环检测使用、距离上一次重定位刚过去不久
• 插入关键帧：长时间没有插入关键帧、局部图空闲、跟踪效果较弱

Relocalization()重定位

• 先计算当前帧的BOW值，并从关键帧数据库中查找候选的匹配关键帧
• 构建PnP求解器，标记杂点，准备好每个关键帧和当前帧的匹配点集
• 用PnP算法求解位姿，进行若干次P4P Ransac迭代，并使用非线性最小二乘优化，直到发现一个有充足inliers支持的相机位置
• 返回成功或失败