ORB-SLAM2 --- Tracking::UpdateLocalKeyFrames函数

最新推荐文章于 2024-05-08 22:01:18 发布

APS2023

最新推荐文章于 2024-05-08 22:01:18 发布

阅读量561

点赞数 1

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签：算法 c++ slam

本文链接：https://blog.csdn.net/qq_41694024/article/details/128307627

版权

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章 45 订阅

订阅专栏

4.2 更新局部关键帧（mvpLocalKeyFrames）

4.3 更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧

1.函数作用

跟踪局部地图函数里，更新局部关键帧。

2. 函数步骤

方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames。
Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧包括以下3种类型
* 类型1：能观测到当前帧地图点的关键帧，也称一级共视关键帧
* 类型2：一级共视关键帧的共视关键帧，称为二级共视关键帧
* 类型3：一级共视关键帧的子关键帧、父关键帧
Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧

3.code

void Tracking::UpdateLocalKeyFrames()
{
    // Each map point vote for the keyframes in which it has been observed
    // Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
    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++)
                    // 这里的操作非常精彩！
                    // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
                    // it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
                    // 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，也就是共视程度
                    keyframeCounter[it->first]++;      
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }

    // 没有当前帧没有共视关键帧，返回
    if(keyframeCounter.empty())
        return;

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

    // Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型
    // 先清空局部关键帧
    mvpLocalKeyFrames.clear();
    // 先申请3倍内存，不够后面再加
    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
    // Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧） 
    for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
    {
        KeyFrame* pKF = it->first;

        // 如果设定为要删除的，跳过
        if(pKF->isBad())
            continue;
        
        // 寻找具有最大观测数目的关键帧
        if(it->second>max)
        {
            max=it->second;
            pKFmax=pKF;
        }

        // 添加到局部关键帧的列表里
        mvpLocalKeyFrames.push_back(it->first);
        
        // 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id
        // 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧
        pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
    }


    // Include also some not-already-included keyframes that are neighbors to already-included keyframes
    // Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧 
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        // Limit the number of keyframes
        // 处理的局部关键帧不超过80帧
        if(mvpLocalKeyFrames.size()>80)
            break;

        KeyFrame* pKF = *itKF;

        // 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙）
        // 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧
        const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
        // vNeighs 是按照共视程度从大到小排列
        for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
        {
            KeyFrame* pNeighKF = *itNeighKF;
            if(!pNeighKF->isBad())
            {
                // mnTrackReferenceForFrame防止重复添加局部关键帧
                if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pNeighKF);
                    pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //? 找到一个就直接跳出for循环？
                    break;
                }
            }
        }

        // 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙）
        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;
                    //? 找到一个就直接跳出for循环？
                    break;
                }
            }
        }

        // 类型3:将一级共视关键帧的父关键帧（将邻居的父母们拉拢入伙）
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
            // mnTrackReferenceForFrame防止重复添加局部关键帧
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                //! 感觉是个bug！如果找到父关键帧会直接跳出整个循环
                break;
            }
        }

    }

    // Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
    if(pKFmax)
    {
        mpReferenceKF = pKFmax;
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
    }
}

4.函数解析

4.1 记录共视

我们遍历当前帧的所有地图点mCurrentFrame.mvpMapPoints[i]：

若地图点存在，用临时变量pMP存储该地图点，若该地图点没有被标记为isBad，则我们用一个map<KeyFrame*, size_t>型变量observations存储能观测到该地图点的关键帧及该地图点在关键帧中的索引，我们遍历每一个能看见该地图点的关键帧：

我们维护了一个map<KeyFrame*,int> keyframeCounter变量存储了当前帧和与当前帧有共视地图点的帧的共视程度，比如keyframeCounter[KeyFrame5,10]表示关键帧5和当前帧有的共视地图点为5个。

如下图：我们这步做的是找到所有红色关键帧和当前帧mCurrentFrame的共视地图点的数目并存储在keyframeCounter中。

如果keyframeCounter为空，则表示和当前关键帧有共视程度的关键帧为0，追踪局部地图失败！

4.2 更新局部关键帧（mvpLocalKeyFrames）

添加局部关键帧有3种类型：

1.能观测到当前帧地图点的关键帧作为局部关键帧（将邻居拉拢入伙）（一级共视关键帧）

这个就是我们如上图所示的红色关键帧，红色关键帧我们存储在了keyframeCounter的第一维中，我们遍历它的第一维，将和当前关键帧有共视的关键帧添加到局部关键帧的列表里，并将该一级共视帧的mnTrackReferenceForFrame标记置为当前帧的ID避免在接下来两步重复添加，并在keyframeCounter中寻找具有最大观测数目的共视关键帧pKFmax和它的得分max。

2.遍历一级共视关键帧，寻找更多的局部关键帧

我们遍历当前关键帧mCurrentFrame的一级关键帧的共视关键帧（和当前关键帧mCurrentFrame有共视的二级关键帧，如上图中绿色帧所示）

如果这个二级共视关键帧没有被添加过（mnTrackReferenceForFrame标记不是mCurrentFrame的ID），则也将这个关键帧添加到局部关键帧mvpLocalKeyFrames中。

3.将一级共视关键帧的子关键帧、父关键帧作为局部关键帧（将邻居的孩子们拉拢入伙、将邻居的父母们拉拢入伙）

这里没有过多的筛选条件，只要这个关键帧没有被添加过就可以。（mnTrackReferenceForFrame标记不是mCurrentFrame的ID）