ORB-SLAM2源码逐行解析系列（十八）：局部建图线程

最新推荐文章于 2024-01-01 09:29:24 发布

LDST_CSDN

最新推荐文章于 2024-01-01 09:29:24 发布

阅读量195

点赞数

文章标签：人工智能计算机视觉自动驾驶

本文链接：https://blog.csdn.net/LDST_CSDN/article/details/130672414

版权

ORB-SLAM2的局部建图线程负责处理关键帧，通过共视关系创建新地图点，优化位姿，剔除不合格点，并进行关键帧的管理。此过程包括处理新关键帧、地图点筛查、生成新地图点、融合地图点、剔除冗余关键帧等步骤，旨在提升跟踪稳定性与地图质量。

摘要由CSDN通过智能技术生成

1. 关于局部建图线程

（1）局部建图线程的内容

局部建图线程接收跟踪线程输入的关键帧，利用关键帧的共视关系生成新的地图点，搜索融合相邻关键帧的地图点，然后进行局部地图优化，删除冗余关键帧等操作，最后将处理后的关键帧发送给闭环线程。

（2）局部建图线程的作用

局部建图线程的目的是让已有的关键帧之间产生更多的联系，生成更多可靠的地图点，优化共视关键帧的位姿及其地图点，使得跟踪更稳定，参与闭环的关键帧位姿更准确。

2. 主线程

（1）大致流程

首先，设置LocalMapping线程繁忙SetAcceptKeyFrames
若关键帧缓冲队列不为空，则执行如下操作：
- 处理关键帧缓冲列表中的第一个关键帧ProcessNewKeyFrame
- 剔除质量不好的地图点MapPointCulling
- 共视关键帧之间生成新的地图点CreateNewMapPoints
- 在处理完缓冲队列中的所有关键帧前提下，融合当前帧与相邻帧中重复的地图点SearchInNeighbors
- 当局部地图中的关键帧大于2个的时候进行局部地图的BA
- 检测并剔除当前帧相邻的关键帧中冗余的关键帧KeyFrameCulling
- 将当前帧加入到闭环检测队列中
若关键帧缓冲队列为空，则等待当前线程执行完毕
检查复位请求，设置可接收关键帧标志位，退出循环

（2）代码实现

void LocalMapping::Run()
{
   

    // 标记状态，表示当前run函数正在运行，尚未结束
    mbFinished = false;
    // 主循环
    while(1)
    {
   
        // Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我
        // LocalMapping线程处理的关键帧都是Tracking线程发来的
        SetAcceptKeyFrames(false);

        // 等待处理的关键帧列表不为空
        if(CheckNewKeyFrames())
        {
   
            // Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
            ProcessNewKeyFrame();

            // Step 3 根据地图点的观测情况剔除质量不好的地图点
            MapPointCulling();

            // Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳
            CreateNewMapPoints();

            // 检查是否已经处理完队列中的最后的一个关键帧
            if(!CheckNewKeyFrames())
            {
   
                //  Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点
                SearchInNeighbors();
            }

            // 终止BA的标志
            mbAbortBA = false;

            // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
            if(!CheckNewKeyFrames() && !stopRequested())
            {
   
                // Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA
                if(mpMap->KeyFramesInMap()>2)
                    // 注意这里的第二个参数是按地址传递的
                    // 当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,
                                                     &mbAbortBA, mpMap);

                // Step 7 检测并剔除当前帧相邻的关键帧中冗余的关键帧
                // 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到
                KeyFrameCulling();
            }

            // Step 8 将当前帧加入到闭环检测队列中
            // 注意这里的关键帧被设置成为了bad的情况,这个需要注意
            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();

        // LocalMapping空闲  设置可接收关键帧
        SetAcceptKeyFrames(true);

        // 如果当前线程已经结束了就跳出主循环
        if(CheckFinish())
            break;

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

    }

    // 设置线程已经终止
    SetFinish();
}

3. 处理关键帧ProcessNewKeyFrame

局部建图线程中的关键帧来自于跟踪线程，这些关键帧会进入一个队列中，等待局部建图线程的处理，包括计算词袋向量、更新观测、描述子、共视图、将关键帧插入到地图中等。

（1）处理流程

从关键帧缓冲队列中取出一帧作为当前关键帧
计算当前关键帧的特征点词袋向量ComputeBoW
遍历当前关键帧的地图点，对于遍历到的有效地图点，执行如下操作：
- 若当前帧没有观测到该地图点，则为该地图点添加当前帧的观测信息、计算该地图点的平均观测方向、观测距离范围，以及该地图点对应的最佳描述子
- 若当前关键帧观测到该地图点，却没有这个关键帧的信息，则存放到新增地图点队列中，等待后续是否要剔除
更新当前关键帧和它共视关键帧之间的连接关系
将当前关键帧插入到地图中

