闭环探测器——LoopClosing

ORB-SLAM2的系统对象ORB_SLAM2::System 中定义了一个闭环探测器 ORB_SLAM2::LoopClosing 的对象，用来完成后端的闭环检测以及全局BA优化的任务。本文中，我们从总体上解一下 ORB_SLAM2::LoopClosing 这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。详细的闭环探测和全局优化过程我们将在第四部分中进行分析。

1. 类内数据类型

如下面的代码片段所示，类 LoopClosing 在开始的时候就定义了两个类型 ConsistentGroup 和 KeyFrameAndPose。它们在搜索闭环关系，计算关键帧的相似变换时有重要作用。

        typedef pair<set<KeyFrame*>,int> ConsistentGroup;
        typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
                Eigen::aligned_allocator<std::pair<KeyFrame *const, g2o::Sim3>>> KeyFrameAndPose;

类型 KeyFrameAndPose 主要记录了关键帧和它的相似变换之间映射关系。C++标准库中的 std::map 的模板类型实际上有4个参数。前两个就是我们常用的键、值的类型，在这里分别是 KeyFrame* 和 g2o::Sim3。后两个参数是两个函数分别用来【比较键】和【申请内存】用的，它们都有一个默认的值，所以一般很少用到。但是矩阵计算库 Eigen 本身有一些加速优化的操作，需要数据按照一定的字节数对齐，所以额外提供了一个内存分配的函数 Eigen::aligned_allocator。 std::less<KeyFrame*> 是一个比较函数供 std::map 查找键值的时候使用。

类型 ConsistentGroup 本质上是在描述关键帧之间的共视关系。它的概念稍微有点绕，我们先来看一下字面上的 组(Group) 和 一致的(Consistent) 的含义。在 LoopClosing 中，把一个关键帧及其在共视图中的邻接关键帧一起称为一个组（包含关键帧本身）。如果有两个组它们的交集非空，或者说有一个关键帧同时出现在两个组里，我们称这两个组是一致的(Consistent)，既它们具有一致关系。

类型 ConsistentGroup 用标准库中的容器 std::set 记录一个组的所有成员，对应着这里的二元组 std::pair 中的第一个元素 std::set<KeyFrame*>。二元组中的第二个元素是一个整型，它记录的是在一个组的集合中，与组 ConsistentGroup::first 具有一致关系的组个数。这个组的集合就是成员变量 mvConsistentGroups 中所记录的各个组。

2. 成员变量

        Map* mpMap;                     //! 地图对象
        Tracking* mpTracker;            //! 轨迹跟踪器
        LocalMapping *mpLocalMapper;    //! 局部地图管理器    
        KeyFrameDatabase* mpKeyFrameDB; //! 关键帧数据库
        ORBVocabulary* mpORBVocabulary; //! 特征字典

如右侧的代码片段所示，我们列举了类 LoopClosing 中的五个成员变量。其中，mpMap 通过稀疏的地图点和关键帧描述构建的地图，是类 LoopClosing 主要维护的数据对象。 mpTracker 和 mpLocalMapper 是 ORB-SLAM2 系统中另外两个任务对象，分别完成前端的相机位姿跟踪和介于前后端之间的局部地图管理任务。关键帧数据库 mpKeyFrameDB 和特征字典 mpORBVocabulary 是词袋模型的两个基本数据对象，用于加速搜索闭环关键帧。

与ORB_SLAM2::LocalMapping类似，闭环探测器也有一个关键帧的等待队列， mlpLoopKeyFrameQueue。由 ORB_SLAM2::Tracking 产出的关键帧，在 ORB_SLAM2::LocalMapping 那里经过了层层筛选之后，将被添加到地图中，同时调用接口 InsertKeyFrame 将之喂给 ORB_SLAM2::LoopClosing 搜索可能存在的闭环。一旦检测出闭环就会进行全局的 BA 优化。

        std::list<KeyFrame*> mlpLoopKeyFrameQueue;     //! 新关键帧的等待队列
    	std::mutex mMutexLoopQueue;                    //! 保护 mlpLoopKeyFrameQueue 的互斥量
    	KeyFrame* mpCurrentKF; 					       //! 当前的关键帧
    	KeyFrame* mpMatchedKF; 					       //! 检测到与当前帧闭环的关键帧

