ORB-SLAM2代码阅读

• • Examples里面存放的分别是基于单目、双目、RGBD的实例程序
  • include文件夹存放的是头文件，ORB-SLAM2可以被当作一个库来使用，很多函数都可以直接调用
  • src文件夹存放的是和include对应的源文件
  • Thirdparty存放的是用到的第三方库
  • Vocabulary存放的是回环检测中BoW用到的视觉词典
  • build.sh，只要配置好了本地环境，运行一下build.sh就行了

System.cc

构造函数system()对SLAM系统初始化，传入ORB字典文件路径、配置文件路径、传感器类型。

1. 首先判断传感器类型是单目、双目还是RGB-D。
2. 然后通过cv::FileStorage fsSetting()传入配置文件路径的读取配置文件，并由isOpened()判断文件是否存在。
3. 若存在，则通过ORBVocabulary()加载ORB字典到mpVocabulary变量，然后通过loadFromTextFile()判断加载是否成功。
4. 接着创建地图Map()，创建视图FrameDrawer()用来显示创建的地图，创建画图器MapDrawer()。
5. 初始化三个线程Tracking()、LocalMapping()、LoopClosing()。并通过thread()开启local mapping和loop closing线程。同时初始化显示线程Viewer()并启动该线程。 
6. 最后通过setLocalMapper()等方法设置线程之间的指针。

TrackStereo()、TrackRGBD()、TrackMonocular()方法：首先判断输入传感器类型是否对应正确的方法，然后检查模型，如果是mbActivateLocalizationMode则休眠1000ms直到停止局部建图。如果是mbDeactivateLocalizationMode则重新开启局部建图的线程。然后分别通过GrabImageStereo(imRectLeft,imRectRight)、GrabImageRGBD(im,depthmap)、GrabImageMonocular(im)开启对应的tracking线程。

SaveTrajectoryTUM()方法求数据集中每一帧的位姿：GetAllKeyFrames()获得所有关键帧vpKFs——sort()对关键帧排序——vpKFs[0]->GetPoseInverse()获得第一帧相对于世界坐标系的位姿——遍历所有帧的位姿和参考帧【如果当前帧追踪失败则丢弃，继续追踪下一帧——追踪成功则将当前参考帧设为关键帧——追踪成功但是关键帧不好，则获取当前关键帧相对于上一帧的位姿，并将上一帧设为关键帧，依次不断的判断关键帧的质量，直到选取合适的关键帧——将关键帧的位姿乘第一帧相对于世界坐标的位姿得到关键帧相对于世界坐标的位姿——在将关键帧相对于世界坐标的位姿乘当前帧相对于关键帧的位姿得到当前帧相对于世界坐标的位姿——取当前帧相对于世界坐标位姿的R和t——转换为四元数——存入文件中】——关闭文件

SaveKeyFrameTrajectoryTUM()方法求数据集中每一关键帧的位姿。

rgb_tum.cc

1. LoadImages()加载图像——判断rgb图是否存在——判断rgb图与depth图数量是否对应相同。
2. ORB_SLAM2::System SLAM()初始化，创建SLAM系统，并初始化各个线程。
3. 遍历每一对RGB图和depth图【读取RGB图和depth图，读取时间戳（vTimestamps存储了时间戳，实际上就是存储了数据文件的每一幅图像的采集时间）——判断文件是否读取成功——SLAM.TrackRGB()将图片转给system处理，在这里从主线程进入tracking线程——计算track一帧的时间——计算两幅图像的采集时间之差，即采集一帧图像的时间——比较追踪时间和采集时间，通过休眠保持同步运行】
4. 关闭系统对象——计算追踪总时间以及时间中位数和平均数——SaveTrajectoryTUM()保存相机轨迹和SaveKeyFrameTrajectoryTUM()关键帧轨迹 

LoadImages()加载图像：读取图像的associate.txt文件，里面每一行有四个数据：rgb采集时间，rgb文件名，depth采集时间，depth文件名。其中文件名都是以采集时间命名的。——整行读取——第一个数据转换为double后存入vTimestamps向量中，第二个数据存入vstrImageFilenamesRGB中，第三个数据丢弃，第四个数据存入vstrImageFilenamesD中。

Tracking.cc

http://blog.csdn.net/chishuideyu/article/details/75314896
http://blog.csdn.net/chishuideyu/article/details/75560999
跟踪模块分析

GrabImageRGBD()：判断彩色图像的通道类型并通过cvtColor()方法相应转换为灰度图——Frame()【Frame.cc：ExtracORB()提取当前图像的ORB特征——消除扭曲——为网格分配关键点特征匹配】——Track()

Tracking::Track() ：

1. 判断tracking状态：如果是NOT_INITIALIZED则针对于单目和非单目分别执行MonocularInitialization()、StereoInitialization()初始化，并更新地图视图。

