23局部地图跟踪创建局部关键帧与地图点

ORBSLAM2

---

@[TOC](23局部地图跟踪创建局部关键帧与地图点)

---

代码

前面几部分是基于关键帧跟踪、重定位跟踪、基于恒速模型跟踪，只用到当前帧或几个关键帧，比较粗糙。而局部地图跟踪比较准确，它的前提是必须知道当前帧的位姿与地图点，让位姿更准确（有点后端的味道）。

void Tracking::UpdateLocalMap()
{
    // This is for visualization
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);//绘制参考参考地图点，用于显示局部地图点（红色）

    // Update
    UpdateLocalKeyFrames();//更新局部关键帧
    UpdateLocalPoints();//更新局部地图点
}

void Tracking::UpdateLocalKeyFrames()
{
    // Each map point vote for the keyframes in which it has been observed
    map<KeyFrame*,int> keyframeCounter;
    for(int i=0; i<mCurrentFrame.N; i++)//遍历当前特征点数
    {
        if(mCurrentFrame.mvpMapPoints[i])//如果当前帧有地图点
        {
            MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];//取出当前帧的地图点
            if(!pMP->isBad())//如果地图点是好的话
            {
                const map<KeyFrame*,size_t> observations = pMP->GetObservations();//得到当前观测到此地图点的关键帧以及其索引
                for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)//遍历观测关键帧
                    keyframeCounter[it->first]++;//这边对关键帧进行了投票，也就是keyframeCounter是一个map，里面的first是关键帧，second是一个int，取出it所对应的关键帧（能观测到此地图点的关键帧）存到keyframeCounter中，并使得其value=1，如果新进来的it所对应的关键帧在keypoint中存在，那么其所对应的value进行自增，如果没有就创建这一个map，这个的作用主要是用来表示这个关键帧能看到多少个地图点，也就是表示共视程度
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }

    if(keyframeCounter.empty())//没有观测就返回
        return;

    int max=0;
    KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);//存贮具有最多观测次数的关键帧

    mvpLocalKeyFrames.clear();//清空局部关键帧
    mvpLocalKeyFrames.reserve(3*keyframeCounter.size());//申请三倍内存

    // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
    for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)//开始遍历局部关键帧
    {
        KeyFrame* pKF = it->first;//取出局部关键帧

        if(pKF->isBad())//如果是bad则进行continue
            continue;

        if(it->second>max)//如果它观测到的地图点大于max
        {
            max=it->second;//就把他的值赋给max
            pKFmax=pKF;//并且将此关键帧赋给pkFmax，表示它是观测次数最多的关键帧
        }

        mvpLocalKeyFrames.push_back(it->first);//然后将此关键帧pushback到mvpLocalKeyFrames，添加到局部关键帧的列表中
        pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;//表示此关键帧已经是局部关键帧，并且将其ID进行添加，防止重复添加，一级局部关键帧
    }


    // Include also some not-already-included keyframes that are neighbors to already-included keyframes
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)//遍历一级局部关键帧
    {
        // Limit the number of keyframes
        if(mvpLocalKeyFrames.size()>80)//如果它的size大于80的话就break
            break;

        KeyFrame* pKF = *itKF;//取出一级局部关键帧

        const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);//取出与一级局部关键帧共视的10个关键帧，

        for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)//然后遍历这10个关键帧
        {
            KeyFrame* pNeighKF = *itNeighKF;//取出这些关键帧
            if(!pNeighKF->isBad())//如果不坏
            {
                if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)//判断是不是重复添加
                {
                    mvpLocalKeyFrames.push_back(pNeighKF);//就把它放到局部关键帧中
                    pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    break;
                }
            }
        }

        const set<KeyFrame*> spChilds = pKF->GetChilds();//获取关键帧的子关键帧
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)//遍历子关键帧
        {
            KeyFrame* pChildKF = *sit;//取出子关键帧
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    break;
                }
            }
        }

        KeyFrame* pParent = pKF->GetParent();//父关键帧。
        if(pParent)
        {
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                break;
            }
        }

    }

    if(pKFmax)//更新当前帧的参考关键帧
    {
        mpReferenceKF = pKFmax;
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
    }
}

void Tracking::UpdateLocalPoints()
{
    mvpLocalMapPoints.clear();//清空局部地图点

    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)//遍历局部关键帧
    {
        KeyFrame* pKF = *itKF;//取出局部关键帧
        const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();//取出局部关键帧中匹配好的地图点

        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)//迭代每个地图点
        {
            MapPoint* pMP = *itMP;
            if(!pMP)
                continue;
            if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)//避免重复添加地图点
                continue;
            if(!pMP->isBad())
            {
                mvpLocalMapPoints.push_back(pMP);
                pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
            }
        }
    }
}