(01)ORB-SLAM2源码无死角解析-(47) 跟踪线程→局部地图跟踪TrackLocalMap():局部地图更新

江南才尽，年少无知！

已于 2022-11-02 17:48:48 修改

阅读量1.1k

点赞数 1

分类专栏： # (01)ORB-SLAM2源码无死角解析-免费文章标签： SLAM 计算机视觉现实增强智能驾驶

于 2022-08-02 14:57:14 首次发布

原文链接：https://cvlife.net/detail/term_634260915e015_SerHfL/25?product_id=term_634260915e015_SerHfL

版权

(01)ORB-SLAM2源码无死角解析-免费专栏收录该内容

50 篇文章 106 订阅

订阅专栏

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过前面的博客，已经对参考关键帧追踪TrackReferenceKeyFrame()，恒速模型跟踪当前普通帧TrackWithMotionModel() 以及重定位跟踪 Relocalization()。他们分别适用于不同的情况，具体就不再这里提及了，因为在前面都进行了细致的分析，其中恒速模型跟踪当前普通帧TrackWithMotionModel() 被使用得最多。

也就是说，对当前帧进行追踪，是从上诉的三种选择一种进行追踪，或者说位姿估算。这里可以很明显的看到，TrackReferenceKeyFrame() 与 TrackWithMotionModel() 追踪利用的都是一帧图像(最近关键帧或者上一帧)，可想而知，其忽略了很多有价值的信息。虽然 Relocalization() 追踪使用了很多关键帧，利用的信息还是比较充足的，但是只有在追踪丢失的情况下才使用重定位追踪。

首先我们在src/Tracking.cc文件中，可以看到如下代码：

        // If we have an initial estimation of the camera pose and matching. Track the local map.
        // Step 3：在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
        // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
        if(!mbOnlyTracking)
        {
            if(bOK)
                bOK = TrackLocalMap();
        }
        else
        {
            // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
            // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
            // the camera we will use the local map again.

            // 重定位成功
            if(bOK && !mbVO)
                bOK = TrackLocalMap();
        }

可以看到，使用上诉三种跟踪方式中任意一种，只要跟踪成功，紧接着就会进行局部地图追踪 TrackLocalMap()。局部地图追踪，可以理解为对上诉上种追踪获得位姿的优化。因为局部地图跟踪会采用更多的地图点进行追踪。

$\color{red}{核心思想:}$ 在追踪的过程中，随着当前帧在一帧一帧的变化，可想而知，当前帧可以观测到的地图点也在变化，利用已经存在的地图点可以计算出相机位姿，同时当前帧会剩下一些未匹配上的关键点，那么这些关键点结合位姿则可以产生新的地图点，这些新的地图点可以提供给下一帧用于位姿估算。

大家这里注意一个点，当前帧生成新的地图点(核心代码为Tracking::UpdateLastFrame())，一般只会只会用于更新局部地图，只有当前帧为关键帧时，进行关键帧创建才会生成新的全局地图点。下面我们先来了解局部关键帧以及局部地图点的概念。

二、局部关键帧

从命名可以看到出来，所谓局部地图跟踪那么肯定是根据局部地图点进行跟踪，在了解局部地图点之前，先来看看局部关键帧。首先来查看下图:
在这里插入图片描述
$\color{blue}{一级共视关键帧:}$ 假若当前帧为F，其能观测到地图中N个地图点，那么能够观测到这些地图点(至少一个)的关键帧，称为一级共视关键帧。如上图，当前帧F能观测到红色的三个地图点，同时这三个地图点又被 KF1与 KF2 观测到，那么那么称 KF1与 KF2 为前帧F 的一级共视关键帧。

$\color{blue}{二级共视关键帧:}$ F的一级共视关键帧 KF1与 KF2 分别能够观测到上图的 KF1地图点(绿色)，KF2地图点(绿色)。那么称能够观测到 KF1地图点的关键帧为 KF1的共视关键帧，也叫 KF1的一级共视关键帧。此时 KF1的共视关键帧对于 F来说，就是二级共视关键帧。同理，KF2 对于 F 来说，也就是二级共视关键帧。

