插入新关键帧和筛选地图点

在TRACKING线程中，关键帧在经过层层筛选和优化之后，会被 Tracking 对象通过 LocalMapping 对象的接口喂给LOCAL MAPPING线程。在 LOCAL MAPPING 线程中前两项工作就是，将新关键帧插到共视图中，然后对最近添加的一些地图点进行筛选。

LOCAL MAPPING 线程根据共视的地图点关系更新共视图，进而推动了地图数据的更新。除此之外，TRACKING 线程还会维护一个生成树(spanning tree)，这棵树可以看做是共视图的一个子图。它具有和共视图一样的节点，但只记录新增的关键帧的共视点最多的那个关键帧，在它们之间生成一条边。

可以说ORB-SLAM2所建的地图，就是由若干个地图点描述的稀疏点云。地图点过多将增加系统的计算量，降低算法的运行效率，伴随着场景的扩张，地图点如果不加限制的增长，最终将导致系统崩溃掉。而一帧图像所能生成的地图点是很多的，为了保证系统的效率，必需对地图点进行一些筛选。 ORB-SLAM论文中指出，地图点只有通过了一些严格的筛选之后才能将被认为是可跟踪的(trackable)并且没有被错误的三角化。原文提出了两个筛选条件：

地图点在TRACKING线程中的查找率(Found Ratio)不能低于25%；
创建了地图点，并经过了两帧关键帧之后，至少要有三个关键帧能够观测到该点。

本文中我们分析这两项工作的源码。了解插入新关键帧的具体工作，地图点的筛选条件，尤其是这里提到的查找率。

1. 插入新关键帧

Tracking对象生成关键帧之后，先后调用了LocalMapping对象的SetNotStop和InsertKeyFrame将新生成的关键帧提供给了LocalMapping对象。

后面分析LOOP CLOSING线程的时候，我们将会看到LoopClosing对象为了保证地图数据的完整性，在更新地图的时候会请求LocalMapping暂停。这里的函数SetNotStop就是防止LocalMapping暂停用的。如下面的代码所示，在函数SetNotStop中为成员变量mbNotStop赋值，该变量决定了右侧的Stop函数是否能够将成员变量mbStopped置为true。

展开函数 SetNotStop >>>

        bool LocalMapping::SetNotStop(bool flag) {
            unique_lock<mutex> lock(mMutexStop);

            if(flag && mbStopped)
                return false;

            mbNotStop = flag;
            return true;
        }

        bool LocalMapping::Stop() {
            unique_lock<mutex> lock(mMutexStop);
            if(mbStopRequested && !mbNotStop) {
                mbStopped = true;
                cout << "Local Mapping STOP" << endl;
                return true;
            }
            return false;
        }

下面的代码是函数 InsertKeyFrame() 的实现，十分简短。首先通过互斥量 mMutexNewKFs 加锁，保护公共资源 mlNewKeyFrames。然后将新的关键帧放入该容器中，并将成员变量 mbAbortBA 置为 true，表示要暂停 BA 优化。

        void LocalMapping::InsertKeyFrame(KeyFrame *pKF) {
            unique_lock<mutex> lock(mMutexNewKFs);
            mlNewKeyFrames.push_back(pKF);
            mbAbortBA=true;
        }

在分析局部建图的总体过程的时候，我们已经看到， LocalMapping 对象在其成员函数 run() 中通过一个超级循环运行着其业务逻辑主线。在该循环中，通过函数 CheckNewKeyFrames() 检查容器 mlNewKeyFrames 是否为空，来判定是否收到了新的关键帧。然后在函数 ProcessNewKeyFrame() 完成具体的插入工作。