2. 对于初始化成功的，接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择 （1）只进行跟踪不建图                                                            （2）同时跟踪和建图，这里以（2）为例。初始化之后ORB-SLAM有三种跟踪模型可供选择。
 - TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。
 - TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点——将上一帧的位姿作为当前帧的初始值——通过优化3D-2D的重投影误差来获得位姿。
 - Relocalization()；重定位模型：计算当前帧的BoW——检测满足重定位条件的候选帧——通过BoW搜索当前帧与候选帧的匹配点——大于15个点就进行PnP位姿估计——优化。
 - 首先假设相机恒速（即Rt和上一帧相同），然后计算匹配点数（如果匹配足够多则认为跟踪成功），如果匹配点数目较少，说明恒速模型失效，则选择参考帧模型（即特征匹配，PnP求解），如果参考帧模型同样不能进行跟踪，说明两帧键没有相关性，这时需要进行重定位，即和已经产生的关键帧中进行匹配（看看是否到了之前已经到过的地方）确定相机位姿，如果重定位仍然不能成功，则说明跟踪彻底丢失，要么等待相机回转，要不进行重置；


3. 假如通过一种模型完成了初始相机的位姿估计，则进一步跟踪局部地图TrackLocalMap()，即和当前帧相关联的地图点做联合优化，获得一个较为准确的相机位姿；

4. 创建关键帧/剔除外点/当前帧置为上一帧等

StereoInitialization()初始化：判断当前帧的关键点是否大于500，满足条件，将当前帧创建为初始关键帧，并添加到地图中，遍历当前帧的所有关键点，并检查正向景深，再将当前帧的关键点创建为地图点并添加到地图中，并关联初始的关键帧，将当前帧设置为参考帧，将当前帧存入局部地图的关键帧序列中，关键点=地图点=局部地图点，获取当前帧的相机位姿。

TrackLocalMap()：更新局部地图，包括关键帧和地图点的更新；——搜索局部地图点是否符合跟踪要求，并匹配当前帧和局部地图点——优化位姿——根据匹配和优化结果更新地图点的状态，并判断匹配的内点数量，最后返回

NeedNewKeyFrame()：局部建图未开启，则不需要插入关键帧；局部地图开启，但是当前帧距离上一次重定位还没有经过一定数量的帧，则不需要插入关键帧；否则，判断局部建图时是否满足关键帧插入的条件（4+1）个条件：

1. 超过MaxFrame帧必须通过全局重定位。（确保良好的定位）
2. 局部建图闲置，超过MinFrame帧必须通过关键帧插入。（确保及时处理）
3. 至少追踪当前帧的点是参考帧点的一定比例。（确保良好的追踪）
4. 当前帧追踪比参考帧90%的点少。（确保最小视觉变化）
5. 追踪到的close点的满足一定的条件                                             

最后返回Boolean值

CreateNewFrame()

LocalMapping.cc

http://blog.csdn.net/u010128736/article/details/53395936

局部建图线程LocalMapping::Run()
主要任务：等待跟踪过程判断是否应该插入一个新的关键帧，并把关键帧插入到地图中，并对局部地图（跟踪过程中有一个局部地图跟踪）进行BA优化。具体细节包括三角化3D点，3D点的融合匹配，关键帧插入等。

1. SetAcceptKeyFrames(false)通知Tracking线程，LocalMapping处于忙碌状态，Tracking的关键帧发送不要太快
2. CheckNewKeyFrames()检查是否生成关键帧，及关键帧列表不为空——ProcessNewFrame()处理关键帧：计算BoW并插入到地图——MapPointCulling()上一步骤产生的不合格地图点，剔除地图点——CreateNewPoints()三角化创建新的地图点——CheckNewKeyFrames()判断关键帧队列处理完毕，则SearchInNeighbors()检查并融合当前关键帧与两级相邻帧间重复的地图点——已经处理完关键帧队列中的关键帧并且闭环检测没有要求停止LocalMapping，则进行局部BA优化——KeyFrameCulling()剔除冗余关键帧——InsertKeyFrame()插入关键帧到闭环检测队列

CheckNewKeyFrames()：返回关键帧列表是否为空

ProcessNewFrame()：计算当前关键帧的BoW，关联当前关键帧和新的地图点，更新当前关键帧的连接关系，将当前关键帧插入地图。

1. 首先从缓冲队列中取出一帧关键帧，设为当前关键帧，然后将栈顶的关键帧出栈
2. 计算该关键帧特征点的Bow映射关系
3. 跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
4. 遍历地图点，将当前关键帧生成的地图点添加到mlpRecentAddedMapPoints中，等待后续检查。而不是当前关键帧生成的地图点，更新该点的属性，将其设置为当前关键帧观察到的地图点，计算最佳的描述子，并获得该点的平均观测方向和观测距离范围。
5. 更新关键帧间的连接关系，Covisibility图和Essential图
6. 将该关键帧插入到地图中。

MapPointCulling()：剔除ProcessNewKeyFrame和CreateNewMapPoints函数中引入的质量不好的MapPoints
针对上一函数中的mlpRecentAddedMapPoints，遍历当前关键帧生成的地图点：

