SLAM ORB-SLAM2（11）单目初始化

    /** * @brief 轨迹跟踪任务函数
     */
    void Tracking::Track()
    {
    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }
    
    mLastProcessedState=mState;
    
    // Get Map Mutex -> Map cannot be changed
    /* 地图对象上锁 */
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
    
    if(mState==NOT_INITIALIZED)
    {
        /* 初始化轨迹跟踪任务 */
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();
    
        mpFrameDrawer->Update(this);
    
        if(mState!=OK)
            return;
    }
    ...

    /** * @brief 单目初始化函数
     */
    void Tracking::MonocularInitialization()
    {
    
    if (!mpInitializer)
    {
        // Set Reference Frame
        /* 当图像的特征点数量超过了100个，才以当前帧作为参考帧，进行初始化操作 */
        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);
    
            /* 初始化时前两帧的匹配关系（匹配点的id），-1表示目前没有任何匹配 */
            fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
    
            return;
        }
    }

    /* 已创建初始化器时，判断新图像特征点数目数量是否超过100，
       即只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程 */
    if ((int)mCurrentFrame.mvKeys.size() <= 100)
    {
    /* 销毁 mpInitializer 重新记录初始帧 */
    delete mpInitializer;
    mpInitializer = static_cast<Initializer *>(NULL);
    fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
    return;
    }

    /* 构建了一个特征点匹配器 */
    ORBmatcher matcher(0.9, true);
    int nmatches = matcher.SearchForInitialization(mInitialFrame, mCurrentFrame, mvbPrevMatched, mvIniMatches, 100);
    
    /* 如果匹配点数量少于 100 个，认为当前帧和初始帧不适合初始化 */
    if (nmatches < 100)
    {
    /* 销毁 mpInitializer 重新记录初始帧 */
    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)
    
    /* 计算基础矩阵和单应矩阵，选取两帧之间最佳的相对姿态，并进行三角化得到初始地图点 */
    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--;
        }
    }
    ...
    }

    /* 建立世界坐标系，设定初始帧的齐次矩阵为单位阵 */
    mInitialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
    
    /* 用刚刚估计的旋转矩阵和平移向量构建当前帧的齐次矩阵 */
    cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F);
    Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
    tcw.copyTo(Tcw.rowRange(0, 3).col(3));
    mCurrentFrame.SetPose(Tcw);
    
    /* 创建初始化地图点 */
    CreateInitialMapMonocular();