局部建图的总体过程

局部建图的任务是处理由轨迹跟踪生成的关键帧，并进行局部的BA优化(local BA)，获得一个对当前相机附近的环境比较好的稀疏重建结果。针对TRACKING线程生成的新关键帧中没有匹配的特征点，LOCAL MAPPING线程还会在共视图中相关的关键帧中进行匹配搜索，进而实现这些特征点的三角化得到它们的空间坐标。此外为了保证系统的运行效率不因时间的推移和场景的扩大而严重降低，该线程还会对地图点和关键帧进行比较严格的筛选，抛弃一些质量较差或者冗余的地图点和关键帧。

我们在第一部分的介绍中，了解到 ORB_SLAM2 通过类 LocalMapping 将上述过程封装到一个局部地图管理器中完成。本文中，我们将结合2015年的论文和代码从总体上，了解局部建图的具体过程。

1. 总体过程浅析

右图是从原文的系统框图中截取的关于局部建图的过程。在论文的第 VI 部分中，作者依次介绍了框图中的五个过程。 LOCAL MAPPING 线程的输入 KeyFrame 就是 TRACKING 线程的输出，整个线程有5个子任务，分别完成了新关键帧的插入、地图点的筛选、新地图点的生成、局部BA优化以及关键帧的筛选。

插入关键帧: 对应框图中的 “KeyFrame Insertion”。新的关键帧将作为共视图中的一个节点，根据共视关系建立边。此外，还会选择共视点最多的那个关键帧更新生成树(spanning tree)。计算关键帧的词袋向量。
筛选地图点：对应框图中的 “Recent MapPoints Culling”。地图点的筛选需要遵循两个原则，尽可能保证其可追踪性(trackable)，同时减少错误三角化的几率。配合上关键帧的筛选，可以控制地图规模。
新建地图点：对应框图中的 “New Points Creation”。对于新插入的关键帧中未匹配的特征点，在其共视图的邻接关键帧中进行匹配，并进一步的通过三角化的方法估计特征点的空间坐标，生成新的地图点。
局部 BA 优化：对应框图中的 “Local BA”。根据当前帧在共视图中的邻接关键帧，以及出现在这些关键帧中的地图点，构建一个局部的位姿图。通过 g2o 库进行 BA 优化，修正涉及到的关键帧和地图点的位姿。
筛选关键帧：对应框图中的 “Local KeyFrames Culling”。为了控制关键帧的数量，LOCAL MAPPING 线程会尝试找出那些冗余的关键帧并删除它们，进而可以支持 lifelong 的重建任务。

我们在局部地图管理器——LocalMapping一文中，查看了类 LocalMapping 的成员变量和成员函数。了解到它的构造函数只有一个初始化列表完成了各个成员变量的初始化工作，其函数体是空的。我们还在系统对象——System中看到创建 LOCAL MAPPING 线程的时候，系统对象就通过如下的语句先后构造了局部地图管理器对象 mpLocalMapper 和线程 mptLocalMapping。

        mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
        mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

与此同时，系统对象指定了类 LocalMapping 的成员函数 Run() 为线程的运行函数。如下面的左侧代码片段所示，该函数整个就是一个 while 超级循环，搁那一直转。这个超级循环就是LOCAL MAPPING线程的业务逻辑主线。在该函数一开始先给成员变量mbFinished赋予初值false表示系统还没有结束，然后就开始转圈了。在while循环中，通过接口SetAcceptKeyFrames将成员变量mbAcceptKeyFrames置为false，目的是要告知TRACKING线程，当前LocalMapping正在忙别着急。

展开 LocalMapping::Run >>>

        void LocalMapping::Run() {
            mbFinished = false;
            while(1) {
                SetAcceptKeyFrames(false);
                if(CheckNewKeyFrames()) {
                    // 为了节省篇幅，省略了很多条件的检查，看个意思
                    ProcessNewKeyFrame();
                    MapPointCulling();
                    CreateNewMapPoints();
                    Optimizer::LocalBundleAdjustment();
                    KeyFrameCulling();
                    mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
                }
                SetAcceptKeyFrames(true);
                if(CheckFinish())
                    break;
                usleep(3000);
            }
            SetFinish();
        }

        void LocalMapping::SetAcceptKeyFrames(bool flag) {
            unique_lock<mutex> lock(mMutexAccept);
            mbAcceptKeyFrames=flag;
        }

        bool LocalMapping::CheckNewKeyFrames() {
            unique_lock<mutex> lock(mMutexNewKFs);
            return(!mlNewKeyFrames.empty());
        }

        bool LocalMapping::CheckFinish() {
            unique_lock<mutex> lock(mMutexFinish);
            return mbFinishRequested;
        }

        void LocalMapping::SetFinish() {
            unique_lock<mutex> lock(mMutexFinish);
            mbFinished = true;    
            unique_lock<mutex> lock2(mMutexStop);
            mbStopped = true;
        }