下面是函数 ProcessNewKeyFrame() 的代码片段，它先在一个局部的环境中从队列mlNewKeyFrames中将对首的关键帧，也就是最早的那个，出队。由于TRACKING线程也会通过InsertKeyFrame操作队列mlNewKeyFrames，为了防止公共资源的冲突，所以在这里对信号量mMutexNewKFs加锁，在程序控制流从第6行的大括号中退出的时候，就会释放局部对象lock，进而解锁mMutexNewKFs。

        void LocalMapping::ProcessNewKeyFrame() {
            {
                unique_lock<mutex> lock(mMutexNewKFs);
                mpCurrentKeyFrame = mlNewKeyFrames.front();
                mlNewKeyFrames.pop_front();
            }

接下来，先完成关键帧的词袋计算，并用vpMapPointMatches获取已经匹配的地图点。

            mpCurrentKeyFrame->ComputeBoW();
            const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();

进而遍历所有的地图点。由于vpMapPointMatches中的每个元素都与关键帧中的每个特征点相对应，并不是所有的特征点都成功匹配到了一个地图点，那些没有匹配的特征点所对应的地图点就是NULL。通过地图点的IsInKeyFrame接口检查是否重复出现过，看注释好像这个条件语句只在双目和RGB-D相机下有意义。总之，只要没有重复出现过，就相应的更新地图点的法向量、深度信息、特征描述。

            for(size_t i=0; i < vpMapPointMatches.size(); i++) {
                MapPoint* pMP = vpMapPointMatches[i];
                if (pMP) {
                    if(!pMP->isBad()) {
                        if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)) {
                            pMP->AddObservation(mpCurrentKeyFrame, i);
                            pMP->UpdateNormalAndDepth();
                            pMP->ComputeDistinctiveDescriptors();
                        } else { // this can only happen for new stereo points inserted by the Tracking
                            mlpRecentAddedMapPoints.push_back(pMP);
            }   }   }   }

最后，通过关键帧的接口 UpdateConnections 计算当前的关键帧与其它关键帧之间的共视关系，建立连边完成共视图的更新。通过地图对象 mpMap 将新增的关键帧添加到地图数据中。

            mpCurrentKeyFrame->UpdateConnections();
            mpMap->AddKeyFrame(mpCurrentKeyFrame);
        }

在轨迹跟踪线程中根据局部地图优化位姿时，我们就已经讨论过共视图的数据结构。在帧 Frame 和关键帧 KeyFrame 中都有一个std::vector<MapPoint*>类型的容器 mvpMapPoints，以指针的形式记录着帧/关键帧观测到的地图点。地图点 MapPoint 中有一个std::map<KeyFrame*,size_t>类型的容器 mObservations，它记录了观测到该地图点的所有关键帧及其在关键帧中的索引。这两个容器建立了关键帧的共视关系，也就是所谓的共视图。

2. 地图点筛选

当 LocalMapping 对象在它的业务循环函数 Run 中调用 ProcessNewKeyFrame 完成了新关键帧的插入操作之后，就会调用函数 MapPointCulling 来完成地图点的筛选任务。下面是这个函数的代码片段，首先获取了最近新增的地图点集合的迭代器和当前关键帧的ID。

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

根据迭代的数据类型，很容易看出来这里的容器 mlpRecentAddedMapPoints 是一个链表，这是一种离散的存储结构，具有很高的增删改效率，但随机索引效率很低。这种数据结构很适合地图点的筛选操作，我们只需要根据链表的指引依次遍历容器中的所有地图点，并删除不满足条件的地图点。下一篇文章我们将分析新地图点的生成，到时将看到容器 mlpRecentAddedMapPoints 新增地图点。

因为想节省一些篇幅，我对源码做了一些调整，得到下面两行的代码。这两行代码就是根据单目相机还是深度相机设定一个阈值，用于筛选条件2的判定。

            int nThObs = mbMonocular ? 2 : 3;
            const int cnThObs = nThObs;