（2）代码实现

/**
 * @brief 处理缓冲列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
 * 
 */
void LocalMapping::ProcessNewKeyFrame()
{
   
	// Step 1：从缓冲队列中取出一帧作为当前关键帧
    // 该关键帧缓冲队列是由Tracking线程向LocalMapping中插入的关键帧组成
    {
   
        unique_lock<mutex> lock(mMutexNewKFs);
        // 取出列表中最前面的关键帧，作为当前要处理的关键帧
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        // 取出最前面的关键帧后，在原来的列表里删掉该关键帧
        mlNewKeyFrames.pop_front();
    }
    
    // Step 2：计算取出的该关键帧特征点的词袋向量
    mpCurrentKeyFrame->ComputeBoW();
    
    // Step 3：对于当前关键帧中有效的地图点，更新平均观测方向normal，观测距离范围，最佳描述子等信息
    // 获取当前帧的地图点
    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    // 对当前处理的这个关键帧中的所有的地图点展开遍历
    for(size_t i=0; i<vpMapPointMatches.size(); i++)
    {
   
        MapPoint* pMP = vpMapPointMatches[i];  // 获取遍历到的地图点
        if(pMP)
        {
   
            if(!pMP->isBad())
            {
   
                // 检查该地图点是否可被当前关键帧观测到
				if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))  
                {
   
                    // 如果地图点不是来自当前帧的观测（比如来自局部地图点），为当前地图点添加观测
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    // 获得该点的平均观测方向和观测距离范围
                    pMP->UpdateNormalAndDepth();
                    // 更新地图点的最佳描述子
                    pMP->ComputeDistinctiveDescriptors();
                }
                else
                {
   
					// 如果当前帧可观测到这个地图点，但却没有这个关键帧的信息
                    // 则这些地图点是 CreateNewMapPoints 中通过三角化产生的新地图点
                    // 将这些地图点放入最近新增地图点队列中，等待后续地图点剔除函数的检验
                    mlpRecentAddedMapPoints.push_back(pMP);
                }
            }
        }
    }
    
    // Step 4：更新当前关键帧和它的共视关键帧之间的连接关系（共视图）
    mpCurrentKeyFrame->UpdateConnections();

    // Step 5：将该关键帧插入到地图中
    mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

4. 剔除不合格的地图点MapPointCulling

ORB-SLAM2局部建图线程中使用的新增地图点主要来自两个地方：第一，在跟踪线程中处理新关键帧时，在双目/RGB-D相机模式下产生新地图点；第二，在局部建图线程中，关键帧之间产生新地图点。

但这些新增地图点需要进行严格筛查，一方面可以提高定位与建图的准确性，另一方面还能控制地图规模，降低计算量，使得ORB-SLAM2可以在较大场景下运行。筛查条件如下，只要满足二者之一，便需要剔除：

跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%
从该点建立开始，到现在已经超过2个关键帧，但观测到该点的相机数目未超过相应阈值

代码实现

/**
 * @brief 检查新增地图点，根据地图点的观测情况剔除质量不好的新增地图点
 * mlpRecentAddedMapPoints：在ProcessNewKeyFrame中存储的新增地图点
 * 对于不靠谱的地图点，从mlpRecentAddedMapPoints和地图中均删除掉
 * 对于质量较高的地图点，仅删除mlpRecentAddedMapPoints中的，地图中的不删除
 */
void LocalMapping::MapPointCulling()
{
   
    // 当前新增地图点的迭代器
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    // 当前关键帧的id
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
    
    // Step 1：根据相机类型设置不同的观测阈值
    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;
    
    // Step 2：遍历检查新添加的地图点
    while(lit!=mlpRecentAddedMapPoints.end())
    {
   
		MapPoint* pMP = *lit;
        if(pMP->isBad())
        {
   
            // Step 2.1：已经是坏点的地图点仅从队列中删除 (疑问: 为什么地图中不删除？？？)
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(pMP->GetFoundRatio()<0.25f)
        {
   
/*
    Step 2.2：跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%从列表以及地图中删除
              (mnFound/mnVisible） < 25%
              mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好
              mnVisible：地图点应该被看到的次数
              (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
*/
            pMP->SetBadFlag();  // 地图中删除该点
            lit = mlpRecentAddedMapPoints.erase(lit);  // 队列中删除该点
        }
        // nCurrentKFid: 当前帧的id  pMP->mnFirstKFid: 创建该地图点的帧id
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && 
                pMP->Observations()<=cnThObs)
        {
   
            // Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧
            // 但是观测到该点的相机数却不超过阈值cnThObs，从列表以及地图中删除
            pMP->SetBadFlag();  // 地图中删除该点
            lit = mlpRecentAddedMapPoints.