(01)ORB-SLAM2源码无死角解析-(34) 跟踪线程→初步了解跟踪线程,参考关键帧追踪TrackReferenceKeyFrame()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过前面博客，相信大家已经了解很多 SLAM 的基础知识，比如ORB特征点、BRIEF描述子；根据Essential、Fundamental、Homography矩阵求解位姿；以及通过BoW加速特征匹配等。有了这些基础知识之后，下面就可以深入的了解 ORB-SLAM2 的整体架构了。ORB-SLAM2 的总体框架，图示如下：

该图主要表达了三个线程: 跟踪线程、局部建图线程、闭环线程大致流程。可以看到跟踪线程主要包含了: $提取ORB特征点$、$地图初始化$、$恒速模型跟踪$、$参考关键帧跟踪$、$重定位跟踪$、$局部跟踪等$。其中 提取ORB特征点 以及 地图初始化 已经进行过详细讲解。

另外，众所周知 SLAM 主要包含了定位与建图。这里所谓的定位，就是对相机进行定位，估算相机的位置与姿态(简称位姿)。跟踪线程的目标就是相机，计算相机的位姿。为什么呢? 如果把摄像头安装在机器人上面，知道了摄像头的位姿，就相当于知道了机器人的位姿，如果再结合建图，就能够实现很多功能了，扫地机器人就是一个很好的例子。

下面接下来要讲解的内容就是跟踪，跟踪线程的主要代码流程在 (01)ORB-SLAM2源码无死角解析-(13)追踪总体框架讲解→Tracking::Track() 博客中，进行了很粗略的讲解，估计大家再看的时候应该比较蒙蔽，其实大家只要领会核心思想即可。

$核心思想:$ 当前帧与上一帧(或者最近关键帧)进行特征点匹配，如果匹配点足够，则能计算出上一帧到当前帧的位姿变换，同时能够获得该关键点对在三维空间的坐标，显然易知对应的地图点。依次循环即可。

接下来，就会把跟踪线程拆分进行讲解。

 

二、跟踪线程图示

跟踪线程中，主要有两种模式，分别为SLAM(建图与定位)模式，仅定位模式。这两种模式涉及的内容是比较相似的，仅定位模式缺少了地图更新部分，相对来说跟简单一点。这里以 SLAM(建图与定位) 模式为例，因为其为常用模式，为大家梳理一下流程(结合上图) ：

$(01):$ 相机传入第一张图像实现的时候，肯定没有进行初始化， 所以会进行地图初始化。如果为单目摄像头则会初始化失败，因为单目相机初始化需要连续两帧为关键帧才有可能初始化成功。

$(02):$ 初始化完成之后，下一帧进来为正常状态，其首先会检查并且替换上一帧的地图点。

$(03):$ 完成地图点的替换之后，会判断目前速度是否为空(如果上一帧才完成初始化，则速度为空)。速度为空使用参考关键帧跟踪。如果速度不为空那么就使用恒速模式跟踪。

$(04):$ 参考关键帧跟踪 或者 恒速模式跟踪 成功之后，会进入到局部地图跟踪(进行整体的位姿优化)，如果局部地图跟踪成功则会进行速度更新，为下次的恒速模式跟踪做铺垫。

$(05):$ 另外，如果在跟踪的过程中，出现跟踪状态不正常(一般为匹配特征点、或者地图点过少)，则使用重定位跟踪

 

三、核心函数列举

其上的过程中，对应源码，位于 Tracking.cc 文中的 Tracking::Track() 函数。该函数调用的如下几个函数是比较重要的：

	//双目与单目初始化，其中单目初始化在前面已经进行过十分详细的讲解
	StereoInitialization();
	MonocularInitialization();
		
	CheckReplacedInLastFrame();// 检查并更新上一帧被替换的MapPoints

	TrackReferenceKeyFrame();// 用最近的关键帧来跟踪当前的普通帧
	TrackWithMotionModel() // 用最近的普通帧来跟踪当前的普通帧
	Relocalization() //  如果跟踪状态不成功,那么就只能重定位了
	
	TrackLocalMap() //用局部地图进行跟踪，进一步优化位姿
	
	NeedNewKeyFrame() //判断是否需要创建关键帧
	CreateNewKeyFrame() //如果需要创建关键帧则创建关键帧

