单目初始化（一）：框架

本次主要讲解ORBSLAM2中单目初始化函数Tracking::MonocularInitialization的框架
单目初始化函数中主要进行以下工作：

• 设置参考帧，由于单目初始化需要两帧，所以需要将第一帧设置为参考帧，并初始化一个初始化器

      // Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
  if(!mpInitializer)
  {
      // 设置参考帧
      // 单目初始帧的特征点数必须大于100
      if(mCurrentFrame.mvKeys.size()>100)
      {
          // 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
          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;
  
          // 由当前帧构造初始器 sigma:1.0 iterations:200
          mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
  
          // 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id
          fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
  
          return;
      }
  }
  

  其中的初始化器构造函数如下

  Initializer::Initializer(const Frame &ReferenceFrame, // 参考帧
                          float sigma,                  // 测量误差
                          int iterations)               // RANSAC迭代次数
  {
  	//从参考帧中获取相机的内参数矩阵
      mK = ReferenceFrame.mK.clone();
  	
  	// 从参考帧中获取去畸变后的特征点
      mvKeys1 = ReferenceFrame.mvKeysUn;
  	
  	//获取估计误差
      mSigma = sigma;
      mSigma2 = sigma*sigma;
  
  	//最大迭代次数
      mMaxIterations = iterations;
  }
  

• 判断当前帧的特征点数目，如果特征点数目太少，则删除初始化器

      // Step 2 如果当前帧特征点数太少（不超过100），则重新构造初始器
      // NOTICE 只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
      if((int)mCurrentFrame.mvKeys.size()<=100)
      {
          delete mpInitializer;
          mpInitializer = static_cast<Initializer*>(NULL);
          fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
          return;
      }
  

• 构造一个特征点匹配器

      // Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
      ORBmatcher matcher(
          0.9,        //最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
          true);      //检查特征点的方向
  

• 进行特征匹配

      // Step 4 进行特征匹配
      // 对 mInitialFrame,mCurrentFrame 进行特征点匹配
      // mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标
      // mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
      int nmatches = matcher.SearchForInitialization(
          mInitialFrame,mCurrentFrame,    //初始化时的参考帧和当前帧
          mvbPrevMatched,                 //在初始化参考帧中提取得到的特征点
          mvIniMatches,                   //保存匹配关系
          100);                           //搜索窗口大小
  

• 验证匹配结果，如果找到的匹配特征点对太少，则进行重新初始化

      // Step 5 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
      if(nmatches<100)
      {
          delete mpInitializer;
          mpInitializer = static_cast<Initializer*>(NULL);
          return;
      }
  

• 通过H矩阵或者F矩阵进行初始化

      // Step 6 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
      if(mpInitializer->Initialize(
          mCurrentFrame,      //当前帧
          mvIniMatches,       //当前帧和参考帧的特征点的匹配关系
          Rcw, tcw,           //初始化得到的相机的位姿
          mvIniP3D,           //进行三角化得到的空间点集合
          vbTriangulated))    //以及对应于mvIniMatches来讲,其中哪些点被三角化了
  

• 初始化成功后，删除无法进行三角化的特征点

          // Step 7 初始化成功后，删除那些无法进行三角化的匹配点
          for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
          {
              if(mvIniMatches[i]>=0 && !vbTriangulated[i])
              {
                  mvIniMatches[i]=-1;
                  nmatches--;
              }
          }
  

• 设置初始化的参考帧和当前帧的位姿，参考帧作为世界坐标系

          // Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
          mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
          // 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到相机坐标系的变换矩阵
          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);
  

• 创建初始化的地图点

          // Step 8 创建初始化地图点MapPoints
          // Initialize函数会得到mvIniP3D，
          // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
          // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
          CreateInitialMapMonocular();