接着，我们在一个while循环中，遍历所有新增的地图点。如果地图点的接口isBad返回true表示这个地图点曾因为某种原因被认定为野点，将被抛弃掉。这些可能的原因一般有两种，要么是上述的筛选条件未通过，要么是后续经过较为严格的约束优化之后发现错的比较离谱。

            while(lit != mlpRecentAddedMapPoints.end()) {
                MapPoint* pMP = *lit;
                if (pMP->isBad()) {
                    lit = mlpRecentAddedMapPoints.erase(lit);
                }

这是第一个筛选条件的判定，不通过就将之标记为野点，并抛弃之，后文我们会讲查找率 FoundRatio 的物理含义和计算方法。

                else if(pMP->GetFoundRatio() < 0.25f) {
                    pMP->SetBadFlag();
                    lit = mlpRecentAddedMapPoints.erase(lit);
                }

然后是第二筛选条件。地图点的成员变量mnFirstKFid记录了生成地图点时的关键帧编号，如果当前关键帧编号与之相差两个帧以上，就需要至少有三个关键帧检测到了该地图点。接口Observations记录了观测到地图点的关键帧数量。

                else if(((int)nCurrentKFid - (int)pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs) {
                    pMP->SetBadFlag();
                    lit = mlpRecentAddedMapPoints.erase(lit);
                }

最后的这个条件是说地图点已经生成并存在很长一段时间了，所以并没有将之标记为Bad，仅仅从容器中移除而已。

                else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
                    lit = mlpRecentAddedMapPoints.erase(lit);
                else
                    lit++;
        }   }

3. 查找率Found Ratio

这个所谓的查找率，是在TRACKING线程中判定匹配到地图点的关键帧数量与预测可以看到地图点的关键帧数量之比。

The tracking must find the point in more than the 25% of the frames in which it is predicted to be visible.

我们通过地图点的接口GetFoundRatio来获取其查找率，如下面的代码所示，mnFound记录了实际观测到地图点的关键帧数量，而mnVisible则是估计能够看到该点的关键帧数量。

        float MapPoint::GetFoundRatio() {
            unique_lock<mutex> lock(mMutexFeatures);
            return static_cast<float>(mnFound)/mnVisible;
        }

地图点的这两个变量mnFound和mnVisible都是在TRACKING线程中得到更新的。在根据局部地图优化位姿的时候，通过Tracking对象的成员函数SearchLocalPoints对局部地图中的地图点进行粗略筛选，会根据观测到地图点的视角余弦值来估计当前帧能否观测到响应的地图点，并通过地图点的接口IncreaseVisible增加mnVisible的计数。

Tracking对象完成地图点的粗筛之后，会进行一次位姿优化。如果位姿优化之后仍然能够匹配到地图点，就认为在该帧中找到了地图点。相应的调用地图点接口IncreaseFound增加mnFound计数。下面的代码片段是根据局部地图优化位姿一文介绍TrackLocalMap时省略的代码。它的工作就是在进一步位姿优化之后，统计仍然匹配的地图点数量，其中第5行就是增加mnFound计数的调用。

展开函数 Tracking::TrackLocalMap 统计优化位姿后仍然匹配的地图点数量 >>>

        // bool Tracking::TrackLocalMap()
        for(int i = 0; i < mCurrentFrame.N; i++) {
            if(mCurrentFrame.mvpMapPoints[i]) {
                if(!mCurrentFrame.mvbOutlier[i]) {
                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                    if(!mbOnlyTracking) {
                        if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                            mnMatchesInliers++;
                    } else
                        mnMatchesInliers++;
                } else if(mSensor==System::STEREO)
                    mCurrentFrame.mvpMapPoints[i] = static_cast(NULL);
        }   }

4. 完

本文中，我们查看了插入新关键帧的代码，了解到 LocalMapping 对象通过一个容器 mlNewKeyFrames 记录新增的关键帧，在函数 ProcessNewKeyFrame 中逐一处理，并更新共视图。我们还查看了地图点的筛选，详细讨论了筛选依据，查找率，的概念和计算过程。