ORB-SLAM（四）追踪
最近在读ORB-SLAM的代码，虽然代码注释算比较多了，但各种类和变量互相引用，看起来有点痛苦。索性总结了一下Tracking部分的代码结构，希望能抓住主要思路，不掉坑里。
作者的程序分为两种模式：SLAM模式和Localization模式。SLAM模式中，三个线程全部都在工作，即在定位也在建图。而Localization模式中，只有Tracking线程在工作，即只定位，输出追踪结果（姿态），不会更新地图和关键帧。Localization模式主要用于已经有场景地图的情况下（在SLAM模式下完成建图后可以无缝切换到Localization模式）。Localization模式下追踪方法涉及到的关键函数是一样的，只是策略有所不同。
以下介绍SLAM模式下的追踪算法。
主要流程
初始追踪（Init pose estimation）
作者在追踪这部分主要用了几种模型：运动模型（Tracking with motion model）、关键帧（Tracking with reference key frame）和重定位（Relocalization）。
下面一一介绍。
Tracking with motion model
假设物体处于匀速运动，那么可以用上一帧的位姿和速度来估计当前帧的位姿。上一帧的速度可以通过前面几帧的位姿计算得到。这个模型适用于运动速度和方向比较一致，没有大转动的情形下，比如匀速运动的汽车、机器人、人等。而对于运动比较随意的目标，当然就会失效了。此时就要用到下面两个模型。
Tracking with reference key frame
假如motion model已经失效，那么首先可以尝试和最近一个关键帧去做匹配。毕竟当前帧和上一个关键帧的距离还不是很远。作者利用了bag of words（BoW）来加速匹配。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配（参见ORB－SLAM（六）回环检测）；最后，利用匹配的特征优化位姿（参见ORB－SLAM（五）优化）。
Relocalization
假如当前帧与最近邻关键帧的匹配也失败了，意味着此时当前帧已经丢了，无法确定其真实位置。此时，只有去和所有关键帧匹配，看能否找到合适的位置。首先，计算当前帧的Bow向量。其次，利用BoW词典选取若干关键帧作为备选（参见ORB－SLAM（六）回环检测）；再次，寻找有足够多的特征点匹配的关键帧；最后，利用特征点匹配迭代求解位姿（RANSAC框架下，因为相对位姿可能比较大，局外点会比较多）。如果有关键帧有足够多的内点，那么选取该关键帧优化出的位姿。
姿态优化（Track local map）
姿态优化部分的主要思路是在当前帧和（局部）地图之间寻找尽可能多的对应关系，来优化当前帧的位姿。实际程序中，作者选取了非常多的关键帧和地图点。在跑Euroc数据集MH_01_easy时，几乎有一半以上的关键帧和地图点（后期>3000个）会在这一步被选中。然而，每一帧中只有200~300个地图点可以在当前帧上找到特征匹配点。这一步保证了非关键帧姿态估计的精度和鲁棒性。个人觉得这里有一定的优化空间。
更新局部地图（Local mapping thread）
这里简单提一下local mapping thread。Tracking成功以后，需要更新motion model，并判断当前帧是否是新的关键帧。如果是，将其加入并更新局部地图（local map），建立当前关键帧与其它关键帧的连接关系，更新当前关键帧与其它关键帧之间的特征点匹配关系，并利用三角法生成新的三维点，最后做一个局部优化（local BA，包括相邻关键帧和它们对应的三维点，参见ORB－SLAM（五）优化）。
程序
作者的这部分程序逻辑判断很多，如果读者对作者的程序有兴趣，那么可以参照下图来阅读追踪这部分的程序。
Map is active应改为Localization is active。从Localization is active开始，NO代表SLAM模式，YES代表Localization模式。
该系列的其它文章：
