局部地图管理器——LocalMapping

在研究ORB_SLAM2::System的时候， 我们了解到 ORB-SLAM2 的系统对象 System 定义了一个局部地图管理器 ORB_SLAM2::LocalMapping 对象， 用来筛选由 ORB_SLAM2::Tracking 生成的关键帧，进行局部的BA优化(Local BA)，更新局部地图。 个人觉得，这个局部地图管理的过程是介于前端视觉里程计和后端闭环检测之间的一段任务。主要是记录环境特征，筛选稀疏地图点和关键帧，达到压缩前端和后端的搜索空间，提高系统运行效率的目的。

本文中，我们从总体上解一下 ORB_SLAM2::LocalMapping 这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的地图管理过程我们将在第三部分中进行分析。

        Map* mpMap;                 //! 地图对象
        Tracking* mpTracker;        //! 轨迹跟踪器
        LoopClosing* mpLoopCloser;  //! 闭环探测器
        bool mbMonocular;           //! 当前系统是否使用单目相机建图  

1. 成员变量

如右侧的代码片段所示，我们列举了类 LocalMapping 中的四个成员变量。其中，mpMap 通过稀疏的地图点和关键帧描述构建的地图，是类 LocalMapping 主要维护的数据对象。 mpTracker 和 mpLoopCloser 是 ORB-SLAM2 系统中另外两个任务对象， 分别完成前端的相机位姿跟踪和后端的闭环检测任务。最后的布尔变量 mbMonocular 为 true 时标识着当前系统正在使用单目相机进行建图。

下面的四个变量可以说是类 LocalMapping 的核心成员了。mlNewKeyFrames 是一个关键帧的缓存，每当前端 ORB_SLAM2::Tracking 输出了一个关键帧之后， 都会通过接口 InsertKeyFrame() 将之添加到 mlNewKeyFrames 中。LocalMapping 则在其线程函数 Run() 中逐一消费这些关键帧，用成员变量 mpCurrentKeyFrame 记录当前正在处理的对象。 或舍弃，或生成更多的地图点，放入 mlpRecentAddedMapPoints 中，等待严格的筛查后添加到地图对象 mpMap 中。 由于有多个线程都会访问 mlNewKeyFrames，所以增加了一个互斥量 mMutexNewKFs 来保证对缓存队列的操作都是安全的。

        std::list<KeyFrame*> mlNewKeyFrames;          //! 新关键帧的等待队列
        KeyFrame *mpCurrentKeyFrame;                  //! 当前正在处理的关键帧
        std::list<MapPoint*> mlpRecentAddedMapPoints; //! 记录新生成的地图点，待筛查
        std::mutex mMutexNewKFs;                      //! 保护 mlNewKeyFrames 的互斥量

        bool mbStopped;        //! 暂停更新局部地图的标识
        bool mbStopRequested;  //! 接收到暂停更新的标识
        bool mbNotStop;        //! mbStopRequested 是否有效的标识    
        std::mutex mMutexStop; //! 保护暂停相关标识的互斥量

右侧的代码片段是关于暂停局部建图的成员变量。其中，mbStopped 标识着是否已经暂停更新局部地图了，mbStopRequested 则标识着是否有其它的子系统请求暂停更新。 请求暂停通常是其它线程的发起的，类 LocalMapping 从接收到请求到做出响应会有一段时间，所以分别用了两个布尔变量。此外，还可以通过置位 mbNotStop 来屏蔽外部的暂停请求。 同样的，暂停更新，也是一个跨越线程的操作。所以还需要一个互斥量 mMutexStop 来保护这些标识成员。类似的，类 LocalMapping 还有复位、终止的操作。 相关变量如下面的代码片段所示，由于它们也都涉及到多个线程，所有分别提供了互斥量进行保护。

        bool mbResetRequested;   //! 当前系统是否有请求重置的信号
        std::mutex mMutexReset;  //! 保护重置信号的互斥量
        bool mbFinishRequested;  //! 接收到结束信号的标识
        bool mbFinished;         //! 局部建图线程退出的标识
        std::mutex mMutexFinish; //! 保护终止相关信号的互斥量

成员变量 mbAbortBA 可以控制随时放弃当前正在进行的 BA 优化。这种操作会在插入新的关键帧、暂停更新的时候发生。 此外，作者还提供了一个公共接口 InterruptBA 可以在其它线程中调用，直接终止 BA 优化。但是并未提供互斥量进行保护，不知道是作者忘了，还是算准了不会发生资源冲突， 这点留到具体分析局部地图管理过程的时候再仔细研究吧。

        bool mbAbortBA;          //! 放弃当前 BA 优化的标识
        bool mbAcceptKeyFrames;  //! 是否可以接收新的关键帧的标识
        std::mutex mMutexAccept; //! 保护 mbAcceptKeyFrames 的互斥量

