欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

小白学习 orbslam2 -- localmapping

程序员文章站 2024-03-25 23:55:16
...

读之前提醒:首先,该文章是按照时间顺序讲述,并且不提前讲各种类关系和数据结构关系,而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的,然后告诉你这个东西是啥。如果你还没有读过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讲完了,该部分讲的比较粗糙,主要是逻辑比较简单。