ORB-SLAM2源码解读（3）：LocalMapping

ORB-SLAM2的前端VO部分分为：Tracking和LocalMapping，

Tracking线程负责根据输入的Frame恢复出相机位姿T并跟踪局部地图，最后生成关键帧传给LocalMapping线程。

LocalMapping线程负责对新加入的KeyFrames和MapPoints筛选融合，剔除冗余的KeyFrames和MapPoints，维护稳定的KeyFrame集合，传给后续的LoopClosing线程。

LocalMapping主要流程

主要的功能点在：

• 处理新的关键帧ProcessNewKeyFrame()
• 剔除不合格地图点MapPointCulling()
• 三角化恢复新地图点CreateNewMapPoints()
• 融合当前帧与相邻帧重复的地图点SearchInNeighbors()
• 局部BA优化LocalBundleAdjustment()
• 剔除冗余关键帧KeyFrameCulling()

---

LocalMapping整体流程

1、SetAcceptKeyFrames(false)

告诉Tracking线程，LocalMapping正处于繁忙状态，不要再添加新的关键帧了。

2、ProcessNewKeyFrame()

计算关键帧BoW向量，将关键帧插入地图，更新Covisibility图和Essential图

（1）从缓冲队列中取出一帧关键帧

    // Tracking线程向LocalMapping中插入关键帧存在该队列中
    {
        unique_lock lock(mMutexNewKFs);
        // 从列表最前面中获得一个等待被插入的关键帧，原列表少一个关键帧
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        mlNewKeyFrames.pop_front();
    }

（2）计算当前关键帧的Bow向量，有助于加速三角化新的MapPoints

    mpCurrentKeyFrame->ComputeBoW();

（3）更新当前关键帧所看到的地图点的属性（平均观测方向，最优描述子）

    const vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();

    for(size_t i=0; iisBad())
            {
                // 非当前帧生成的MapPoints
				// 为当前帧在tracking过程跟踪到的MapPoints更新属性
                if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
                {
                    // 添加观测
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    // 获得该点的平均观测方向和观测距离范围
                    pMP->UpdateNormalAndDepth();
                    // 加入关键帧后，更新3d点的最佳描述子
                    pMP->ComputeDistinctiveDescriptors();
                }
                else // this can only happen for new stereo points inserted by the Tracking
                {

                    mlpRecentAddedMapPoints.push_back(pMP);
                }
            }
        }
    }    

(4) 更新Covisibility图的边，并将该关键帧插入到地图中

    // Update links in the Covisibility Graph
    // 步骤4：更新关键帧间的连接关系，Covisibility图和Essential图(tree)
    mpCurrentKeyFrame->UpdateConnections();

    // Insert Keyframe in Map
    // 步骤5：将该关键帧插入到地图中
    mpMap->AddKeyFrame(mpCurrentKeyFrame);

3、MapPointCulling()

剔除ProcessNewKeyFrame和CreateNewMapPoints函数中引入的质量不好的MapPoints

