【ORB_SLAM2源码解读】LocalMapping线程执行流程

LocalMapping 线程在 SLAM 初始化时创建， LocalMapping::Run() 函数是LocalMapping 线程的主要函数
LocalMapping::Run()函数包含了LocalMapping线程的所有处理逻辑，执行过程中运用了大量的全局变量和布尔类型变量来实现函数接口之间，线程之间的通信，当然还有锁的机制，保障主线程的正常有序的运行。我觉得可以把主线程理解为一个可以正常运转的机器，一个机器肯定是有子功能模块的，可以理解为子线程或者并行线程，但是还是主线程说了算，你要时刻让主线程知道子线程的状态，我好实时做出响应，虽然很庞大，但是总体的架构在那里，无非是细节的差异，所谓"抓大放小"是也。

先来看一下LocalMapping构造函数

LocalMapping::LocalMapping(Map *pMap, cosnt float bMonocular):
mbMonocular(bMonocular),
mpMap(pMap),// 可以看到传入的参数是地图
mbAbortBA(false),BA操作的信号变量的初始值是false
mbAcceptKeyFrame(true)处理关键帧操作的初始值是true

LocalMapping 线程进行如下操作

1. 插入关键帧；
2. 当前帧地图点剔除；
3. 创建新的地图点；
4. 局部 BA；
5. 局部关键帧剔除。

LocalMapping* mpLocalMapper;
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

A b = A(1);  //栈中分配 
A* c = new A(1);  //堆中分配  
mpMap = new Map();
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

1、通过SetAcceptKeyFrames(false)让tracking线程知道LocalMapping线程正在处理关键帧
下面两句代码可以实现tracking线程和LocalMapping线程通信
mpLocalMapper->SetTracker(mpTracker);//mpTracker=pTracker;
mpLocalMapper->SetLoopCloser(mpLoopCloser);//mpLoopCloser = pLoopCloser;
2、判断std::list<KeyFrame*> mlNewKeyFrames关键帧列表是否有关键帧
3、计算该关键帧所有特征点的词袋向量 mpCurrentKeyFrame->ComputeBoW();
4、当前帧的所有地图点的指针均存放在mvpMapPoints（mvp代表：member、vector、pointer）vector中

是否是关键帧的判断在 Tracking 线程中进行，
关键帧、地图点插入到地图在 LocalMapping 线程进行

2D特征点cv::KeyPoint和3D地图点ORB_SLAM2::MapPoint*之间的关系

首先每一帧图像会提取很多的特征点->Number of KeyPoints->int N
假设每个2D特征点(std::vector)都可以计算出3D地图点(std::vector)

2D特征点cv::KeyPoint

KeyPoint通过类封装的了一些属性和接口，值得注意的是_octave属性，通过它可以获取特征点提取自哪一个金字塔层级。

3D地图点ORB_SLAM2::MapPoint*

MapPoint通过类封装的了一些属性和接口，值得注意的是MapPoint的Pos属性，每个MapPoint都有一个在世界坐标系下面的位置和姿态；
每个MapPoint都来自于一个普通帧或者是关键帧的特征点，通过特征点的 idxF也就是特征点的编号来标记KeyPoint和MapPoint之间的对应关系。

mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL)); 

其中有N个空指针，因此有的位置上的MapPoint并没有指向实际的地图点（虽然对应有特征点，有索引idx，但是是外点），获取时需要注意。

InsertKeyFrame

这是插入关键帧的函数，注意插入的是std::list，执行插入操作时，不进行BA优化
void LocalMapping::InsertKeyFrame(keyFrame *pKF){
    std::list<KeyFrame*> mlNewKeyFrames;
    mlNewKeyFrames.push_back(pKF);
    mbAbortBA = true;
}

void LocalMapping::InterruptBA(){
    mbAbortBA = true;
}

bool LocalMapping::CheckNewFrames()
{
    std::list<KeyFrame*> mlNewKeyFrames;
    return(!mlNewKeyFrames.empty());
}