为了不影响Tracking和LocalMapping两个线程的正常工作，LoopClosing也在一个线程中完成它的业务逻辑。因此，接口 InsertKeyFrame 并不会直接开始进行闭环检测，而是放到缓存 mlpLoopKeyFrameQueue 中慢慢消费。如上面的代码片段所示，LoopClosing 还为之增加了一个互斥量 mMutexLoopQueue 来同步多线程操作。此外，还用指针 mpCurrentKF 和 mpMatchedKF 分别表示当前正在处理的关键帧和与之对应的闭环关键帧。

关键帧之间的共视关系是闭环检测时需要考虑的的一个关键因素，为此作者还专门定义了数据类型 ConsistentGroup 来描述帧之间共视关系，以及由共视的关键帧构成的集合的一致关系，既是否与其它集合有交集，暂且称它为共视组吧。如下面的代码片段中第2行所示，容器 mvConsistentGroups 中记录了共视组，这是一个随着关键帧增加不断变化的集合，详细工作方式我们将在第四部分中用到时具体分析。容器中的各个共视组的第二个元素描述的是与容器中其它共视组有交集的组数量。

        float mnCovisibilityConsistencyTh;                    //! 闭环检测共视关系一致性判定阈值
        std::vector<ConsistentGroup> mvConsistentGroups;      //! 上一个关键帧的共视组集合的一致关系
        std::vector<KeyFrame*> mvpEnoughConsistentCandidates; //! 当前关键帧的闭环候选集合

对于每一个关键帧，我们都可以通过词袋模型给出一组可能存在闭环的关键帧集合。这些候选关键帧我们可以写出一个共视组，只有那些与 mvConsistentGroups 中具有一致关系的共视组数量大于阈值 mnCovisibilityConsistencyTh 的关键帧才是具有足够一致性的候选关键帧，可以添加到容器 mvpEnoughConsistentCandidates 中。

每当成功检测到一次闭环，都会对当前帧和闭环帧共视的的地图点进行更新，同时它也会将闭环关键帧视为其一个邻接关键帧。下面的容器 mvpCurrentConnectedKFs, mvpCurrentMatchedPoints, mvpLoopMapPoints 分别记录了当前关键帧的邻接关键帧、当前帧的地图点、闭环帧的地图点。闭环检测成功之后还会进行全局的 BA 优化修正各个关键帧的额位姿，优化求解器是 g2o，而 ORB-SLAM2 以 OpenCV 的 Mat 来表示位姿，所以这里整了成员变量 mScw 和 mg2oScw。

        std::vector<KeyFrame*> mvpCurrentConnectedKFs;  //! 当前关键帧的邻接关键帧
        std::vector<MapPoint*> mvpCurrentMatchedPoints; //! 当前关键帧中成功定位的地图点集合
        std::vector<MapPoint*> mvpLoopMapPoints;        //! 闭环关键帧上的地图点集合
        cv::Mat mScw;                                   //! OpenCV 形式的 sim3 变换
        g2o::Sim3 mg2oScw;                              //! 优化后当前帧的 sim3 变换

        long unsigned int mLastLoopKFid; //! 上一次检测到闭环的关键帧ID
        bool mbRunningGBA;               //! 全局 BA 优化正在进行中
        bool mbFinishedGBA;              //! 全局 BA 优化已经结束
        bool mbStopGBA;                  //! 暂停全局 BA ?
        std::mutex mMutexGBA;            //! 保护全局 BA 的互斥量
        std::thread* mpThreadGBA;        //! 进行全局 BA 的线程对象

右侧的代码片段是关于全局 BA 优化的成员变量。其中，mLastLoopKFid 记录了上次成功检测到闭环的关键帧 ID。由于全局 BA 优化是在一个独立的线程中进行的，所以作者还提供了成员变量 mbRunningGBA 和 mbFinishedGBA 来标识全局优化是否正在进行。不太理解为啥要整两个变量，感觉没啥用，一个就够。互斥量 mMutexGBA 用来保护涉及全局 BA 优化的公共资源，指针 mpThreadGBA 记录的是实际进行优化的线程对象。类似的，类 LoopClosing 还有复位、终止的操作。相关变量如下面的代码片段所示，由于它们也都涉及到多个线程，所有分别提供了互斥量进行保护。

