ORB-SLAM2 关于初始化流程和代码解释

SLAM系统最关键的部分是tracking和maping。除此之外还有让系统更加完整和完善的初始化、回环检测、可视化等模块。
初始化是SLAM系统在运行时是都要经历的一步，系统一定要有一个初始的传感器位姿和初始的地图。单目相机需要两帧具有一定视差的图像来完成这一任务。

为什么说ORB_SLAM2是相对完美的系统，因为它改进了初始化，在该系统中初始化是自动完成的。针对平面场景(planar scene)和非平面场景(non-planar scene)， ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。
整个初始化过程可以总结为六个步骤：

1.计算两帧图像的ORB特征点，并进行匹配；
2.在两个线程中以RANSAC策略并行的计算单应矩阵和基础矩阵；
3.根据评判标准在单应矩阵和基础矩阵之间选择一个模型；
4.根据选定的模型分解相机的旋转矩阵和平移向量，并对匹配的特征点进行三角化；
5.建立关键帧和地图点，进行完全BA(full BA)优化；
6.以参考帧下深度的中位数为基准建立基础尺度；

初始化的最基本任务就是建立坐标系，估计机器人的初始位姿，创建初始的地图。在查看源码之后了解到，初始化的过程是在System中，在构建了System对象之后，由System对象中的成员mpTracker完成。

在mono_tum.cc中，TrackMonocular（）是用来接收数据完成单目位姿估计的。输入为图像和时间戳。

SLAM.TrackMonocular(im,tframe);//用来接收输入，以便初始化

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
{
    if(mSensor!=MONOCULAR)//函数运行前先检查类别选择是否是单目
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;//如果不是单目，报错提醒
        exit(-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;
    }
    }
    //下面是关键，将图像和时间戳数据提供给轨迹跟踪器进行处理，轨迹追踪器将在下文进行介绍
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

    return Tcw;//返回位姿矩阵
}

可以看到，TrackMonocular（）函数接收图像im和时间戳tframe，返回图像与图像之间的位姿变换矩阵Tcw。如果省略中间的状态检查和下面的状态和成员变量的更新，完成位姿获取的是函数GrabImageMonocular（至少在上一段代码上是这样子显示的）。

这一部分的代码还是很重要的，因为在之后的tracking中，我们还会看到这一部分。
GrabImageMonocular（）需要的输入也是图像和时间戳，返回当前帧的位姿。该函数除了完成当前帧的获取之外还会完成对输入图像的通道判断，根据输入通道的不同调用不同的函数进行灰度图转换。转为灰度图之后，使用Frame（）把灰度图、时间戳、ORB特征提取函数等输入，获得当前帧。

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) {
            mImGray = im;
            // 可以省略不看，该步骤为彩色图转换为灰度图
            if(mImGray.channels()==3)//如果输入的是三通道彩色图
            {
            if(mbRGB)//如果是RGB通道顺序，采用RGBtogay
            
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
            else//BGR顺序有专门的转换函数
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
            }
            else if(mImGray.channels()==4)//如果是四通道
            {
            if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
            else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
            }
            if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)//如果当前是未初始化状态，下一步就是获取当前帧
                mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);//根据状态不同，选择不同的ORB特征提取器完成当前帧的获取。
            else
                mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
            Track();//这里用到了Track函数
            return mCurrentFrame.mTcw.clone();//返回当前帧的位姿
        }

看到这里有些许疑惑呀，这怎么一个函数套一个函数套来套去也没提到怎么获取位姿的。我们先来捋一捋TrackMonocular（）是最外层，该函数通过GrabImageMonocular（）函数获取当前图像的位姿（GrabImageMonocular就是第二层）。GrabImageMonocular的关键就是Track()。

void Tracking::Track() {
            if(mState==NO_IMAGES_YET)
                mState = NOT_INITIALIZED;
            mLastProcessedState=mState;

            unique_lock lock(mpMap->mMutexMapUpdate);
            if(mState==NOT_INITIALIZED) {
                if(mSensor==System::STEREO || mSensor==System::RGBD)
                    StereoInitialization();
                else
                    MonocularInitialization();
                mpFrameDrawer->Update(this);
                if(mState!=OK)//如果初始化不成功就继续直到成功为止
                   return;
            }

写的这里，还没有涉及最核心的三角测量的初始化原理，通过上述代码可以看到Track函数，这里的Track函数其实不算初始化部分，它是属于Tracking线程的。其中只有一部分是涉及初始化的。Track函数在查找资料的时候，发现一篇不错的文章。网址：
https://blog.csdn.net/qq_18834185/article/details/103081531

/*******************************************************************************
 *     函数属性：类Tracking的成员函数Track()   跟踪线程有四个状态分别是 没有图片,未初始化(第一张图片),线程完好, 追踪线程失败
 *     函数功能：检测当前系统状态是处于NO_IMAGES_YET，NOT_INITIALIZED，还是OK or LOST
 * 		1. 根据当前的跟踪状态来进行相机位姿的计算,处于NO_IMAGES_YET则将位姿状态转化为NOT_INITIALIZED
 * 								处在NOT_INITIALIZED则进行初始化程序,StereoInitialization() or MonocularInitialization()
 * 								处在LOST则进行重定位
 * 								处在OK状态,则通过TrackReferenceKeyFrame()和TrackWithMotionModel()进行相机位姿的计算
 * 								TrackReferenceKeyFrame()是根据上一帧的位姿为初始位姿进行优化,匹配的是关键帧和当前帧得到匹配地图点
 * 								TrackWithMotionModel()是根据上一帧的速度和上一帧的积来估算相机初始位姿,匹配的是上一帧和当前帧得到匹配地图点
 * 		2. 局部地图的跟踪,更新局部地图(局部地图点和局部关键帧)
 * 		3. 检测是否需要加入关键帧,将关键帧交给局部建图线程
 * 		4. 如果上述都顺利进行,则更新当前相机的最终位姿,存储完整的帧姿态信息以获取完整的相机轨迹
 * 
 *   			 另:	 仅线程追踪模式下只存在以前未开启仅线程追踪模式下的关键帧,不会添加新的关键帧,因此不会有新的地图点,仅仅追踪的是现有地图中的地图点
 *  	
 *     函数参数介绍：NULL
 *     备注：追踪线程的主要成员函数，实现视觉里程计
 * 			仅线程追踪模式下只存在以前未开启仅线程追踪模式下的关键帧,不会添加新的关键帧,因此不会有新的地图点,仅仅追踪的是现有地图中的地图点
 ******************************************************************************/

cv::Mat Rcw; // Current Camera Rotation
                cv::Mat tcw; // Current Camera Translation
                vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
                if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) {
                for(size_t i=0, iend=mvIniMatches.size(); i < iend;i++) {
                        if(mvIniMatches[i] >=0 && !vbTriangulated[i]) {
                            mvIniMatches[i]=-1;
                            nmatches--;
                        }
                    }