---

通过下面两个函数传递是否进行关键帧插入的信号
bool LocalMapping::SetAcceptKeyFrames(bool flag){
    mbAcceptKeyFrames=flag;
}

bool LocalMapping::AcceptKeyFrames(){
    bool mbAcceptKeyFrames;
    mbAcceptKeyFrames(true)// 初始值
    SetAcceptKeyFrames(true);// 通过这个变量改变这个值为true
    SetAcceptKeyFrames(false);// 通过这个变量改变这个值为false
    mbAcceptKeyFrames;
}

---

ProcessNewKeyFrame

开始处理关键帧 
void LocalMapping::ProcessNewKeyFrame(){
    {
    　　 // 从 std::list mlNewKeyFrames 列表的头部取出一帧，赋值给 mpCurrentKeyFrame
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        // 从 std::list mlNewKeyFrames 列表中删除一帧
        mlNewKeyFrames.pop_front();
    }
　　// 计算当前帧的词典
    mpCurrentKeyFrame->ComputBow();

    // 存储地图点的一个vector下标是特征点的索引，值是地图点
    // 下面三个函数共同获取地图点的vector
    std::vector<MapPoint*> mvpMapPoints;
    void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx){
        mvpMapPoints[idx]=pMP;
    }

    vector<MapPoint*> KeyFrame::GetMapPointMathces(){
        return mvpMapPoints;
    }
    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();

SetBadFlag

• bool类型变量mbBad初始值是mbBad(false)
• 通过函数SetBadFlag()去改变
• • 通过isBad()函数传递

bool isBad();
bool MapPoint::isBad(){
    return mbBad;
}

• 如果一个地图点是坏点首先要做如下工作

void MapPoint::SetBadFlag()
{
    map<KeyFrame*, size_t> obs;
    {
        mbBad = true;
        obs = mObservations;
        mObservations.clear();
    }
    for(map<KeyFrame*, size_t>::itreator mit=obs.brgin(), mend=obs.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        pKF->EraseMapPointMatch(mit->second);
    }
    maMap->EraseMapPoint(this);
}

0.  std::map<KeyFrame*,size_t> mObservations;
    pNewMP->AddObservation(pKFini,i);
    mObservations[pKF]=idx;


1.  将这个地图点的观测mObservations存储到新的obs中,然后执行mObservations.clear()
    注意观测量的数据结构是std::map, 这种数据结构的特点是键值一一对应, 所以同一个帧作为键值只能在这个结构中出现一次,
    最终得出的结论就是每一个地图点都有一个std::map去存储这个地图点来自于哪一个关键帧,并且和那个特征点对应.


2.  然后遍历obs中每一个关键帧pKF, 然后执行pKF->EraseMapPointMatch(mit->second), 将关键帧中的这个地图点删除
    接下来看一下EraseMapPointMatch函数的执行流程

    // 根据特征点的索引直接将MapPoint*指针指向空
    void KeyFrame::EraseMapPointMatch(const size_t &idx){
        mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
    }

    // 通过关键帧在mObservations中寻找地图点对应的特征点ID
    int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF){
        if(mObservations.count(pKF))
            return mObservations[pKF];
        else
            return -1;
    }

    // 根据关键帧寻找特征点的索引再将MapPoint*指针指向空
    // 和上面的不同就是已知条件不同, 最终的结果都是一样的.
    void KeyFrame::EraseMapPointMatch(MapPoint* pMP){
        int idx = pMP->GetIndexInKeyFrame(this);
        if(idx>=0)
            mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
    }

3.  在地图中删除这个地图点
    std::set<MapPoint*> mspMapPoints;
    // 删除 set 容器存储的元素
    // set 所包含的元素是唯一的,集合中的元素按一定的顺序排列
    void Map::EraseMapPoint(MapPoint *pMP){
        mspMapPoints.erase(pMP);
    }

