ORB-SLAM2源码阅读(2)

Local Mapping线程

我们是以mono_tim.cc示例程序为例的，在该例中首先需要构建ORB-SLAM系统

// Create SLAM system. It initializes all system threads and gets ready to process frames.
//构建SLAM系统，调用有参构造函数，传入参数为：ORB字典，参数配置文件，相机类型
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);

System的函数定义中，初始化了Local Mapping、Loop Closing等线程，详细可以看我们上一篇博客

在上一篇博客中已经介绍了ORB-SLAM的主要线程：Tracking线程，今天主要介绍一下剩下的两个线程

//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

可以看到初始化Local Mapping线程主要通过ORB_SLAM2::LocalMapping::Run( )函数实现，来看一下这个函数的实现

void LocalMapping::Run()
{

    mbFinished = false;

    while(1)
    {
        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(false);

        // Check if there are keyframes in the queue
        if(CheckNewKeyFrames())     //若队列中有新的关键帧
        {
            // BoW conversion and insertion in Map
            ProcessNewKeyFrame();

            // Check recent MapPoints
            MapPointCulling();

            // Triangulate new MapPoints
            CreateNewMapPoints();

            if(!CheckNewKeyFrames())    //队列中的关键帧处理完毕
            {
                //检查Current KF的Covisible KFS,对重复构建的MapPoints进行融合
                // Find more matches in neighbor keyframes and fuse point duplications
                SearchInNeighbors();
            }

            mbAbortBA = false;

            if(!CheckNewKeyFrames() && !stopRequested())    //没有新的KF且无停止LoopClosing线程的请求
            {
                // Local BA
                if(mpMap->KeyFramesInMap()>2)   //地图中的关键帧要大于两帧
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

                //剔除冗余关键帧
                // Check redundant local Keyframes
                KeyFrameCulling();
            }

            //筛选后的关键帧插入LoopClosing线程
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        else if(Stop())
        {
            //确保KF筛选完成
            // Safe area to stop
            while(isStopped() && !CheckFinish())
            {
                usleep(3000);
            }
            if(CheckFinish())
                break;
        }

        //在收到请求后可以删除新添加的KF和MapPoints
        ResetIfRequested();

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(true);

        //等待插入完成
        if(CheckFinish())
            break;

        usleep(3000);
    }

    SetFinish();
}

我们可以再对照着这张图看一下程序

请添加图片描述

可以看到其主要步骤为：

接收从Tracking线程插入的KF,并进行预处理
当 Tracking 线程确定一个要插入的 KF 时，实际上它并没有真的完成将 KF 插入 Map 的动作，当将一个 KF 插入 Map 中时，需要同时做很多更新工作
更新Covisibility Graph；
更新生成树：
计算新KF的BoW
剔除质量较差的MapPoints
存储在 Map 中的 MapPoints 需要有较高的质量（追踪良好，三角化正确），所以此处需要采取一些措施去掉质量较差的 MapPoints
通过三角化生成新的MapPoints
ORB-SLAM 将在 Current KF 的未能与已存在 MapPoints 匹配上的 FeaturePoints，与其 Covisible KFs 中同样未能与已存在 MapPoints 匹配上的 FeaturePoints 进行匹配
如果匹配上了，则可以通过三角化，生成一个新的 MapPoint（生成之后要检查其位置，视差，重投影误差，尺度一致性）
这个 FeaturePoint 与 FeaturePoint 之间的匹配是通过 BoW 搜索实现的
通过两个 KFs 生成新的 MapPoint 后，还要检查它有没有在别的 KFs 中出现。所以要将该 MapPoint 投影至其他的 Covisible KFs，与它们的 FeaturePoints 进行匹配
匹配上的话就将该 MapPoint 与那个 KF 的那个 FeaturePoint 链接上
局部地图BA优化
对 Current KF 及其 Covisible KFs 及其它们所观察到的所有 MapPoints 进行 BA 优化
剔除冗余的局部关键帧
在 Tracking 线程中，ORB-SLAM 以非常宽松的条件，向 Map 中插入了很多很多 KFs，但显然 Map 中是不能永久保留这么多 KFs 的，这会使 Map 过于庞大，且极大增加各种 BA 的运算量，所以要剔除一些信息冗余的
如果 Current KF 及其 Covisible KFs 中，有哪个 KF 它所观测到的 90% 的 MapPoints 都能被其它至少3个（尺度相同或更好的）KFs 观测到，则这个 KF 的信息就算作是冗余的，就把它去掉。这样做的目的是让 Map 的 KF 数不要太多，且在规模一定的场景内，Map 中的 KF 数目不要无上限的增长，这样也有利于减轻 BA 优化的负担

对应程序流程图如下：

请添加图片描述

Loop Closing线程

Loop Closing线程主要通过ORB_SLAM2::LoopClosing::Run( )函数实现

mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

具体函数如下：

void LoopClosing::Run()
{
    mbFinished =false;

    while(1)
    {
        // Check if there are keyframes in the queue
        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
               // 计算SIM3或SE3，确定最终的Loop KF
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }       

        ResetIfRequested();

        if(CheckFinish())
            break;

        usleep(5000);
    }

    SetFinish();
}