ORB-SLAM3 LocalMapping源码分析（三）

2021SC@SDUSC

4. SearchInNeighbors：局部地图点的融合

产生新地图点（MapPoint）后，需要检查之前的keyframe和当前的keyframe之间的相同的地图点，并且进行融合，而这一整个操作是在SearchInNeighbors()函数中进行的。具体解释如下：

首先是将当前的keyframe的相邻关键帧加入到vpTargetKFs中，然后进行二级搜索，也就是把当前keyframe相邻的关键帧的再相邻的关键帧也都找出来，加入到vpTargetKFs中

void LocalMapping::SearchInNeighbors()
{
    //检索相邻关键帧
    int nn = 10;
    if(mbMonocular)
        nn=20;
    //获取当前关键帧与当前关键帧共视程度高的帧放入vpNeighKFs，并标记
    const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
    vector<KeyFrame*> vpTargetKFs;
    //遍历vpNeighKFs，遍历当前帧essential graph图中权重排名前nn的邻接关键帧
    for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        //如果这个keyframe是坏帧，或者已经被当前的关键帧标记则跳出循环进行push_back
        if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
            continue;
        vpTargetKFs.push_back(pKFi);
        pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
        //通过获取与pKFi共视程度高的关键帧来拓展vpNeighKFs
        const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
        for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
        {
            KeyFrame* pKFi2 = *vit2;
            if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
                continue;
            vpTargetKFs.push_back(pKFi2);
        }
    }

 通过两帧之间相互投影，计算出待fuse的点加入到vpFuseCandidates这个vector中，再进行一次matcher的fuse。然后再进行一次update。

    ORBmatcher matcher;
    
//取出当前关键帧的mappoint与pKFi中的mappoint进行融合
    vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    
//循环遍历整个vpTargetKFs中的keyframe
    for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        //将vpMapPoints中的mappoint与pKF的特征点进行匹配，若匹配的特征点已有mappoint与其匹配，
	    //则选择其一与此特征点匹配，并抹去没有选择的该MapPoint，这为MapPoint的融合
        matcher.Fuse(pKFi,vpMapPointMatches);
    }

    //获取vpTargetKFs所有良好MapPoints的集合
    vector<MapPoint*> vpFuseCandidates;
    vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());

    for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
    {
        KeyFrame* pKFi = *vitKF;

        vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();
//利用循环遍历地图点，根据pmp是不是坏的进行不同的操作
        for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
        {
            MapPoint* pMP = *vitMP;
            if(!pMP)
                continue;
            if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
                continue;
            pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
            vpFuseCandidates.push_back(pMP);
        }
    }

    matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);


    

计算出其融合后的深度

//这里利用循环更新mappoint的属性
    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
        MapPoint* pMP=vpMapPointMatches[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
               
 // 在这个mappoint能被看到的特征点中找出最能代表此mappoint进行描述
                pMP->ComputeDistinctiveDescriptors();
                
// 更新平均观测方向以及观测的距离
                pMP->UpdateNormalAndDepth();
            }
        }
    }
    mpCurrentKeyFrame->UpdateConnections();
}