【最终版本】ORB_SLAM2系统，代码详细总结

初始化部分(文章)：

1.对当前帧寻找合适的参考帧(匹配点的数目要足够多)。

2.进行单应矩阵和基础矩阵初始化，并且对每次初始化都打分，选择最高的两组分。

3.之后根据一个筛选机制(阈值)，判断使用H矩阵还是F矩阵。

4.这时直接三角化，8个点(F矩阵)或4个点(E矩阵)判断三角化后的点在两帧图像上是否都有较小的重投影误差，是否大多数的点都在极线上。如果大多数都不在，则重新初始化(这一步使得初始化过程变得鲁棒)。

5.之后如果初始化成功就进行Full BA，对整个滑窗进行Full BA

Tracking部分(代码)：

Tracking.cc

1. 先判断速度模型。速度模型正确则继续，trackreferencekeyframe()错误则与参考关键帧匹配，利用词袋加速匹配，如果匹配的点数小于15，则返回错误，若匹配点对大于15，优化时采用motion-only BA优化。优化的同时会把keypoint对应的3D点设置为外点。并且poseoptimization()函数会返回内点的数目。
2. TrackWithMotionModel()，里的第一个函数UpdataLastFrame()，将上一帧里的关键点和其深度通过反投影，将深度排序，传入到临时地图中。TrackWithMotionModel()，当前帧和上一帧通过投影匹配（searchforproject()），如果匹配点对小于20则失败，反之则成功。然后对当前帧和匹配到的地图点做motion-only BA优化，之后剔除外点。
3. TrackReferrenceKeyframe()，计算当前帧的词袋向量，利用词袋加速将当前帧和参考关键帧匹配，若匹配点的数目小于15，则返回false 跟踪失败。当前帧初值设为上一帧位姿，进行motion-only BA优化。之后剔除外点。
4. relocation是先计算当前帧的词袋向量，然后在之前的关键帧中寻找词袋相似对最高的候选关键帧序列，对关键帧序列中的每一个关键帧。利用词袋加速匹配，如果匹配的点对小于15，则把这个候选关键从序列中剔除，如果大于15，就建立一个PnPsolver。之后对这个候选关键帧序列，进行pnpsolver Ransac优化，迭代5次，迭代过程中会剔除一些关键帧(当Ransac reachs max)。对pnpransac 成功的帧，将其mappoints放入到当前帧的mvppoints中，这样mvppoints中的点数量就会越来越多(只加内点，外点剔除掉)，对当前帧进行motion-only BA。
5. TrackLocalMap： (UpdataLocalKeyframes)第一步根据当前帧看到的地图点，只要之前的关键帧有看到这个地图点就加入到LocalMap中去，第二步再加入与第一步的关键帧中相连的关键帧，但是总和不能超过80帧。(UpdataLocalPoints)把上述关键帧看到的地图点加入到一个mvpLocalMapPoints中去。若优化后的内点数目大于30，则成功。若(刚过去重定位1s的帧数，并且内点数目小于50)就判定失败，否则就成功。
6. NeedNewKeyframe()：条件1，距离上一帧重定位已经过去 fps 帧了；条件二，localmapping线程空闲；条件三：当前帧跟踪的内点数 小于 关键帧跟踪到的数目*一个阈值，或者 跟踪到的近距离点的数目在70-100之间；条件四，条件三 与 内点数要多于15.
7. CreateNewKeyFrame()：将当前帧设为关键帧，将当前帧中的关键点对应的深度，按照从小到大排序。最后根据每一个关键点和(RGB-D相机观测的深度或者是三角化的深度)，重投影到地图中，经过计算描述子，更新方向和深度后，加入到地图mpMap中。把当前关键帧放入到mpLocalMapper中，把当前关键帧做为参考关键帧。

Local Mapping部分(文章)：

a.关键帧的插入：更新covisibility graph，spanning tree ，计算关键帧的词袋向量。

b.地图点的删除：为了保存地图点，必须通过测试，才能真正被保存，这样才能保证可跟踪且不容易在三角化时出现较大误差。一个点要被加入Map，需要满足下面条件： 