其上有些函数已经在前面分析过了，接下来的几篇博客，主要是对其上TrackReferenceKeyFrame(), TrackWithMotionModel()， Relocalization() 三个核心跟踪函数进行讲解。

 

四、TrackReferenceKeyFrame()

根据前面的分析，可以知道，在初始化完成之后，紧接下来一帧使用的跟踪方式为参考帧跟踪，简单的说就是通过参考最近的关键帧，估算出当前帧的位姿。 其对应于 src/Tracking.cc 文件中的 TrackReferenceKeyFrame() 函数。该函数还是比较简单的，主要流程如下：
$(01):$ 将当前帧的描述子转化为BoW向量，该内容已经在前面进行了详细的讲解，主要作用就是对特征点进行归类，把所属节点相同的特征点归为一类，存储于Frame 的 mBowVec 与 mFeatVec 成员变量之中。

$(02):$ 根据计算出来的BoW向量进行特征匹配，该对应的函数为 SearchByBoW(),在前面进行过很详细的讲解。其返回的是关键帧与当前帧匹配特征点对，对应的地图点，也就是 vpMapPointMatches。

$(04):$ 首先将上一帧的的位姿作为当前帧的初始值(在PoseOptimization可以收敛快一些)，然后通过优化3D-2D的重投影误差来获得位姿,该内容在下一篇博客中进行详细的讲解。

$(05):$ 剔除优化后的匹配点中的外点，清除外点在当前帧中存在过的痕迹。扣除外点之后匹配数目在10个以上，则认为是追踪成功。

 

五、源码注释

src/Tracking.cc 文件中的 TrackReferenceKeyFrame() 函数注释如下：

/*
 * @brief 用参考关键帧的地图点来对当前普通帧进行跟踪
 * 
 * Step 1：将当前普通帧的描述子转化为BoW向量
 * Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配
 * Step 3: 将上一帧的位姿态作为当前帧位姿的初始值
 * Step 4: 通过优化3D-2D的重投影误差来获得位姿
 * Step 5：剔除优化后的匹配点中的外点
 * @return 如果匹配数超10，返回true
 * 
 */
bool Tracking::TrackReferenceKeyFrame()
{
    // Compute Bag of Words vector
    // Step 1：将当前帧的描述子转化为BoW向量
    mCurrentFrame.ComputeBoW();

    // We perform first an ORB matching with the reference keyframe
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.7,true);
    vector<MapPoint*> vpMapPointMatches;

    // Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配
    int nmatches = matcher.SearchByBoW(
        mpReferenceKF,          //参考关键帧
        mCurrentFrame,          //当前帧
        vpMapPointMatches);     //存储匹配关系

    // 匹配数目小于15，认为跟踪失败
    if(nmatches<15)
        return false;

    // Step 3:将上一帧的位姿态作为当前帧位姿的初始值
    mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些

    // Step 4:通过优化3D-2D的重投影误差来获得位姿
    Optimizer::PoseOptimization(&mCurrentFrame);

    // Discard outliers
    // Step 5：剔除优化后的匹配点中的外点
    //之所以在优化之后才剔除外点，是因为在优化的过程中就有了对这些外点的标记
    int nmatchesMap = 0;
    for(int i =0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            //如果对应到的某个特征点是外点
            if(mCurrentFrame.mvbOutlier[i])
            {
                //清除它在当前帧中存在过的痕迹
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];

                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                //匹配的内点计数++
                nmatchesMap++;
        }
    }
    // 跟踪成功的数目超过10才认为跟踪成功，否则跟踪失败
    return nmatchesMap>=10;
}

 

六、源码注释

通过该篇博客，首先了解了跟踪线程的总体流程，另外还对对跟踪线程中比较重要的函数→参考关键帧追踪TrackReferenceKeyFrame()进行了讲解。但是这里有一个比较重要的地方还没有进行详细的讲解。那就是其中的函数 Optimizer::PoseOptimization(&mCurrentFrame); 该部分内容暂且略过一下，后面在优化的篇章中，会进行详细的讲解。

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