1. 直接剔除坏点
2. 跟踪到该点的帧数比预计可以观测到该点的帧数的比例小于0.25，则剔除该点
3. 从该点建立开始，到现在已经过了2个关键帧以上，但是观测到该点的关键帧数小于cnThObs(单目是2，非单目是3)，则剔除该点
4. 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点，因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测

SearchInNeighbors()：将当前帧和与当前帧有最高共视程度的nn个相邻关键帧做特征匹配，将匹配到的特征点进行三角化，生成3D点，然后检查正向景深、视差、重投影误差、尺度一致性，最后添加到地图点。

SearchInNeighbors()：检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints，并更新当前关键帧的地图点和与其他帧的连接关系。

1. 首先获得与当前关键帧具有最高共视程度的nn个关键帧，即是当前关键帧的一级相连帧，然后遍历一级相连帧，对每一个关键帧再获得与之具有最高共视程度的5个关键帧，即相当于当前关键帧的二级相连帧。
2. 将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合。
3. 将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合。
4. 更新当前帧MapPoints的描述子，深度，观测主方向等属性。
5. 更新当前帧的MapPoints后，更新covisibility图中当前关键帧与其它帧的连接关系。

KeyFrameCulling()：

1. 根据Covisibility Graph提取当前帧的共视关键帧。
2. 遍历该共视关键帧的MapPoints：判断该MapPoint是否同时被三个关键帧观测到，并且满足尺度约束条件
3. 统计以上满足条件的MapPoints
4. 判断以上统计的地图点数量是否大于有效地图点的90%。（有效地图点：不是坏点，且对于双目和深度相机，深度值满足一定条件）
5. 剔除上一条件不满足的共视关键帧。

LoopClosing.cc

http://blog.csdn.net/chishuideyu/article/details/76165461

闭环检测线程：LoopClosing::Run()
CheckNewKeyFrames()检查是否生成关键帧，及关键帧列表不为空——DetectLoop()检测回环——ComputeSim3()计算相似变换，对于单目是计算[sR|t]，对于双目或者深度相机，s=1——CorrectLoop()回环融合以及位姿优化

DetectLoop()：

1. 首先从闭环关键帧缓冲队列中取出一帧关键帧，设为当前关键帧，然后将栈顶的关键帧出栈
2. 判断距离上一次闭环检测是否超过10帧，不超过则不进行闭环检测，超过检测
3. 计算当前帧与相连关键帧的BoW最低得分minScore
4. 检测达到闭环候选帧DetectLoopCandidates()

    1. 获得与当前帧有共视关系的但并不相连的关键帧
    2. 获得当前关键帧与上述关键帧的最高单词数maxCommonWords
    3. 阈值minCommonWords=0.8*maxCommonWords
    4. 删选出大于阈值minCommonWords且评分得分大于minScore的关键帧
    5. 将上述筛选后的相连的关键帧分为一组，不相连的剔除（单独得分很高的无关匹配关键帧）
    6. 计算每一组最高得分bestAccScore
    7. 阈值minScoreToRetain=0.75*bestAccScore
    8. 大于阈值的为最后的候选关键帧vpLoopCandidates
5. 三个for循环检测候选关键帧的连续性

    1. 首先遍历候选关键帧——vpCandidateKFs
    2. 然后遍历当前帧及其相连帧（连续组）——mvCconsistentGroups
    3. 最后遍历候选关键帧及其相连帧（候选组）——spCandidateeGroup

最后筛选出的闭环关键帧应该满足：候选关键帧能在三个连续组中同时被选作候选关键帧。

ComputeSim3()：主要目的是通过计算当前帧和闭环关键帧的Sim3，以获得更多的匹配点，反过来再优化当前帧的位姿以及与当前帧相连的帧的位姿。

1. SearchByBow() 对每一个闭环帧，通过BoW的matcher方法进行第一次匹配，匹配闭环帧和当前关键帧之间的匹配关系，如果对应关系少于20个，则丢弃，否则构造一个Sim3求解器并保存起来。这一步主要是对效果较好的闭环帧构建Sim3求解器 
2. 对上一步得到的每一个满足条件的闭环帧，通过RANSAC迭代，求解Sim3。
3. SearchBySim3()利用Sim3再去进行匹配点的查找，本次查找的匹配点数量，会在原来的基础上有所增加。

    1. 当前帧的地图点投影到闭环帧上，得到更多的匹配点
    2. 闭环帧的地图点投影到当前帧上，得到更多的匹配点
4.  OptimizeSim3()在得到了第二次匹配的结果以后，要通过这些匹配点，再去优化Sim3的值，从而再精细化Rt和s
5. SearchByProjection()将闭环帧以及与其链接的关键帧所看到的所有的MapPoint都恢复出来。通过这个方法，可以尽可能得到我们当前关键帧所能看到的所有的地图点，为下一步做投影匹配，得到更多的匹配点做准备。 
6. 使用投影得到更多的匹配点，如果匹配点数量充足，则接受该闭环。

CorrectLoop()：