成员变量 mbAcceptKeyFrames 表示 LocalMapping 是否可以接收新的关键帧。主要是 ORB_SLAM2::Tracking 需要根据这个标识生成关键帧，也会在多个线程中同时访问， 所以提供了互斥量 mMutexAccept 予以保护。

2. 成员函数

类 LocalMapping 只有一个构造函数。它的输入参数 pMap 记录了系统的地图对象，bMonocular 则表示建图时使用的相机是否是单目的。这个构造函数只有一个成员变量的初始化列表， 它的函数体是空的。构建了 LocalMapping 对象之后，我们就可以通过 Set 接口记录下系统的轨迹跟踪器和闭环探测器。

        LocalMapping(Map* pMap, const float bMonocular);  //! 构造函数
        void SetTracker(Tracking* pTracker);              //! 设置轨迹跟踪器
        void SetLoopCloser(LoopClosing* pLoopCloser);     //! 设置闭环探测器

轨迹跟踪器根据一定规则生成关键帧之后，通过接口 InsertKeyFrame 添加新的关键帧到队列 mlNewKeyFrames 中。 局部地图管理器将在其线程函数 Run 中进一步对关键帧进行筛选，插入到地图中，进行 Local BA 优化。

        void InsertKeyFrame(KeyFrame* pKF);  //! 插入关键帧

Run 是一个线程函数，系统对象ORB_SLAM2::System创建 LOCAL MAPPING 线程的时候通过 std::thread 指定局部地图管理的入口。该函数通过不断的调用下面的这些成员函数完成局部地图更新的操作，我们在注释中简单描述了各个函数的功能， 详细的管理过程我们将在第三部分一一解释。

        void Run();                     //! 局部地图管理的线程函数
        bool CheckNewKeyFrames();       //! 队列 mlNewKeyFrames 中是否有新的关键帧
        void ProcessNewKeyFrame();      //! 处理新的关键帧，插入新的关键帧，更新共视图
        void CreateNewMapPoints();      //! 根据共视关系三角化，生成地图点
        void MapPointCulling();         //! 剔除质量较差的地图点
        void SearchInNeighbors();       //! 根据关键帧的两级邻接关系，更新共视的地图点坐标
        void KeyFrameCulling();         //! 剔除质量较差的关键帧

此外，LocalMapping 还定义了两个辅助的函数。其中，ComputeF12 用来计算两个关键帧之间的基础矩阵，通过该矩阵和匹配特征点可以求得关键帧之间的相对位姿关系。 SkewSymmetricMatrix 用来将一个三维向量转换成斜对称矩阵的形式。 ComputeF12 的传参有点莫名其妙，pKF1 和 pKF2 都已经是指针了，还需要整个引用在这里吗？

        cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); //! 计算两个关键帧之间的基础矩阵
        cv::Mat SkewSymmetricMatrix(const cv::Mat &v);        //! 三维向量的反对称矩阵形式

类 LocalMapping 中的成员函数大多都是一些标识的设置和查询接口，如下面的代码片段所示，不再一一展开介绍了。

        void RequestStop();                     //! 请求暂停更新地图
        void RequestReset();                    //! 请求重置局部地图管理器
        void RequestFinish();                   //! 请求终止局部地图管理线程的工作
        bool SetNotStop(bool flag);             //! 设置 mbNotStop 的状态，屏蔽暂停线程的功能
        void InterruptBA();                     //! 停止当前正在进行的 Local BA 优化
        bool Stop();                            //! 线程函数 Run 调用，检查是否需要停止局部建图
        void Release();                         //! 释放 mlNewKeyFrames 中的关键帧
        bool isStopped();                       //! 检查 mbStopped 是否置位
        bool stopRequested();                   //! 检查 mbStopRequested 是否置位
        bool AcceptKeyFrames();                 //! 检查 mbAcceptKeyFrames 是否置位，是否接收新的关键帧
        void SetAcceptKeyFrames(bool flag);     //! 设置 mbAcceptKeyFrames 的状态
        bool isFinished();                      //! 检查 mbFinished 是否置位，管理线程是否终止运行
        int KeyframesInQueue();                 //! 查询等待队列中新的关键帧数量
        void ResetIfRequested();                //! 检查复位标识 mbResetRequested，若置位，则重置管理器
        bool CheckFinish();                     //! 检查结束标识 mbFinishRequested，若置位，则停止局部建图的线程
        void SetFinish();                       //! 置位标识 mbFinished，终止线程

3. 完

本文中，我们从总体上解一下 ORB_SLAM2::LocalMapping 这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的地图管理过程我们将在第三部分中进行分析。