ORB-SLAM3源码阅读笔记（5）-视觉初始化

视觉初始化分为单目/单目imu、双目/双目imu/RGBD两种。单目初始化要处理2D-2D匹配问题，比双目初始化要复杂一些。视觉初始化的入口在//track.cc文件中，完成单目初始化功能的代码在TwoViewReconstruction.cc中，ORB-SLAM3还加入了imu，imu相关的初始化在LocalMapping.cc线程主函数中完成。

1、Tracking::MonocularInitialization()

void Tracking::MonocularInitialization()//单目和单目imu初始化函数
{
    if(!mpInitializer)//判断是否创建了初始化求解器，如果没有，说明是第一帧，如果有，说明已经是第二帧
    {   //ORB-SLAM3中mpInitializer感觉只是一个标志，真正的初始化在TwoViewReconstruction类中
        //不像ORB-SLAM2是在Initializer类中。
        if(mCurrentFrame.mvKeys.size()>100)//特征点必须大于100
        {
            mInitialFrame = Frame(mCurrentFrame);//第一帧设为初始化帧
            mLastFrame = Frame(mCurrentFrame);//mLastFrame也是它
            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)
            {  //如果是imu模式，新建预积分求解器
                if(mpImuPreintegratedFromLastKF)
                {
                    delete mpImuPreintegratedFromLastKF;
                }
                mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
                mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
            }
            return;
        }
    }
    else//第二帧
    {  //如果当前帧的关键点小于100，或者imu模式下相对于上一帧超过1s,则一切重新来过
        if (((int)mCurrentFrame.mvKeys.size()<=100)||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0)))
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
        // Find correspondences 找匹配点
        ORBmatcher matcher(0.9,true);
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
        // Check if there are enough correspondences
        if(nmatches<100)//匹配点数小于100，则一切重新来
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
        if(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,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);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);
            CreateInitialMapMonocular();//根据三角化的结果，创建初始地图点，并做一些必要的计算
        }
    }
}

2、Tracking::StereoInitialization()

双目有深度信息，所以第一帧(左右目)可以直接产生3D地图点，然后下一帧图像可以直接进行3D-2D匹配进行跟踪。所以双目初始化过程相对单目比较简单。
算法流程：

1. 提取的特征点必须大于500
2. 如果是Stereo_IMU，检查一下是否有预积分，如果没有说明没有Imu测量。再检查一下当前帧加速度和上一帧加速度的差值是否大于0.5，如果小于说明加速度变化不够，也不行。
3. 直接设置初始化帧(当前帧)的位姿为单位阵
4. 将当前帧构造成关键帧，加入地图集Atlas中，然后生成初始地图点，添加地图点和关键帧之间的连接关系，计算地图点最好的描述子ComputeDistinctiveDescriptors()，更新平均观测方向和深度范围UpdateNormalAndDepth()
5. 最后，将关键帧加入LocalMapping线程，然后做一下标记工作。

3、单目初始化TwoViewReconstruction::Reconstruct( )

单目初始化通过H矩阵恢复R、t时采用这篇论文中的算法：

下面是ORB-SLAM3单目初始化的算法流程图。