(01)ORB-SLAM2源码无死角解析-(50) 局部建图线程→流程以及处理新关键帧:ProcessNewKeyFrame()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

在前面博客: (01)ORB-SLAM2源码无死角解析-(25) 关键帧KeyFrame→判断系统目前是否需要关键帧 (01)ORB-SLAM2源码无死角解析-(26) 关键帧KeyFrame→如何创建、插入关键帧 两篇博客中对关键帧的需求判断以及创建过程都进行了十分细致的讲解。 追踪线程中，会在合适的时候创建关键帧，这些关键帧除了追踪线程本身会用到，局部建图线程以及闭环线程都会用到，并且这里需要注意一个点:局部建图线程以及闭环线程不会对普通帧做处理，它们只关注关键帧。

从上图可以看出，局部建图线程主要包含了参入插入关键帧，剔除地图点，生成地图点，局部BA优化，剔除冗余关键帧。  

二、LocalMapping::Run()

那么先来看看 局部建图线程 LocalMapping::Run() 的主体流程，位于 LocalMapping.cc 文件中，注释如下(大致看一下即可，后续有详细的介绍)。

( 1 ) : \color{blue}{(1):} (1): 首先告诉追踪线程，目前处于繁忙状态，之前你送过来的关键帧都没有处理完，所以现在不要不要发送关键帧过来，等我处理完了再说。 ( 2 ) : \color{blue}{(2):} (2): 判断一下存储于 mlNewKeyFrames 变量中的关键帧是否都处理完成了，如果全部处理完成了，则会进一步判是否需要结束当前线程。 ( 3 ) : \color{blue}{(3):} (3): 如果 mlNewKeyFrames 变量中有关键帧需要处理，则对关键帧进行如下处理: ①计算BoW、更新观测、描述子、共视图，插入到地图。②根据地图点的观测情况剔除质量不好的地图点。③当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳。④ 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点。⑤当局部地图中的关键帧大于2个的时候进行局部地图的BA。⑥检测并剔除当前帧相邻的关键帧中冗余的关键帧。⑦将当前帧加入到闭环检测队列中。 ( 4 ) : \color{blue}{(4):} (4): 每处理完一帧关键帧，查看是否有复位线程的请求，并且取消繁忙状态，如果当前线程已经结束了就跳出主循环。  

// 线程主函数
void LocalMapping::Run()
{

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

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

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

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

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

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

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

                // Check redundant local Keyframes
                // 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();

        // 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));

    }

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

 

三、ProcessNewKeyFrame();

前面对 局部建图线程LocalMapping::Run() 的主体流程进行了讲解，那么现在就来看看其中调用的第一个函数ProcessNewKeyFrame()。其本身的逻辑还是很简单的：

( 1 ) : \color{blue}{(1):} (1): 从 mlNewKeyFrames 中取出列表最前面的关键帧，随后马上把列表中的该关键帧删除，然后对该关键帧进行处理。

( 2 ) : \color{blue}{(2):} (2): 利用关键帧特征点的BRIEF描述子计算BoW，主要获得两个变量:        ①mBowVec→单词word的id,以及该word对应的权重        ②mFeatVec→第一个元素为节点id，第二个元素为该节点id下所有特征点在图像中的索引。

( 3 ) : \color{blue}{(3):} (3): 获得关键帧的地图点，然后循环对每个地图点进行处理。如果地图点不是坏点，则进一步处理。

( 4 ) : \color{blue}{(4):} (4):如果该地图点存在于关键帧中：        ①如果地图点不是来自当前帧的观测（比如来自局部地图点），为当前地图点添加观测→pMP->AddObservation(mpCurrentKeyFrame, i);。        ②跟新该点的平均观测方向和观测距离范围→pMP->UpdateNormalAndDepth();        ③更新地图点的最佳描述子→pMP->ComputeDistinctiveDescriptors();