void LocalMapping::MapPointCulling()
{
    // Check Recent Added MapPoints
    list::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;
	
	// 遍历等待检查的MapPoints
    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;
        if(pMP->isBad())
        {
            // 步骤1：已经是坏点的MapPoints直接从检查链表中删除
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(pMP->GetFoundRatio()<0.25f)
        {
            // 步骤2：将不满足VI-B条件的MapPoint剔除
            // VI-B 条件1：
            // 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%
            // IncreaseFound / IncreaseVisible < 25%，注意不一定是关键帧。
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
            // 步骤3：将不满足VI-B条件的MapPoint剔除
            // VI-B 条件2：从该点建立开始，到现在已经过了不小于2个关键帧
            // 但是观测到该点的关键帧数却不超过cnThObs帧，那么该点检验不合格
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            // 步骤4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
            // 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

 整体利用迭代器遍历所有地图点：list::iterator lit = mlpRecentAddedMapPoints.begin();

在循环函数内新建pMP表示地图点：MapPoint* pMP = *lit;

剔除时使用erase：lit = mlpRecentAddedMapPoints.erase(lit);

四种情况下会剔除地图点：

• 如果地图点是被标记为bad的时候
• IncreaseFound / IncreaseVisible < 25%，即有超过25%的关键帧可以看到这个点
• 未被超过2个关键帧看到，并且当前关键帧的ID和看到该点的第一个关键帧之间的ID之差大于等于2的时候
• 当前关键帧的ID和看到该点的第一个关键帧之间的ID之差大于等于3的时候

4、CreateNewMapPoints()

利用对极约束对当前帧和共视关键帧通过三角化恢复出一些MapPoints

（1）在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs

const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

 （2）遍历相邻关键帧vpNeighKFs，得到基线向量vBaseline = Ow2-Ow1

（3）判断相机运动的基线是不是足够长，邻接关键帧的场景深度中值medianDepthKF2，baseline与景深的比例，如果特别远(比例特别小)，那么不考虑当前邻接的关键帧，不生成3D点

（4）根据两个关键帧的位姿计算它们之间的基本矩阵F

cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

 (5) 通过极线约束限制匹配时的搜索范围，对满足对极约束的特征点进行特征点匹配

        vector > vMatchedIndices;
        matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

 SearchForTriangulation通过基础矩阵找到匹配的特征点。

（6）对每对匹配通过三角化生成3D点,和Triangulate函数差不多

（7）接着分别检查新得到的点在两个平面上的重投影误差，如果大于一定的值，直接抛弃该点。

（8）检查尺度连续性

（9）如果满足对极约束则建立当前帧的地图点及其属性（a.观测到该MapPoint的关键帧  b.该MapPoint的描述子  c.该MapPoint的平均观测方向和深度范围），

（10）将地图点加入关键帧，加入全局map

5、SearchInNeighbors()

检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints

（1）获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧，找到当前帧一级相邻与二级相邻关键帧

（2）将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合

matcher.Fuse(pKFi,vpMapPointMatches);

         // 投影当前帧的MapPoints到相邻关键帧pKFi中，并判断是否有重复的MapPoints
        // 1.如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
        // 2.如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint

（3） 将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合

（4）更新当前帧MapPoints的描述子，深度，观测主方向等属性

                // 在所有找到pMP的关键帧中，获得最佳的描述子
                pMP->ComputeDistinctiveDescriptors();

                // 更新平均观测方向和观测距离
                pMP->UpdateNormalAndDepth();

（5）更新当前帧的MapPoints后更新与其它帧的连接关系

    mpCurrentKeyFrame->UpdateConnections();

6、LocalBundleAdjustment()

先看下吃水的鱼的解释

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

7、KeyFrameCulling()

剔除冗余关键帧

在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。

（1）根据Covisibility Graph提取当前帧的共视关键帧

    vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

（2）对所有的局部关键帧进行遍历，提取每个共视关键帧的MapPoints

const vector vpMapPoints = pKF->GetMapPointMatches();

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

        for(size_t i=0, iend=vpMapPoints.size(); iisBad())
                {
                    if(!mbMonocular)
                    {
                        // 对于双目，仅考虑近处的MapPoints，不超过mbf * 35 / fx
                        if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
                            continue;
                    }

                    nMPs++;
                    // MapPoints至少被三个关键帧观测到
                    if(pMP->Observations()>thObs)
                    {
                        const int &scaleLevel = pKF->mvKeysUn[i].octave;//当前关键点所处层级
                        const map observations = pMP->GetObservations();//当前地图点被观察数
                        // 判断该MapPoint是否同时被三个关键帧观测到
                        int nObs=0;
                        for(map::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
                        {
                            KeyFrame* pKFi = mit->first;
                            if(pKFi==pKF)
                                continue;
                            const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;

                            // Scale Condition 
                            // 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
                            if(scaleLeveli<=scaleLevel+1)
                            {
                                nObs++;
                                // 已经找到三个同尺度的关键帧可以观测到该MapPoint，不用继续找了
                                if(nObs>=thObs)
                                    break;
                            }
                        }
                        // 该MapPoint至少被三个关键帧观测到
                        if(nObs>=thObs)
                        {
                            nRedundantObservations++;
                        }
                    }

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

        if(nRedundantObservations>0.9*nMPs)
            pKF->SetBadFlag();