$\color{blue}{当前帧}:$ mCurrentFrame(当前帧是普通帧)
$\color{blue}{参考关键帧:}$ 与当前帧共视程度最高的关键帧作为参考关键帧，mCurrentFrame.mpReferenceKF，在KeyFrame::UpdateConnections() 里确定关键帧的父子关系（当前帧必须是关键帧）
$\color{blue}{父关键帧:}$ 和当前关键帧共视程度最高的关键帧
$\color{blue}{子关键帧:}$ 是上述父关键帧的子关键帧
$\color{blue}{mvpLocalMapPoints:}$ 示意图

$\color{red}{局部关键帧:}$
        ①当前帧的一级共视关键帧(包含所有)。
        ②当前帧的二级共视关键帧(只取了共视程度最高的一帧)。
        ③前面两种共视帧的父关键帧与子关键帧

局部关键帧主要包含了上述三种关键帧，可想而只，随着当前帧在变化，那么当前帧观测到的地图点也随着变化，从而局部关键帧也在变化。更新局部关键帧的代码位于 sec/Tracking.cc 文件中，注释如下：

/**
 * @brief 跟踪局部地图函数里，更新局部关键帧
 * 方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames
 * Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧 
 * Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧包括以下3种类型
 *      类型1：能观测到当前帧地图点的关键帧，也称一级共视关键帧
 *      类型2：一级共视关键帧的共视关键帧，称为二级共视关键帧
 *      类型3：一级共视关键帧的子关键帧、父关键帧
 * Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
 */
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;
    }
}

三、局部地图点

局部关键帧能够观测到的所有地图点，总和起来，称为局部地图点，如下图，其中红色的部分称为就是局部地图点：
在这里插入图片描述
其代码还是十分简单的，位于 sec/Tracking.cc 文件中，注释如下：

/*
 * @brief 更新局部关键点。先把局部地图清空，然后将局部关键帧的有效地图点添加到局部地图中
 */
void Tracking::UpdateLocalPoints()
{
    // Step 1：清空局部地图点
    mvpLocalMapPoints.clear();

    // Step 2：遍历局部关键帧 mvpLocalKeyFrames
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        KeyFrame* pKF = *itKF;
        const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();

        // step 2：将局部关键帧的地图点添加到mvpLocalMapPoints
        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
        {
            MapPoint* pMP = *itMP;
            if(!pMP)
                continue;
            // 用该地图点的成员变量mnTrackReferenceForFrame 记录当前帧的id
            // 表示它已经是当前帧的局部地图点了，可以防止重复添加局部地图点
            if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
                continue;
            if(!pMP->isBad())
            {
                mvpLocalMapPoints.push_back(pMP);
                pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
            }
        }
    }
}

四、结语

上述的的两个函数 UpdateLocalKeyFrames 与 UpdateLocalPoints 都是在 Tracking::UpdateLocalMap() 中被调用，如下所示：

/**
 * @brief 更新LocalMap
 *
 * 局部地图包括： 
 * 1、K1个关键帧、K2个临近关键帧和参考关键帧
 * 2、由这些关键帧观测到的MapPoints
 */
void Tracking::UpdateLocalMap()
{
    // This is for visualization
    // 设置参考地图点用于绘图显示局部地图点（红色）
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    // Update
    // 用共视图来更新局部关键帧和局部地图点
    UpdateLocalKeyFrames();
    UpdateLocalPoints();
}

可以明显的知道，更新了局部关键帧与局部地图点之后，相当于对局部地图进行了更新，那么其更新的目的时什么呢？当然是为了局部地图跟踪TrackLocalMap() 服务，在下一篇博客中进行详细的讲解。

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

江南才尽，年少无知！

关注

1
点赞
踩
5

收藏

觉得还不错? 一键收藏
1
评论
(01)ORB-SLAM2源码无死角解析-(47) 跟踪线程→局部地图跟踪TrackLocalMap():局部地图更新

通过前面的博客，已经对参考关键帧追踪TrackReferenceKeyFrame()，恒速模型跟踪当前普通帧TrackWithMotionModel() 以及重定位跟踪 Relocalization()。他们分别适用于不同的情况，具体就不再这里提及了，因为在前面都进行了细致的分析，其中恒速模型跟踪当前普通帧TrackWithMotionModel() 被使用得最多。也就是说，对当前帧进行追踪，是从上诉的三种选择一种进行追踪，或者说位姿估算。
复制链接

扫一扫