orbslam2学习笔记 -- localmapping

读之前提醒：首先，该文章是按照时间顺序讲述，并且不提前讲各种类关系和数据结构关系，而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的，然后告诉你这个东西是啥。如果你还没有读过tracking的部分，请先读tracking，因为很多概念会在tracking里先讲出来，后面再遇到，就不会细讲了。

tracking : https://blog.csdn.net/michaelhan3/article/details/89742663

 

      该部分是一个独立的线程，靠一个死循环执行。只讲与知识相关的，至于线程间的锁，或者联系等这里是忽略的。言归正传，开始讲述。下面代码来自泡泡机器人大神的注释。我只讲几个关键的函数。

    while(1)
    {
        // Tracking will see that Local Mapping is busy
        // 告诉Tracking，LocalMapping正处于繁忙状态，
        // LocalMapping线程处理的关键帧都是Tracking线程发过的
        // 在LocalMapping线程还没有处理完关键帧之前Tracking线程最好不要发送太快
        SetAcceptKeyFrames(false);

        // Check if there are keyframes in the queue
        // 等待处理的关键帧列表不为空
        if(CheckNewKeyFrames())
        {
            // BoW conversion and insertion in Map
            // VI-A keyframe insertion
            // 计算关键帧特征点的BoW映射，将关键帧插入地图
            ProcessNewKeyFrame();

            // Check recent MapPoints
            // VI-B recent map points culling
            // 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints
            MapPointCulling();

            // Triangulate new MapPoints
            // VI-C new map points creation
            // 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints
            CreateNewMapPoints();

            // 已经处理完队列中的最后的一个关键帧
            if(!CheckNewKeyFrames())
            {
                // Find more matches in neighbor keyframes and fuse point duplications
                // 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
                SearchInNeighbors();
            }

            mbAbortBA = false;

            // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
            if(!CheckNewKeyFrames() && !stopRequested())
            {
                // VI-D Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

                // Check redundant local Keyframes
                // VI-E local keyframes culling
                // 检测并剔除当前帧相邻的关键帧中冗余的关键帧
                // 剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到
                // trick! 
                // Tracking中先把关键帧交给LocalMapping线程
                // 并且在Tracking中InsertKeyFrame函数的条件比较松，交给LocalMapping线程的关键帧会比较密
                // 在这里再删除冗余的关键帧
                KeyFrameCulling();
            }

            // 将当前帧加入到闭环检测队列中
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        else if(Stop())
        {
            // Safe area to stop
            while(isStopped() && !CheckFinish())
            {
                // usleep(3000);
                std::this_thread::sleep_for(std::chrono::milliseconds(3));
            }
            if(CheckFinish())
                break;
        }

        ResetIfRequested();

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

        if(CheckFinish())
            break;

        //usleep(3000);
        std::this_thread::sleep_for(std::chrono::milliseconds(3));

    }

函数ProcessNewKeyFrame()

       该函数的作用是，从队列取出一个关键帧，计算该关键帧的Bow特征，更新关键帧观测到的地图点的信息，并且将该关键帧新生成的地图点添加进mlpRecentAddedMapPoints，等待后续检测。然后将该关键帧插入地图。

 

函数MapPointCulling()

       该函数的主要作用是筛选mlpRecentAddedMapPoints里的点，对于不好的点标记为bad。

1：已经是坏点的MapPoints直接从检查链表中删除，SetBadFlag()

2：跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%，SetBadFlag()

      IncreaseFound / IncreaseVisible < 25%，注意不一定是关键帧。

3：从该点建立开始，到现在已经过了不小于2个关键帧，但是观测到该点的关键帧数却不超过cnThObs帧，那么该点检验不合格。SetBadFlag()

4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点，因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测

mlpRecentAddedMapPoints剩下的点，需要继续经过以后的检测。

 

函数CreateNewMapPoints()

       相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints。首先得到与当前关键帧共视程度前10或者20的关键帧。然后遍历每个共视帧。根据当前帧和共视帧的位姿，计算他们之间的基础矩阵cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);。

然后matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);该函数的主要作用就是寻找当前帧和共视帧之间的匹配关系。匹配的方法也很简单，和SearchBow（）方式一样，通过比较同一个单词（node）下的描述符距离，求出最优匹配。不同的是在该函数里加入了基础矩阵的极平面约束。在循环找最优距离的时候，只有符合极平面约束的匹配，才会去更新最优匹配距离。

然后根据当前的匹配，新建立一些新的地图点。并且添加到后续检查队列里mlpRecentAddedMapPoints。建立新的地图点的方法是，如果两个相机对地图点观测的视差角度小时用三角法恢复3D点，视差角大时用双目恢复3D点（双目以及深度有效）。三角化方法见 https://blog.csdn.net/michaelhan3/article/details/89483148里的 (2) 线性三角化法。并且利用求出来的三维点坐标计算在两个关键帧上的重投影误差，利用卡方验证方法做阈值，删除重投影误差大的点。

 

函数SearchInNeighbors()

       检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints，更新当前关键帧的连接关系。

 

局部优化LocalBundleAdjustment

       优化的顶点是包括局部地图帧的位姿，概念lLocalKeyFrames，指的是当前关键帧和其相连接的关键帧组成的集合。还包括这些关键帧可以观测到的所有地图点，地图点的位置也会优化。还有一些帧，这些帧能够观测到这些地图点，但却不是局部地图里，这些帧的位姿也作为顶点添加进图中，但是却固定不动，不会被优化。

 

删除冗余关键帧KeyFrameCulling()

在Covisibility Graph，也就是局部地图中的关键帧，一个关键帧的90%以上的MapPoints能被其他关键帧（至少3个，这里的其他关键帧不特指当前的局部地图关键帧）观测到，则认为该关键帧为冗余关键帧。

 

最后将当前关键帧加入闭环检测队列。

 

至此，localmapping讲完了，该部分讲的比较粗糙，主要是逻辑比较简单。