首页 > 其他分享> 文章详细

从另一个角度看ORB-SLAM3——第0帧

2021-06-18 12:33:20 阅读：293 来源： 互联网

标签：mpTracker 初始化 false mCurrentFrame SLAM3 IMU 角度看 ORB size

源码：https://github.com/UZ-SLAMLab/ORB_SLAM3

上电时系统处于NO_IMAGES_YET状态，如果这时第一张图片，即第0帧，被系统读取，它会经过哪些函数，会被系统如何处理呢？

1. 主函数

以mono_inertial_euroc为例，main()在mono_inertial_euroc.cc中。

1.1

系统首先读取图片路径和时间戳，IMU测量值、参数和时间戳。

LoadImages(pathCam0, pathTimeStamps, vstrImageFilenames[seq], vTimestampsCam[seq]);
LoadIMU(pathImu, vTimestampsImu[seq], vAcc[seq], vGyro[seq]);

1.2

用预训练好的词袋、yaml配置文件初始化并启动系统的4个线程，包括跟踪mpTracker、建图mpLocalMapper、回环mpLoopCloser和可视化mpViewer。

ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);

之后的工作就很直接了，系统需要将每一帧im、时间戳tframe和对应的IMU测量值vImuMeas传递到跟踪线程。函数入口是：

SLAM.TrackMonocular(im,tframe,vImuMeas);

我们只考虑第0帧，来看看这个函数对第0帧做了什么处理。

2. 跟踪

2.1

首先检查是否有纯定位模式和系统重置被要求使能。

// Check mode change
unique_lock<mutex> lock(mMutexMode);
if(mbActivateLocalizationMode)
{
    mpLocalMapper->RequestStop();

    // Wait until Local Mapping has effectively stopped
    while(!mpLocalMapper->isStopped())
    {
        usleep(1000);
    }

    mpTracker->InformOnlyTracking(true);
    mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
{
    mpTracker->InformOnlyTracking(false);
    mpLocalMapper->Release();
    mbDeactivateLocalizationMode = false;
}

// Check reset
unique_lock<mutex> lock(mMutexReset);
if(mbReset)
{
    mpTracker->Reset();
    mbReset = false;
    mbResetActiveMap = false;
}
else if(mbResetActiveMap)
{
    cout << "SYSTEM-> Reseting active map in monocular case" << endl;
    mpTracker->ResetActiveMap();
    mbResetActiveMap = false;
}

2.2

传递进来的IMU测量值被存储在队列mlQueueImuData中。

for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
    mpTracker->GrabImuData(vImuMeas[i_imu]);

之后跟踪传递进来的图像，计算它的位姿Tcw。因为这里处理的是第0帧，Tcw应该是一个4维的单位矩阵。

cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);

下面看看GrabImageMonocular()的实现细节，和第0帧处理无关的就直接略去。

3. GrabImageMonocular()

3.1

将第0帧转成灰度图。

cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY);

此时系统状态mState为NO_IMAGES_YET，用灰度图创建当前帧mCurrentFrame。

mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);

记录下Id后，Track()开始跟踪当前帧。Track()和系统所使用的传感器无关，无论是单目、双目还是IMU方案，都是执行该函数来估计每一帧的位姿。

lastID = mCurrentFrame.mnId;
Track();

3.2 Track()

Track()对第0帧的处理很简单。首先改变系统状态为NOT_INITIALIZED，为读取下一帧进行初始化做准备。

if(mState==NO_IMAGES_YET)
    mState = NOT_INITIALIZED;

调用PreintegrateIMU()计算帧帧之间的预积分。因为第0帧没有对应的IMU测量值传递进来，不满足条件，mbImuPreintegrated置1，然后被返回。

if(mlQueueImuData.size() == 0)
{
    Verbose::PrintMess("Not IMU data in mlQueueImuData!!", Verbose::VERBOSITY_NORMAL);
    mCurrentFrame.setIntegrated();
    return;
}

之后根据系统传感器类型进入MonocularInitialization()执行单目初始化。

3.3 MonocularInitialization()

在跟踪线程初始化时，mpInitializer指针为空，条件判断成功，进入主体设置用于单目初始化的参考帧。

if(!mpInitializer)
{
    // Set Reference Frame
    if(mCurrentFrame.mvKeys.size()>100)
    {

        mInitialFrame = Frame(mCurrentFrame);
        mLastFrame = Frame(mCurrentFrame);
        mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
        for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
            mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;

        if(mpInitializer)
            delete mpInitializer;

        mpInitializer =  new Initializer(mCurrentFrame,1.0,200);

        fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

        if (mSensor == System::IMU_MONOCULAR)
        {
            if(mpImuPreintegratedFromLastKF)
            {
                delete mpImuPreintegratedFromLastKF;
            }
            mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
            mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;

        }
        return;
    }
}

mCurrentFrame.mvKeys.size()>100判断当前帧提取的特征点数是否大于100。如果满足条件，设置初始帧mInitialFrame和最新帧mLastFrame均为当前帧，并将去畸变后的特征点坐标传给mvbPrevMatched。delete mpInitializer这一步我没看明白，外层条件语句已经判断了mpInitializer为空指针，根本无法进入这一层的条件判断，我把这两行注释后，程序运行似乎也没出问题(?)。现在可以用当前帧重新设置mpInitializer了，fill()函数初始化mvIniMatches元素为-1，表示未进行特征点匹配或匹配未成功。根据ORB-SLAM3的论文，系统会先跑2秒的纯视觉，用来初始化IMU，包括bias、尺度、重力方向和前几帧的速度，系统从第0帧开始累计预积分，使用yaml配置文件给出的IMU参数设置第0帧的预积分，并给定bias初值为0。设置完成后，返回到上一层函数Track()。

3.4

最后一步，判断初始化是否成功。到这里我们只处理了第0帧，因此系统状态仍旧是NOT_INITIALIZED。进入条件语句主体，设置最新帧为当前帧。当然这一步对于第0帧是多余的，因为在MonocularInitialization()中，已经设置过一次最新帧了。

if(mState!=OK) // If rightly initialized, mState=OK
{
    mLastFrame = Frame(mCurrentFrame);
    return;
}

对第0帧的处理就全结束了，下次看看对第1帧及单目初始化是怎么处理的。

标签：mpTracker,初始化,false,mCurrentFrame,SLAM3,IMU,角度看,ORB,size
来源： https://www.cnblogs.com/yiqian/p/14899026.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9