( 5 ) : \color{blue}{(5):} (5):如果该地图点不存在于关键帧中：如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息，这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生

( 6 ) : \color{blue}{(6):} (6): 更新关键帧间的连接关系(共视图)，将该关键帧插入到地图中。

代码位于 LocalMapping.cc 文件中，注释如下：

/**
 * @brief 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等
 * 
 */
void LocalMapping::ProcessNewKeyFrame()
{
    // Step 1：从缓冲队列中取出一帧关键帧
    // 该关键帧队列是Tracking线程向LocalMapping中插入的关键帧组成
    {
        unique_lock lock(mMutexNewKFs);
        // 取出列表中最前面的关键帧，作为当前要处理的关键帧
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        // 取出最前面的关键帧后，在原来的列表里删掉该关键帧
        mlNewKeyFrames.pop_front();
    }

    // Compute Bags of Words structures
    // Step 2：计算该关键帧特征点的词袋向量
    mpCurrentKeyFrame->ComputeBoW();

    // Associate MapPoints to the new keyframe and update normal and descriptor
    // Step 3：当前处理关键帧中有效的地图点，更新normal，描述子等信息
    // TrackLocalMap中和当前帧新匹配上的地图点和当前关键帧进行关联绑定
    const vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    // 对当前处理的这个关键帧中的所有的地图点展开遍历
    for(size_t i=0; iisBad())
            {
                if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
                {
                    // 如果地图点不是来自当前帧的观测（比如来自局部地图点），为当前地图点添加观测
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    // 获得该点的平均观测方向和观测距离范围
                    pMP->UpdateNormalAndDepth();
                    // 更新地图点的最佳描述子
                    pMP->ComputeDistinctiveDescriptors();
                }
                else // this can only happen for new stereo points inserted by the Tracking
                {
                    // 如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息
                    // 这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生
                    // 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验
                    mlpRecentAddedMapPoints.push_back(pMP); 
                }
            }
        }
    }    

    // Update links in the Covisibility Graph
    // Step 4：更新关键帧间的连接关系（共视图）
    mpCurrentKeyFrame->UpdateConnections();

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

从上面可以看到几个重要的函数，分别为 pMP->UpdateNormalAndDepth();pMP->ComputeDistinctiveDescriptors();以及mpCurrentKeyFrame->UpdateConnections()，下面则分别对其进行详细讲解。  

四、UpdateNormalAndDepth

在执行该函数之前，先执行了 pMP->AddObservation(mpCurrentKeyFrame, i)，也就是说地图点的可观测者(能观测到该点的关键帧)已经发生了变化，那么该地图点的平均观测方向是需要更新的，代码位于src/MapPoint.cc 文件中：

( 1 ) : \color{blue}{(1):} (1): 获得观测到该地图点的所有关键帧，观测到该点的参考关键帧（第一次创建时的关键帧），以及地图点在世界坐标的位置。

( 2 ) : \color{blue}{(2):} (2): 计算地图点的的平均观测方向，首先计算地图点被单个关键帧观测到的方向，也就是用地图点的世界坐标减去该关键帧相机的世界坐标，即 mWorldPos - Owi。然后把所有关键帧观测该地图点的方向 去平均值即可。

( 3 ) : \color{blue}{(3):} (3): 根据参考帧相机到该地图点的距离，然后结合金字塔层级更新观测距离范围。

/**
 * @brief 更新地图点的平均观测方向、观测距离范围
 *
 */
void MapPoint::UpdateNormalAndDepth()
{
    // Step 1 获得观测到该地图点的所有关键帧、坐标等信息
    map observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock lock1(mMutexFeatures);
        unique_lock lock2(mMutexPos);
        if(mbBad)
            return;

        observations=mObservations; // 获得观测到该地图点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
        Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    // Step 2 计算该地图点的平均观测方向
    // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
    // 初始值为0向量，累加为归一化向量，最后除以总数n
    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        // 获得地图点和观测到它关键帧的向量并归一化
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);                       
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 观测到该地图点的当前帧的特征点在金字塔的第几层
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数
    const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔总层数，默认为8

    {
        unique_lock lock3(mMutexPos);
        // 使用方法见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
        mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
    }
}

 