重点是 5 到 13 行，检查有新的关键帧之后调用的这些个函数，看字面意思也能理解，它们是实际完成各项任务的函数。我们将在后续的文章中依次详细介绍这些函数的设计思想和实现细节。上面右侧的代码片段中还列举了一些辅助函数，用于更新和获取一些状态变量。

2. 插入新关键帧

在TRACKING线程中，图像数据在经过层层筛选和优化之后，被加工成关键帧，通过局部地图管理器 LocalMapping 的接口喂给LOCAL MAPPING线程。因为涉及到多线程的操作，轨迹跟踪器生成的关键帧并没有直接添加到地图中，而是先放到一个缓存队列中。函数 CheckNewKeyFrames() 也是先检查该缓存队列是否为空来判定是否有新的关键帧生成了，然后就会在分支语句块中执行更新局部地图的五个任务。

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

更新了共视图之后，为了方便后续进行特征点查找和关键帧匹配，ORB-SLAM2 还会在这里计算一下新接受的关键帧的词袋向量。关于词袋模型我们专门在介绍重定位的时候，介绍了它的基本思想。大体上是通过统计特征点描述子出现的频率，来描述图像的总体特征，进而完成图像的分类工作。在这里，我们假设出现在同一个地方的图像应当具有极大的相似性，那么通过对比图像的总体特征向量，评价图像的近似程度，就可以加速搜索过程了。

3. 筛选地图点

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

地图点的查找率(Found Ratio)不能低于25%；
一个地图点被创建了之后，至少要有三个关键帧能够观测到该点。

第一个条件中的查找率指的是，实际观测到该地图点的关键帧数量与估计可以观测到它的关键帧数量之比。实际的观测数量，可以通过类 MapPoint 中记录的关键帧容器查询得到。估计的观测数量，是要根据相机的位姿计算视角的余弦值来确认的。

第二个条件很好理解，就是要求在创建地图点的时候，能够持续观测到该点。这样才能说这个地图点是比较可靠的，而且经过连续几帧的观测和修正，它的三角化定位结果也是比较可信的。

地图点在创建之初，可能有很多关键帧都能观测到。但随着局部 BA 优化和关键帧的筛选工作的进行，记录的观测到它的关键帧数量可能会减少。当少于三个关键帧的时候，该地图点将被移除。

4. 生成新地图点

对于ORB-SLAM而言，地图的扩张是由新地图点的生成来实现的。关键帧之间通过共视的地图点建立联系，形成共视图和基图。所以新地图点的生成是ORB-SLAM建图的一个重要环节，所有地图点的集合可以看作是其稀疏建图的结果。

ORB-SLAM论文中提到，每插入一个新的关键帧，局部地图管理器会都会在共视图中找到它的邻接关键帧，然后通过词袋模型匹配的方法，搜索新关键帧中尚未成功匹配的特征点，并抛弃其中不满足对极约束的匹配点对。接着进行三角化确定特征点对应的空间位置，生成地图点。虽然地图点的生成只依赖于在两个关键帧，但这一步会有找出多个观测到该点的关键帧，局部地图管理器会将之投影到这些关键帧上，进行位姿优化，也就是所谓的局部 BA 优化。

5. 局部 BA 优化

Bundle Adjustment是一种优化方法，通过迭代的方式调整系统的状态变量，使得系统的误差或者说是损失函数最小化。在 SLAM 中，BA 优化通常都运行在后端，用于修正相机的历史轨迹和地图点的空间位置。ORB-SLAM2 中的 BA 优化有两层，一个是在LOOP CLOSING线程中进行的全局优化，修正共视图中所有的关键帧位姿和地图点坐标；另一个就是这里的局部 BA 优化，取共视图中与当前帧关联的一个子图进行优化。

在局部 BA 中优化的状态变量是，当前帧及其在共视图中一级邻接的关键帧的位姿，我们把这些关键帧构成的集合记为 \(\mathcal{K_c}\)。以及\(\mathcal{K_c}\)观测到的所有地图点的位置。此外所有观测到这些地图点的关键帧也会参与优化，但是它们的位姿时固定的，在优化的过程中是不变的。与根据局部地图优化位姿类似，通过 g2o 把这些关键帧和地图点的约束关系用一个图的结构来描述，通过图优化的方式求解。

6. 筛选局部关键帧

出于效率的考虑，局部地图管理器还会检查关键帧，删除那些冗余的关键帧。这样可以防止随着系统的长时间运行，关键帧无限制的增长。根据论文中的描述，如果集合 \(\mathcal{K_c}\) 中某个关键帧中 90% 的地图点都可以在该集合中至少3个关键帧中找到，那么就认定它是冗余的，将被删掉。

7. 完

本文中，我们结合2015年的论文和代码从总体上了解局部建图的具体过程。它接收轨迹跟踪器生成的关键帧，更新共视图，计算词袋向量。筛选并创建地图点之后，以当前帧为中心，从共视图中选择一个子图进行局部的 BA 优化。这个子图可以称为是一个局部地图，最后还会对局部地图中出现的关键帧进行筛选，删掉那些冗余的关键帧。