展开 LoopClosing 的复位、终止的相关成员变量 >>>

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

3. 成员函数

类 LoopClosing 只有一个构造函数。它的输入参数 pMap 记录了系统的地图对象，pDB 和 pVoc 分别是用于词袋模型的关键帧数据库和特征字典，布尔类型 bFixScale 表示是否需要固定尺度。这个构造函数只有一个成员变量的初始化列表，它的函数体是空的。构建了对象之后，我们就可以通过 Set 接口记录下系统的轨迹跟踪器和局部地图管理器。

        LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale); //! 构造函数
        void SetTracker(Tracking* pTracker);             //! 设置轨迹跟踪器
        void SetLocalMapper(LocalMapping* pLocalMapper); //! 设置局部地图管理器

对于单目相机，由于存在尺度缺失的问题，ORB-SLAM2 在通过 g2o 进行优化的时候，除了估计相机的位置和姿态之外，还额外对尺度因子进行了优化估计，所以在构造 LoopClosing 的时候参数 bFixScale 传参为 true。对于双目和深度相机而言，因为尺度可以直接测量得到，不需要估计尺度因子，所以构造的时候 bFixScale 传参为 false。

当局部地图管理器接受一个关键帧之后，就会通过接口 InsertKeyFrame 添加新的关键帧到队列 mlpLoopKeyFrameQueue 中。闭环探测器将在其线程函数 Run 中逐一消费队列中的关键帧，检测是否发生了闭环事件。当成功检测到闭环，就会新建一个线程进行全局的 BA 优化。

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

Run 是一个线程函数，系统对象ORB_SLAM2::System创建 LOOP CLOSING 线程的时候通过 std::thread 指定闭环探测的入口。该函数通过不断的调用下面的这些成员函数完成闭环探测并触发全局 BA 优化操作，我们在注释中简单描述了各个函数的功能，详细的处理过程我们将在第四部分中一一解释。

        void Run();                                                   //! 闭环探测器的线程函数
        bool CheckNewKeyFrames();                                     //! 等待队列中是否有新的关键帧
        bool DetectLoop();                                            //! 探测回环
        bool ComputeSim3();                                           //! 计算闭环的两帧之间的 Sim3 变换
        void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap); //! 根据闭环和共视关系搜索地图点并融合
        void CorrectLoop();                                           //! 检测到闭环之后修正关键帧的位姿

由于全局的 BA 优化计算复杂度比较高，是一个非常耗时的工作，为了不影响闭环检测的操作，ORB_SLAM2 专门开辟了一个线程。下面的接口 RunGlobalBundleAdjustment 是在构造 std::thread 对象的时候，指定的线程函数，完成具体的优化。

        void RunGlobalBundleAdjustment(unsigned long nLoopKF); //! 进行全局的 BA 优化

类 LoopClosing 中还定义了一些成员函数，主要用于设置和查询各种标识，如下面的代码片段所示，不再一一展开介绍了。

展开 LoopClosing 各个标识的设置和查询接口 >>>

        void RequestReset();         //! 请求重置闭环探测器
        bool isRunningGBA();         //! 判定全局 BA 优化线程是否正在运行
        bool isFinishedGBA();        //! 判定全局 BA 优化是否已经结束
        void RequestFinish();        //! 请求终止闭环探测线程的工作
        bool isFinished();           //! 判定闭环探测线程是否已经终止
        void ResetIfRequested();     //! 检查复位标识 mbResetRequested，若置位，则重置探测器
        bool CheckFinish();          //! 检查结束标识 mbFinishRequested，若置位，则停止闭环探测线程
        void SetFinish();            //! 置位标识 mbFinished，终止线程

4. 完

本文中，我们从总体上解一下 ORB_SLAM2::LoopClosing 这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。详细的闭环探测和全局优化过程我们将在第四部分中进行分析。