4.  补充std容器知识
    // 一、vector：随机存取, 在缺省情况下应该使用
    // 二、list：大量的插入和删除
    // 三、deque：头部和尾部插入和删除
    // 四、set和multiset：查找一个元素是否在集合内
    // 五、map和multimap：数据字典,根据key找value,map[key] = value

IsInKeyFrame

// 判断是当前地图点和关键帧之间是否有关系，直接搜索mObservations这个map
// 使用count返回的是被查找元素的个数有返回1,否则返回0;
// map中不存在相同元素,返回值只能是1或0
bool MapPoint::IsInKeyFrame(KeyFrame *pKF){
    return (mObservations.count(pKF));
}

std::map<KeyFrame*,size_t> mObservations;
pMP->AddObservation(mpCurrentKeyFrame, i);
mObservations[pKF]=idx;

待整理

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

---

在之前的Tracking中，我们得到了新的关键帧Ki。Local Mapping这部分 包括插入关键帧，剔除冗余的地图点和关键帧，还有进行局部集束调整。

首先将新的关键帧Ki作为新的节点Ki加入Covibility Graph，并且更新与那些能够共享地图点的关键帧节点相连接的边。
同时更新关键帧Ki的生长树，并计算表示关键帧的词袋BOW。
这一部分的接口是在LocalMapping.cc中的

 // BoW conversion and insertion in Map
     ProcessNewKeyFrame();

计算词袋，整合地图点到新的关键帧，计算法线和描述子的接口如下：

Compute Bags of Words structures
mpCurrentKeyFrame->ComputeBoW();

Associate MapPoints to the new keyframe and update normal and descriptor
const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();

更新Covisibility Graph，将关键帧插入Map中的接口如下：

Update links in the Covisibility Graph
mpCurrentKeyFrame->UpdateConnections();

Insert Keyframe in Map
mpMap->AddKeyFrame(mpCurrentKeyFrame);

为了保存地图点，必须在创建该点云的前三帧测试通过约束，才能真正被保存，这样才能保证可跟踪且不容易在三角化时出现较大误差。这部分的接口如下：

Check recent MapPoints
MapPointCulling();

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

一旦地图点被创建了，就只有在少于3个关键帧能够观察到该点时才会被剔除。而要剔除关键帧，通常是在局部集束调整剔除外点或者在后面剔除关键帧时才会发生。这样就保证了地图点很少存在外点影响效果。

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

Triangulate new MapPoints
CreateNewMapPoints();

if(!CheckNewKeyFrames()){
Find more matches in neighbor keyframes and fuse point duplications
SearchInNeighbors();
}

局部集束调整（local BA）会将当前处理的关键帧Ki进行优化，优化时如下图所示：现在优化Pos3位置的关键帧。同时参与优化的还有：
所有在Covibility Graph中与该关键帧相连的关键帧Kc，
所以被这些关键帧观察到的地图点，即X1和X2。
另外还有能观察到地图点的但并未与当前处理的关键帧相连的关键帧，即下图中的Pos1。
  但要注意的是，诸如Pos1的关键帧，参与优化中的约束，但不作为变量去改变它们的值。优化时得到的外点会在优化的中期或后期被剔除。

这部分的接口如下：

Local BA
    if(mpMap->KeyFramesInMap()>2)
        Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

为了控制重建的紧凑度，LocalMapping会去检测冗余的关键帧，然后删除它们。这样的话会有利于控制，随着关键帧数目增长后，集束调整的复杂度。因为除非视角改变了，否则关键帧的数量在相同的环境中不应该无休止地增长。本文将那些有90%的点能够被超过三个关键帧观察到的关键帧认为是冗余关键帧，并将其删除。这个部分的接口如下：

Check redundant local Keyframes
    KeyFrameCulling();

最后，在所有步骤结束后，会将关键帧记录到数据库列表中。完成LocalMapping工作，将标志设为SetAcceptKeyFrames(true)，以允许Tracking线程继续得到关键帧。