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

源码：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帧及单目初始化是怎么处理的。