ORB-SLAM2中LocalMapping线程主要流程梳理

一、LocalMapping线程简述

在ORB-SLAM2当中，LocalMapping作为一个线程在系统启动后就一直在运行，只是在Tracking线程“生产出”新的关键帧并插入到LocalMapping线程中的mlNewKeyFrames列表后，LocalMapping线程检测到该列表中有关键帧后开始处理这些新插入的关键帧(CheckNewKeyFrames函数用来检测mlNewKeyFrames中是否有关键帧)，没有关键帧插入的时候LocalMapping线程处于“空闲的睡眠状态”。

LocalMapping的字面意思是“局部建图”，这里指的是新的关键帧周围的地图（主要是共视关键帧和地图点），相对与全局地图来说新的关键帧周围的地图就是局部地图。那么，LocalMapping线程要做什么就大概比较清楚了，就是为新创建的关键帧进行局部建图，也就是创建或者更新该关键帧的共视关键帧和地图点信息，这些信息明确后局部地图就创建好了。

二、LocalMapping线程中的处理流程

1.处理新插入的关键帧(ProcessNewKeyFrame)

    计算新关键帧的BoW(bag of word)；
    遍历新关键帧的地图点，判断每个地图点的关键帧观测列表（能观测到当前地图点的所有关键帧组成的map）中有没有新的关键帧。
        如果没有的话对地图点做下面三项处理：
            1）新的关键帧加入到地图点的关键帧观测序列当中；
            2）更新地图点的平均深度；
            3）计算地图点的描述子，这里地图点选择的描述子是所有描述子中距离其他描述子平均距离最小的那个描述子；
        如果有的话（这种情况仅出现在通过Tracking线程插入的双目地图点，也就是说对于单目不可能出现）：
            将该地图点加入到mlpRecentAddedMapPoints列表当中；
    更新当前新关键帧的连接关系，也就是在共视图当中的连接关系mpCurrentKeyFrame->UpdateConnections()；
    将当前新关键帧插入到地图当中；

2.剔除地图中新添加的质量不好的地图点MapPointCulling

    以下四种情况的地图点都要剔除：
     1）地图点设置了badflag的。设置badflag的函数是MapPoint::SetBadFlag()；
     2）地图点实际被观测率小于0.25。这里的实际被观测率指的是一个地图点的mnFound和mnVisible的比值：mnFound/mnVisible。
          mnFound为经过优化后该地图点仍为inlier，此时可以观测到该地图点的关键帧数；
          mnVisible表示未经过优化钱该MapPoint可以被多少个关键帧观测到的关键帧的数量；

     3）第一次观察到该地图点的帧id与当前帧id相隔>=2并且和该地图点被观察到的帧的个数小于阈值（单目阈值为2，非单目为3）；
     4）第一次观察到该地图点的帧id与当前帧id相隔>=3；

3.对当前关键帧和其共视程度最高的关键帧之间通过对极约束和三角化创建新的地图点CreateNewMapPoints

    1）获取和当前关键帧共视化程度最高的20个关键帧（非双目为10个）；
    2）遍历1）中获取的和当前关键帧共视化程度最高的关键帧列表；
       a)当前关键帧的相机中心和遍历的关键帧相机中心相减可获得基线向量（进而得到基线长度）。
            单目情况下计算遍历关键帧的基线和场景深度中值之比，如果<0.01则不能进行三角化；
            非单目情况下，基线长度小于遍历关键帧的基线长度时不能生成3D地图点；
       b)计算当前关键帧和遍历关键帧之间的基础矩阵；
       c)用对极约束来查找当前关键帧和遍历关键帧之间满足对极约束关系的特征点，并进行特征点匹配（SearchForTriangulation）；
       d)遍历c)中两个关键帧之间匹配的特征点，通过三角化创建地图点。并添加地图点的观测帧、将地图点插入到当前关键帧和遍历关键帧的地图点列表中、计算地图点的描述子、更新地图点的深度、将地图点加入到地图当中；   

