ORB-SLAM2源码逐行解析系列（七）：ORB-SLAM2中的单目初始化

1. 为什么需要初始化

 初始化是为了获取第一帧特征点对应的三维点，以此作为地图点来实现跟踪。在ORB-SLAM2中初始化和使用的传感器类型有关，对于RGB-D相机，它可以直接输出RGB图像和对应的深度图像，所以每个像素点对应的深度值是确定的，也就是说，在第一帧提取了特征点后，特征点对应的三维点在空间的绝对坐标可以通过内参计算出来。对于双目相机来说，也可以通过第一帧左右目图像立体匹配来得到特征点对应的三维点在空间中的绝对坐标。而对于单目相机，其初始化相对复杂，需要运行一段时间才能成功初始化。所以理论来说，双目相机、RGB-D相机在第一帧就可以完成初始化。

2. 单目初始化

（1）单目初始化大致思路

• 记录当前帧和参考帧（第1帧）之间的特征匹配关系
• 在特征匹配点对中随机选择8对匹配点作为一组
• 用这8对点分别计算基础矩阵F和单应矩阵H，并得到得分
• 计算得分比例，据此判断选取基础矩阵还是单应矩阵求位姿和三角化

（2）大致流程

• 在没有创建初始化器mpInitializer的情况下，按照如下方式创建初始化器mpInitializer：
  • 必须要求当前帧的特征点数大于100
  • 保存当前帧中的特征点
  • 根据当前帧new一个初始化器mpInitializer
  • 将特征点匹配关系设置为-1
• 在已创建初始化器mpInitializer的情况下，按照如下方式进行初始化
  • 同样要求当前帧的特征点个数必须大于100，否则删除初始化器mpInitializer，并返回
  • 构造特征点匹配器matcher，在初始化帧mInitialFrame与当前帧mCurrentFrame中查找匹配的特征点对，获取两帧的匹配关系mvIniMatches和匹配的特征点数nmatches
  • 验证匹配结果，如果初始化的两帧之间的匹配点太少(小于100)，则重新初始化
  • mpInitializer->Initialize：通过H模型或F模型使用初始化器进行单目初始化，得到两帧间相对运动、初始MapPoints
  • 初始化成功后，删除那些无法进行三角化的匹配点
  • 假设初始帧为世界坐标系的情况下，根据初始化得到的两帧相对位姿设置当前帧位姿
  • CreateInitialMapMonocular：将初始化得到的3D点包装成MapPoints

（3）代码实现

/*
 * @brief 单目的地图初始化
 *
 * 并行地计算基础矩阵F和单应性矩阵H，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
 * 得到初始两帧的匹配、相对运动、初始MapPoints
 * 
 * Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
 * Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
 * Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
 * Step 4：如果初始化的两帧之间的匹配点太少，重新初始化
 * Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
 * Step 6：删除那些无法进行三角化的匹配点
 * Step 7：将三角化得到的3D点包装成MapPoints
 */
void Tracking::MonocularInitialization()
{
   
    // Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
    if(!mpInitializer)  // 创建mpInitializer
    {
   
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {
   
            // 初始化需要两帧，分别是初始帧mInitialFrame和当前帧mCurrentFrame
            // 首先将当前帧mCurrentFrame赋给初始化帧mInitialFrame，以作为参考帧
            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;  
            // new之前判断一下是否已存在
            if(mpInitializer)
                delete mpInitializer;
            // 由当前帧构造初始化器 sigma:1.0 iterations:200    
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
            // 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

            return;
        }
    }
    // Step 2: 进行单目初始化
    else  
    {
   
        // Step 2.1: 如果当前帧特征点数太少(不超过100)，则删掉初始化器mpInitializer，直接返回
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
   
            delete mpInitializer;  
            mpInitializer = static_cast<Initializer*>(NULL);  
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1); 
            return;  
        }
        
        // Step 2.2: 构造特征匹配器matcher
        ORBmatcher matcher(
            0.9,        // 最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
            true);      // 检查特征点的方向
        
        // Step 2.3: 在mInitialFrame与mCurrentFrame中寻找匹配的特征点对
        // 返回的nmatches为有效的特征匹配点对数
        int nmatches = matcher.SearchForInitialization(
            mInitialFrame,mCurrentFrame,    // 初始化时的参考帧和当前帧
            mvbPrevMatched,                 // 在初始化参考帧中提取得到的特征点
            mvIniMatches,                   // 保存匹配关系 [第一帧特征点ID -- 第二帧特征点ID]
            100);                           // 搜索窗口大小
        
        // Step 2.4: 验证匹配结果，如果初始化的两帧之间的匹配点太少(小于100)，则重新初始化
        if(nmatches<100)
        {
   
            delete mpInitializer;  
            mpInitializer = static_cast<Initializer*>(NULL);  
            return;
        }
        
        cv::Mat Rcw;  // Current Camera Rotation
        cv::Mat tcw;  // Current Camera Translation
        vector<