ORB-SLAM2从理论到代码实现（六）：Tracking程序详解（上）

原创

已于 2024-09-05 15:53:22 修改 · 2.4k 阅读

47 ·

CC 4.0 BY-SA版权

文章标签：

#人工智能 #计算机视觉 #slam

于 2024-05-23 09:29:54 首次发布

1. Tracking框架

Tracking线程流程框图：

各流程对应的主要函数

2. Tracking整体流程图

上面这张图把Tracking.cc讲的特别明白。

tracking线程在获取图像数据后，会传给函数GrabImageStereo、GrabImageRGBD或GrabImageMonocular进行预处理，这里以GrabImageMonocular为例。

GrabImageMonocular(const cv::Mat &im, const double &timestamp)

函数功能

1. 将图像转为mImGray并初始化mCurrentFrame；

2. 进行tracking过程，输出世界坐标系到该帧相机坐标系的变换矩阵

输入图像

timestamp

时间戳

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
 
{
    mImGray = im;//读取图像
    // 步骤1：将RGB或RGBA图像转为灰度图像
    if(mImGray.channels()==3)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
    }
    // 步骤2：构造Frame
    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    // 步骤3：跟踪
    Track();
    return mCurrentFrame.mTcw.clone();
}

数据，流到Track()，由于代码超长，分几段粘贴注释。

void Tracking::Track()

步骤

1. 判断tracking状态：如果是未初始化（NOT_INITIALIZED），则对单目和非单目分别执行MonocularInitialization()、StereoInitialization()进行初始化，并更新地图视图。

2.对于初始化成功的，接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking判断）

（1）只进行跟踪不建图

（2）同时跟踪和建图：

初始化之后ORB-SLAM有三种跟踪模型可供选择

a.TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。

b.TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点——将上一帧的位姿作为当前帧的初始值——通过优化3D-2D的重投影误差来获得位姿。

c.Relocalization()；重定位模型：计算当前帧的BoW——检测满足重定位条件的候选帧——通过BoW搜索当前帧与候选帧的匹配点——大于15个点就进行PnP位姿估计——优化。

这三个模型的选择方法：

首先假设相机恒速（即Rt和上一帧相同），然后计算匹配点数（如果匹配足够多则认为跟踪成功），如果匹配点数目较少，说明恒速模型失效，则选择参考帧模型（即特征匹配，PnP求解），如果参考帧模型同样不能进行跟踪，说明两帧键没有相关性，这时需要进行重定位，即和已经产生的关键帧中进行匹配（看看是否到了之前已经到过的地方）确定相机位姿，如果重定位仍然不能成功，则说明跟踪