详谈ORB-SLAM2的局部建图线程LocalMapping

ORB-SLAM2的局部建图线程LocalMapping分为5个步骤，非常简单。当得到缓冲队列里的关键帧，第一步处理当前关键帧的地图点关系等；第二步判断地图点是否为新创建，如果是那就进入测试，测试地图点的好坏，如果不好的地图点就舍弃；第三步创建新地图点，这里创建的是局部地图（真正会被注册到环境变量的里面的局部地图），trakcing的局部地图只用来跟踪使用；第四步进行局部的BA优化；第五步删除掉冗余的关键帧。

在ORB-SLAM2中创建的局部地图点这些都是从头开始创建的，根本没有利用到tracking线程里面的局部地图点局部关键帧的信息，这个操作在使用上感觉是作者的疏漏。其实LocalMapping管理全部的地图或者在Map对象里面管理全部的地图，Tracking线程就查找LocalMapping创建的变量这样是最好的选择，不然它创建的局部地图没有参与其中，局部地图创建了两遍设计的不是很好。

一、局部建图线程的成员函数/变量

Tracking线程向LocalMapping线程插入关键帧的缓冲队列，这两个线程之间通过关键帧进行交互信息传递的，所以有一个缓冲队列。

成员函数/变量	访问控制	意义
std::list<KeyFrame*> mlNewKeyFrames	protected	Tracking线程向LocalMapping线程插入关键帧的缓冲队列
void InsertKeyFrame(KeyFrame* pKF)	public	向缓冲队列mlNewKeyFrames内插入关键帧
bool CheckNewKeyFrames()	protected	查看缓冲队列mlNewKeyFrames内是否有待处理的新关键帧
int KeyframesInQueue()	public	查询缓冲队列mlNewKeyFrames内关键帧个数
bool mbAcceptKeyFrames	protected	LocalMapping线程是否愿意接收Tracking线程传来的新关键帧
bool AcceptKeyFrames()	public	mbAcceptKeyFrames的get方法
void SetAcceptKeyFrames(bool flag)	public	mbAcceptKeyFrames的set方法

LocalMapping线程局部关键帧太多处理不过来，所以把mbAcceptKeyFrames变量设置为false,通知Tracking线程停止输入，实际上mbAcceptKeyFrames变量在系统非常需要关键帧的时候，无论这个mbAcceptKeyFrames变量的取值，Tracking线程依然会输入，这个mbAcceptKeyFrames变量表示接受关键帧意愿

成员函数mbAcceptKeyFrames表示当前LocalMapping线程是否愿意接收关键帧，这会被Tracking线程函数Tracking::NeedNewKeyFrame()用作是否生产关键帧的参考因素之一；但即使mbAcceptKeyFrames为false,在系统很需要关键帧的情况下Tracking线程函数Tracking::NeedNewKeyFrame()也会决定生成关键帧。

二、局部建图主函数: Run()

主函数就是一个无限循环，每次循环检查当前环境队列里面有无关键帧，如果当前队列没有传来关键帧，当前线程暂停3毫秒开启下一轮查询；如果当前环境队列有当前帧，就处理队列的关键帧剔除坏点、创建新点、将局部地图关键帧与地图点融合、局部BA优化、剔除冗余关键帧和将当前关键帧加入闭环检测中。

void LocalMapping::Run() {

    while (1) {
        SetAcceptKeyFrames(false);		// 设置当前LocalMapping线程处于建图状态,不愿意接受Tracking线程传来的关键帧

		// step1. 检查缓冲队列内的关键帧 
        if (CheckNewKeyFrames()) {
            // step2. 处理缓冲队列中第一个关键帧
            ProcessNewKeyFrame();
            
            // step3. 剔除劣质地图点
            MapPointCulling();

            // step4. 创建新地图点
            CreateNewMapPoints();

            if (!CheckNewKeyFrames()) {
                // step5. 将当前关键帧与其共视关键帧地图点融合
                SearchInNeighbors();
                    
                // step6. 局部BA优化: 优化局部地图
                mbAbortBA = false;	
                Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);

                // step7. 剔除冗余关键帧
                KeyFrameCulling();
            }

            // step8. 将当前关键帧加入闭环检测中
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        } 
        
        SetAcceptKeyFrames(true);		// 设置当前LocalMapping线程处于空闲状态,愿意接受Tracking线程传来的关键帧

        // 线程暂停3毫秒再开启下一轮查询
        std::this_thread::sleep_for(std::chrono::milliseconds(3));
    }
}

三、处理队列中第一个关键帧: ProcessNewKeyFrame()

从缓冲队列中选一个关键帧进行处理，去除关键帧，计算词袋向量

依次处理当前帧的每个地图点，判断当前地图点是本帧跟踪时新创建的还是匹配到的，关键帧与地图点建立的双向还是单向的联系，这个方式不是很好。

1、判断该地图点新生成

在第3步中处理当前关键点时比较有意思,通过判断该地图点是否观测到当前关键帧(pMP->IsInKeyFrame(mpCurrentKeyFrame))来判断该地图点是否是当前关键帧中新生成的.

2、若地图点是本关键帧跟踪过程中匹配得到的(Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()和Tracking::SearchLocalPoints()中调用了ORBmatcher::SearchByProjection()和ORBmatcher::SearchByBoW()方法),则是之前关键帧中创建的地图点,只需添加其对当前帧的观测即可.

3、若地图点是本关键帧跟踪过程中新生成的

包括:1.单目或双目初始化Tracking::MonocularInitialization()、Tracking::StereoInitialization();2.创建新关键帧Tracking::CreateNewKeyFrame()),则该地图点中有对当前关键帧的观测,是新生成的地图点,放入容器mlNewKeyFrames中供LocalMapping::MapPointCulling()函数筛选

四、剔除坏地图点: MapPointCulling()

冗余地图点的标准：满足以下两个条件之一就算是坏地图点
$$1、召回率=\frac{实际观测到该地图点的帧数mnFound}{实际观测到该地图点的帧数mnFound}<0.25$$

（Tracking线程里面的Track Local Map这里涉及到统计地图点观测到的值）
$$2、在创建的3帧内观测数目少于2(双目为3)$$
若地图点经过了连续3个关键帧仍未被剔除,则被认为是好的地图点
(观测数目这件事很简单，nObservation在Mpoint中已提及)

void LocalMapping::MapPointCulling() {
    list<MapPoint *>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    int nThObs;
    if (mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;

    while (lit != mlpRecentAddedMapPoints.end()) {
        MapPoint *pMP = *lit;
        if (pMP->isBad()) {
            // 标准0: 地图点在其他地方被删除了
            lit = mlpRecentAddedMapPoints.erase(lit);
        } else if (pMP->GetFoundRatio() < 0.25f) {
            // 标准1: 召回率<0.25
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        } else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs) {
            // 标准2: 从创建开始连续3个关键帧内观测数目少于cnThObs
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        } else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 3)
            // 通过了3个关键帧的考察,认为是好的地图点
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

此博客参考ncepu_Chen的《5小时让你假装大概看懂ORB-SLAM2源码》