29局部建图线程两级局部关键帧地图点融合

ORBSLAM2

---

---

代码

void LocalMapping::SearchInNeighbors()
{
    // Retrieve neighbor keyframes
    int nn = 10;
    if(mbMonocular)
        nn=20;
    const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);//取出共视关系前nn的关键帧，一级关键帧
    vector<KeyFrame*> vpTargetKFs;
    for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)//遍历相邻关键帧
    {
        KeyFrame* pKFi = *vit;//取出这个关键帧
        if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)//bad或者已经将进行了融合操作
            continue;//继续
        vpTargetKFs.push_back(pKFi);//将相邻关键帧存入vpTargetKFS中
        pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;//然后将相邻关键帧中的mnFuseTargetForKF设置成当前关键帧的ID，标记融合

        // Extend to some second neighbors
        const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);//取出相邻关键帧的前5帧相邻关键帧
        for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)//遍历这5帧二级关键帧
        {
            KeyFrame* pKFi2 = *vit2;//取出二级关键帧
            if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)//如果是bad或者二级关键帧已经融合过了或者在这个二级关键帧就是他自己本身
                continue;
            vpTargetKFs.push_back(pKFi2);//不然将其存入VPTargetKFS，这里面存的是当前关键帧的一级与二级关键帧
        }
    }

	//将一二级相邻关键帧取出存储好后，下面开始进行融合，分为正向融合与反向融合
    // Search matches by projection from current KF in target KFs
    ORBmatcher matcher;
    vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//得到当前关键帧的地图点匹配关系
    for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)//开始遍历一二级关键帧
    {
        KeyFrame* pKFi = *vit;//取出一二级关键帧

        matcher.Fuse(pKFi,vpMapPointMatches);//开始融合，正向
    }

    // Search matches by projection from target KFs in current KF，反向融合
    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();//获得地图点

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


    // Update points
    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
        MapPoint* pMP=vpMapPointMatches[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                pMP->ComputeDistinctiveDescriptors();
                pMP->UpdateNormalAndDepth();
            }
        }
    }

    // Update connections in covisibility graph
    mpCurrentKeyFrame->UpdateConnections();
}