（1）这个点要在可预测到能够观察到该点的关键帧中，有超过25%的关键帧能够跟踪到这个点； 
（2）如果一个地图点被构建，它必须被超过三个关键帧观察到（在代码中，可以发现如果是单摄像头，这个阈值被设置为2）。

注：地图点map point 中存储的信息：坐标；点被观测的平均方向(随着更新)；描述子(随着更新)；点能被观测到的最大最小距离，与尺度相关。

c.地图点的插入： 通过将检测到的ORB特征点，找到Covisibility Graph中与之相连的关键帧Kc，进行特征匹配，然后将匹配到的特征点进行三角化。对于没有匹配上的点，本文又与其他关键帧中未被匹配的特征点进行匹配。匹配方法使用的是之前的方法，并且将不满足对极几何约束的匹配点舍弃。ORB特征点对三角化后，检查1.正向景深、2.视差、3.反投影误差和4.尺度一致性，这时才得到地图点。一个地图点是通过两个关键帧观察到的，而它也可以投影到与之相连的其他关键帧中，这个时候可以使用Tracking部分的跟踪局部地图来在附近的关键帧中找到匹配(添加数据关联)。

d.Local BA :优化与当前帧相连接的关键帧(covisibility graph)，这些关键帧看到的地图点 同时又被其他一些关键帧看到的关键帧序列。 优化相机位姿和地图点坐标。

e.关键帧删除：

冗余地图点数目占地图点数目超过90%的局部关键帧.
冗余地图点：1)被超过3个其他关键帧观测到.

遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到，该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧。

关键帧删除：(1)剔除和地图点之间的关联关系 (2)更新连接图、Essential Graph和生成树.

知识点：

共视图(covisibility graph)：将每个关键帧作为一个节点，若两个关键帧存在一定数量（至少15个）共视地图点则这两个这两个关键帧之间存在一条边，边的权重为共视地图点的数量。

本质图(essential graph)：使用位姿图来校正闭环处的累积误差，由于共视图中边的数量较多，我们创建共视图的子图-本质图。本质图包含生成树、共视地图点数量超过100的边和回环的边

生成树(spanning tree)：ORB-SLAM系统在第一个关键帧开始就维护一个生成树，每个关键帧只与共视点最多的相邻帧产生边。

            

Loop Closing部分(文章)：

a.回环候选帧选择：首先计算 Ki和 convisibility graph 相连的帧最小相似度 smin，回环检测时在 recognition database 中去掉相似度小于 smin的帧，并且去掉 convisibility graph 中和当前帧相连的帧。在判断某一帧为回环时，还需要判断连续的 3 帧都为回环(时间一致性检验)。

b.计算相似变换：

（1）当前关键帧有关联地图点的特征与候选帧进行匹配，利用3D-3D的RANSAC算法，得到相似变换(7DOF)。如果这个方法寻找到足够的内点，则进行第二步。

（2）用初始估计的位姿获得更多的匹配点。再用附录的双边优化的策略，从而获得一个更准确的位姿估计值。

c.回环融合：首先将计算的相似变换作用到当前帧，并且传播到和当前帧连接的关键帧，通过这样做回环两端的地图可以对齐。 然后回环帧，和回环帧相连的关键帧能看到的所有地图点，向当前帧，和当前帧相连接的关键帧投影寻找匹配点，所有的这些匹配上的点会融合，从而需要在 convisibility graph 中更新当前帧及其周围的关键帧与回环帧之间的边连接。

d.位姿图优化：全局sim3优化(位姿图)+全局优化(新线程)

优化的参数：每个关键帧的 pose，优化参数 pose 是个相似变换，优化完后再作用到关键帧位姿，作用后，关键帧位姿仍然是个欧式变换，优化后也作用到地图点。优化的约束 ：这里的约束都是位姿约束，位姿约束指帧和帧之间的位姿变换。 具体的约束有： 回环形成的位姿约束；essential graph 图中的约束。