ORB-SLAM2源码阅读(2)

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();
}

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

可以看到其主要步骤为：

1. 接收从Tracking线程插入的KF,并进行预处理
   当 Tracking 线程确定一个要插入的 KF 时，实际上它并没有真的完成将 KF 插入 Map 的动作，当将一个 KF 插入 Map 中时，需要同时做很多更新工作
   更新Covisibility Graph；
   更新生成树：
   计算新KF的BoW
2. 剔除质量较差的MapPoints
   存储在 Map 中的 MapPoints 需要有较高的质量（追踪良好，三角化正确），所以此处需要采取一些措施去掉质量较差的 MapPoints
3. 通过三角化生成新的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 链接上
4. 局部地图BA优化
   对 Current KF 及其 Covisible KFs 及其它们所观察到的所有 MapPoints 进行 BA 优化
5. 剔除冗余的局部关键帧
   在 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();
}

再祭上这张图：

Loop Closing线程可以分为两步：

1. 回环检测：Loop Detection
2. 回环校正：Loop Correction

• DetectLoop( )函数检测出一批Candidate KFs
• ComputeSim3( )函数计算Current KF与Candidiate KF之间的相似变换，并据此确定最终的Loop KF
• CorrectLoop( )函数进行回环校正

上述步骤全部进行完了之后，精度已经很高了，但 ORB-SLAM2 还是选择在最后再进行一次 Global BA 锦上添花（ORB-SLAM1 中似乎没有这步）

注意，为了不影响主要3个线程的工作，这里创建了第4个线程，专门进行 Global BA，但该 Global BA 随时可能被打断，只有在系统特别闲的时候才会运行

对应程序流程图如下：

主要参考

ORB-SLAM2 论文&代码学习 —— LocalMapping 线程 - MingruiYu - 博客园 (cnblogs.com)

ORB-SLAM2 论文&代码学习 —— LoopClosing 线程 - MingruiYu - 博客园 (cnblogs.com)

如有侵权，请联系删除