ORB SLAM2学习笔记之mono_kitti（四）

探索Track函数之MonocularInitialization（单目初始化）

Tracking.cpp里面的Track()函数可以说是 Tracking 线程最主体部分了，来看看调用Track()函数的过程：

Tracking线程中初始化函数用的是MonocularInitialization()，在第一次进入这个函数之前，初始化了mpInitializer（单目初始器）赋值为null，所以一开始会进入下面的if语句：

void Tracking::MonocularInitialization()
{
   

    //(第一次进入函数时)初始化tracking时赋了null
    //(第二次进入函数时)第一次新建了mpInitializer，所以执行else
    if(!mpInitializer)
    {
   
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {
   

            //初始化mono的时候，初始帧和上一帧都是当前帧赋值
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);

            // mvbPrevMatched最大的情况就是所有特征点都被跟踪上
            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;

            // 这里实例化Initialize类的一个对象指针，初始化的值为选取的当前帧和测量误差1.0，RANSAC迭代次数200
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);

            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);  //先都填上-1 现在还没匹配呢

            return;
        }
    }

第二次进入这个函数的时候已经创建了一个mpInitializer，所以会进入else语句：

    else
    {
   
        // Try to initialize
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
   
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }

        // Find correspondences
        ORBmatcher matcher(0.9,true);

        // mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
        // mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引
        // mvbPrevMatched，mvIniMatches获得更新
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

        // Check if there are enough correspondences
        if(nmatches<100)
        {
   
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            return;
        }

        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)

        //如果初始化成功，intializer得到Rcw, tcw
        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--;
                }
            }

            // Set Frame Poses  初始帧也就是第一帧为世界坐标系，所以无需旋转平移
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);

            //Tcw赋值给mCurrentFrame
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose