一文详解ORB-SLAM3中的位姿跟踪Tracking类实现

作者丨XingXin-C@知乎

来源丨https://zhuanlan.zhihu.com/p/349770246

编辑丨3D视觉工坊

Tracking.cc文件的主要内容有：

   ·在构造函数中读取配置文件中的参数。

   ·处理图像和imu数据，进行位姿跟踪，最主要的是track( )函数。

1、Tracking构造函数

{
    // Load camera parameters from settings file
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);//只读
    //读取相机参数
    //如果是ROS，DepthMapFactor应该设为1，即深度不进行缩放
    bool b_parse_cam = ParseCamParamFile(fSettings);
    if(!b_parse_cam)
    {
        std::cout << "*Error with the camera parameters in the config file*" << std::endl;
    }
    // Load ORB parameters
    //读取ORB特征提取的相关参数，该函数中还会
    //根据参数构造ORB提取器mpORBextractorLeft(左目)、mpORBextractorRight(右目)、mpIniORBextractor(初始化用)
    bool b_parse_orb = ParseORBParamFile(fSettings);
    if(!b_parse_orb)
    {
        std::cout << "*Error with the ORB parameters in the config file*" << std::endl;
    }
    initID = 0; lastID = 0;
    // Load IMU parameters
    bool b_parse_imu = true;
    if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO)
    {
        //读取imu参数，该函数中还会
        //根据参数构建预积分处理器mpImuPreintegratedFromLastKF
        b_parse_imu = ParseIMUParamFile(fSettings);
        if(!b_parse_imu)
        {
            std::cout << "*Error with the IMU parameters in the config file*" << std::endl;
        }
        mnFramesToResetIMU = mMaxFrames;
    }
    mbInitWith3KFs = false;
    mnNumDataset = 0;
    if(!b_parse_cam || !b_parse_orb || !b_parse_imu)
    {
        std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
        try
        {
            throw -1;
        }
        catch(exception &e)
        {

        }
    }
#ifdef SAVE_TIMES
    f_track_times.open("tracking_times.txt");
    f_track_times << "# ORB_Ext(ms), Stereo matching(ms), Preintegrate_IMU(ms), Pose pred(ms), LocalMap_track(ms), NewKF_dec(ms)" << endl;
    f_track_times << fixed ;
#endif
}

2、开始跟踪

以下是单目情况。双目和RGBD同理

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename)
{
    mImGray = im;
    //将彩色图像转为灰度图像
    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);
    }
    //构造Frame，同时完成特征点的提取、计算词袋等操作
    if (mSensor == System::MONOCULAR)
    {
        if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET ||(lastID - initID) < mMaxFrames)
            //还未初始化，用mpIniORBextractor提取器，提取的特征点是正常的5倍
            mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);
        else
            mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);
    }
    //imu模式的Frame构造函数
    else if(mSensor == System::IMU_MONOCULAR)
    {
        if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)  
        {
            mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
        }
        else
            mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
    }
    // t0存储未初始化时的第1帧时间戳
    if (mState==NO_IMAGES_YET)
        t0=timestamp;
    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    mCurrentFrame.mNameFile = filename;
    mCurrentFrame.mnDataset = mnNumDataset;
    lastID = mCurrentFrame.mnId;
    // 跟踪
    Track();//最主要的函数
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    double t_track = std::chrono::duration_cast