五、ComputeDistinctiveDescriptors

除了更新地图点的平均观测方向，还有可能需要更新最具有代表性的描述子。其主要思路：由于一个地图点会被许多相机观测到，因此在插入关键帧后，需要判断是否更新代表当前点的描述子先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。代码位于src/MapPoint.cc 文件中，注释如下：

/**
 * @brief 计算地图点最具代表性的描述子
 *
 * 由于一个地图点会被许多相机观测到，因此在插入关键帧后，需要判断是否更新代表当前点的描述子 
 * 先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值
 */
void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector vDescriptors;

    map observations;

    // Step 1 获取该地图点所有有效的观测关键帧信息
    {
        unique_lock lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());

    // Step 2 遍历观测到该地图点的所有关键帧，对应的orb描述子，放到向量vDescriptors中
    for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        // mit->first取观测到该地图点的关键帧
        // mit->second取该地图点在关键帧中的索引
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad())        
            // 取对应的描述子向量                                               
            vDescriptors.push_back(pKF->mDescriptors.row(mit->second));     
    }

    if(vDescriptors.empty())
        return;

    // Compute distances between them
    // Step 3 计算这些描述子两两之间的距离
    // N表示为一共多少个描述子
    const size_t N = vDescriptors.size();
	
    // 将Distances表述成一个对称的矩阵
    // float Distances[N][N];
	std::vector Distances;
	Distances.resize(N, vector(N, 0));
	for (size_t i = 0; imnId==mnId)
                continue;
            // 这里的操作非常精彩！
            // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
            // mit->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
            // 所以最后KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度
            KFcounter[mit->first]++;
        }
    }

    // This should not happen
    // 没有共视关系，直接退出 
    if(KFcounter.empty())
        return;

    // If the counter is greater than threshold add connection
    // In case no keyframe counter is over threshold add the one with maximum counter
    int nmax=0; // 记录最高的共视程度
    KeyFrame* pKFmax=NULL;
    // 至少有15个共视地图点才会添加共视关系
    int th = 15;

    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector vPairs;
    vPairs.reserve(KFcounter.size());
    // Step 2 找到对应权重最大的关键帧（共视程度最高的关键帧）
    for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }

        // 建立共视关系至少需要大于等于th个共视地图点
        if(mit->second>=th)
        {
            // 对应权重需要大于阈值，对这些关键帧建立连接
            vPairs.push_back(make_pair(mit->second,mit->first));
            // 对方关键帧也要添加这个信息
            // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
            // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);
        }
    }

    //  Step 3 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
    if(vPairs.empty())
    {
	    // 如果每个关键帧与它共视的关键帧的个数都少于th，
        // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
        // 这是对之前th这个阈值可能过高的一个补丁
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

    //  Step 4 对满足共视程度的关键帧对更新连接关系及权重（从大到小）
    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序
    sort(vPairs.begin(),vPairs.end());         // sort函数默认升序排列
    // 将排序后的结果分别组织成为两种数据类型
    list lKFs;
    list lWs;
    for(size_t i=0; iAddChild(this);
            mbFirstConnection = false;
        }
    }
}

 

七、结语

该篇博首先讲解了 LocalMapping::Run() 的总体流程，其中重要的几个函数分别为：

ProcessNewKeyFrame();
MapPointCulling();
CreateNewMapPoints();
SearchInNeighbors();
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
KeyFrameCulling();
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

目前我们只对 ProcessNewKeyFrame() 进行了详细的讲解，接下来的博客，对每一个函数进行细致的讨论。

    本文内容来自计算机视觉life ORB-SLAM2 课程课件