ORB-SLAM3 Tracking类源码分析（二）

最新推荐文章于 2024-05-23 09:29:54 发布

qq_53377744

最新推荐文章于 2024-05-23 09:29:54 发布

阅读量286

点赞数

文章标签：目标跟踪

本文链接：https://blog.csdn.net/qq_53377744/article/details/121613414

版权

2021SC@SDUSC

2、开始跟踪（tracking）
本次仅介绍单目情况下的跟踪。

第一步：将图像转换为灰度图像

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, 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（帧），同时完成一系列相关操作如：特征点的提取、词袋的计算；并初始化一个时间戳

若还未初始化就利用mpIniORBextractor提取器，提取的特征点是正常的5倍

第三步：调用跟踪函数（Track（））并保存完整的耗时，最后返回当前帧（Frame）的位置

 //构造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<std::chrono::duration<double,std::milli> >(t1 - t0).count();

//保存耗时
#ifdef SAVE_TIMES
    f_track_times << mCurrentFrame.mTimeORB_Ext << ",";
    f_track_times << mCurrentFrame.mTimeStereoMatch << ",";
    f_track_times << mTime_PreIntIMU << ",";
    f_track_times << mTime_PosePred << ",";
    f_track_times << mTime_LocalMapTrack << ",";
    f_track_times << mTime_NewKF_Dec << ",";
    f_track_times << t_track << endl;
#endif
    //最后返回当前帧的位置
    return mCurrentFrame.mTcw.clone();
}