4.在当前新关键帧的两级共视关键帧中查找匹配的地图点并进行地图点的融合，更新地图点信息(SearchInNeighbors)

   1）获取当前新关键帧共视关系最好的关键帧(单目最多获取前20个，其他最多获取前10个)以及这些共视关键帧的共视关键帧(最多5个);
   2）遍历1）中获取的共视关键帧，将这些关键帧和当前新关键帧中的地图点进行融合。融合方法(Fuse)如下：
       a)遍历当前新关键帧的地图点，对每一个地图点计算其在和当前关键帧共视的共视关键帧中的相素投影坐标，并判断所计算的像素坐标是否在共视关键帧当中，不在则继续遍历下一个地图点，在则执行b）操作；
       b)计算地图点的深度范围和地图点在共视关键帧中的深度，并判断深度是否在深度范围内，如果不在深度范围内则继续遍历下一个地图点，否则接着执行c)操作；
       c)判断共视关键帧观测地图点的方向角是否小于60度。如果小于60度，则返回a）中继续遍历地图点；大于等于60度，则继续执行d);
       d)地图点会在共视关键帧中投影出一个特征点的像素坐标，此时在共视关键帧中查找该特征点（在像素坐标的一定范围内）。如果没查到则继续遍历地图点；如果查到了匹配的特征点则接着执行e);
       e)遍历d)中查到的特征点，计算重投影误差，如果误差在一定范围内（单目5.99，非单目7.8），则继续比较描述子的距离，描述子距离最小的特征点为最佳匹配点；
       f)在描述子的最小距离小于最小阈值的情况下，在共视关键帧中查找最佳匹配特征点对应的地图点。
         如果查找到了地图点，并且地图点是ok的，此时查找到的地图点的观测帧数大于当前地图点的观测帧数，则用查找到的地图点替换当前地图点，否则用当前地图点替换查找到的地图点；
         如果没有查找到地图点，则对当前地图点添加观测帧，并将当前地图点加入到共视关键帧当中；
   3）将当前关键帧的所有共视关键帧的地图点进行融合；
   4）更新当前新关键帧的地图点描述子和深度；
   5）更新当前新关键帧在共视图中和其他关键帧的连接关系（其实就是共视关系）；

5.对和当前关键帧相连的关键帧和地图点做局部BA优化(Optimizer::LocalBundleAdjustment)

   这里使用g2o来进行图优化，优化的是局部地图当中关键帧的位姿和地图点的3D坐标。

   优化参与的对象有：

   1）对当前关键帧以及与其存在共视关系的关键帧组成的所有关键帧；

   2）1）中的所有关键帧所能观测到的所有地图点；

   优化是一种约束，这里将参与优化的所有局部地图的关键帧（其实也包括能看到局部地图当中地图点的非局部关键帧）的位姿和所有地图点的3D坐标作为g2o优化边的顶点，对其建立最小二乘的重投影误差表达式。在满足总体误差尽可能小的情况下求解得到关键帧的位姿和地图点的3D坐标，也是就优化所得的结果。此时肯定有一些地图点不满足优化约束，这些点称为outlier，是要被剔除掉的。

6.局部地图关键帧剔除KeyFrameCulling

   遍历当前关键帧的所有共视关键帧，通过共视关键帧的地图点以及能够观测到地图点的关键帧个数进行判断。如果一个共视关键帧中的90%的地图点能够被不少于3个其他关键帧看到，则认为该共视关键帧是冗余的，此时就剔除该共视关键帧。其实道理比较好理解，举个例子：如果在一个小团体里一个人能干的事情，许多人都能干，那么这个人就被认为是“冗余”的，就可以剔除他（让其他人替代）。

7.将当前关键帧插入到闭环检测线程的关键帧列表当中mpLoopCloser->InsertKeyFrame，闭环检测线程中就会对新插入的关键帧做闭环检测操作。

三、一些说明

1.LocalMapping中每个地图点都会被多个关键帧观测到，那么这个地图点的描述子应该选哪个？

理论上来说，所有能看到该地图点的关键帧中都有该地图点对应的描述子。所以，需要从所有关键帧中选择最优的描述子。这里地图点选择的描述子是所有描述子中距离其他描述子平均距离最小的那个描述子。

2.LocalMapping中每个地图点会被多个关键帧观测到，每个关键帧中对于该地图点都有一个深度，那么该地图点的深度应该怎样计算？

遍历能够观测到该地图点的关键帧，计算地图点的3D坐标距离每个关键帧相机中心的距离，求解平均距离作为该地图点的深度。

3.什么是关键帧的共视图？什么是两级共视关键帧？

一个世界坐标系当中的地图点能够同时被多个关键帧观测到，那么这些关键帧就存在共视关系。和当前关键帧能够共同观测到多个世界坐标系当中地图点的关键帧就构成了共视图。
当前关键帧的共视关键帧组成其第一级关键帧，第一级关键帧中的每个关键帧的共视关键帧组成了当前关键帧的第二级共视关键帧。

4.重投影误差的简单理解

重投影的过程是世界坐标中的3D点投影到相机的像素坐标中的过程，也就是计算世界坐标系下的一个地图点在相机坐标（图像帧）中所匹配的特征点。那么，投影所得的特征点的坐标和实际图像帧中特征点的坐标是有一定误差的（描述子也不可避免存在距离），这个误差就是重投影误差。