单目初始化（七）：使用基础矩阵恢复位姿

本次讲解的内容为单目初始化中使用基础矩阵恢复位姿的函数Initializer::ReconstructF
函数中所用到的数学原理请参考十四讲
首先是函数参数

bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers,            // 匹配点对的内点标记
                                            cv::Mat &H21,                 // 从参考帧到当前帧的单应矩阵
                                            cv::Mat &K,                   // 相机的内参数矩阵
                                            cv::Mat &R21,                 // 计算出来的相机旋转
                                            cv::Mat &t21,                 // 计算出来的相机平移
                                            vector<cv::Point3f> &vP3D,    // 世界坐标系下，三角化测量特征点对之后得到的特征点的空间坐标
                                            vector<bool> &vbTriangulated, // 特征点是否成功三角化的标记
                                            float minParallax,            // 对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
                                            int minTriangulated)          // 为了进行运动恢复，所需要的最少的三角化测量成功的点个数

在函数中主要进行了以下步骤

• 统计有效匹配点数量

  // Step 1 统计有效匹配点个数，并用 N 表示
  // vbMatchesInliers 中存储匹配点对是否是有效
  int N=0;
  for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
      if(vbMatchesInliers[i]) N++;	```
  

• 根据基础矩阵和相机的内参数矩阵计算本质矩阵

  // Step 2 根据基础矩阵和相机的内参数矩阵计算本质矩阵
  cv::Mat E21 = K.t()*F21*K;
  
  // 定义本质矩阵分解结果，形成四组解,分别是：
  // (R1, t) (R1, -t) (R2, t) (R2, -t)
  cv::Mat R1, R2, t;
  

• 从本质矩阵求解两个R解和两个t解，共四组解

      // Step 3 从本质矩阵求解两个R解和两个t解，共四组解
  // 不过由于两个t解互为相反数，因此这里先只获取一个
  // 虽然这个函数对t有归一化，但并没有决定单目整个SLAM过程的尺度. 
  // 因为 CreateInitialMapMonocular 函数对3D点深度会缩放，然后反过来对 t 有改变.
  //注意下文中的符号“'”表示矩阵的转置
  //                          |0 -1  0|
  // E = U Sigma V'   let W = |1  0  0|
  //                          |0  0  1|
  // 得到4个解 E = [R|t]
  // R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3
  DecomposeE(E21,R1,R2,t);  
  cv::Mat t1=t;
  cv::Mat t2=-t;
  

• 遍历所有解，计算对应的解可以成功恢复的3d点数量

      // Step 4 分别验证求解的4种R和t的组合，选出最佳组合
  // 原理：若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合
  // 实现：根据计算的解组合成为四种情况,并依次调用 Initializer::CheckRT() 进行检查,得到可以进行三角化测量的点的数目
  // 定义四组解分别在对同一匹配点集进行三角化测量之后的特征点空间坐标
  vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
  
  // 定义四组解分别对同一匹配点集的有效三角化结果，True or False
  vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
  
  // 定义四种解对应的比较大的特征点对视差角
  float parallax1,parallax2, parallax3, parallax4;
  
  // Step 4.1 使用同样的匹配点分别检查四组解，记录当前计算的3D点在摄像头前方且投影误差小于阈值的个数，记为有效3D点个数
  int nGood1 = CheckRT(R1,t1,							//当前组解
  					 mvKeys1,mvKeys2,				//参考帧和当前帧中的特征点
  					 mvMatches12, vbMatchesInliers,	//特征点的匹配关系和Inliers标记
  					 K, 							//相机的内参数矩阵
  					 vP3D1,							//存储三角化以后特征点的空间坐标
  					 4.0*mSigma2,					//三角化测量过程中允许的最大重投影误差
  					 vbTriangulated1,				//参考帧中被成功进行三角化测量的特征点的标记
  					 parallax1);					//认为某对特征点三角化测量有效的比较大的视差角
  int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
  int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
  int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
  

• 获取最大的三角化成功的点的数量

  int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
  
  // 重置变量，并在后面赋值为最佳R和T
  R21 = cv::Mat();
  t21 = cv::Mat();
  

• 获取最小的三角化成功的点的数量，并区分最优解的显著水平

  // Step 4.3 确定最小的可以三角化的点数 
  // 在0.9倍的内点数 和 指定值minTriangulated =50 中取最大的，也就是说至少50个
  int nMinGood = max(static_cast<int>(0.9*N), minTriangulated);
  
  // 统计四组解中重建的有效3D点个数 > 0.7 * maxGood 的解的数目
  // 如果有多个解同时满足该条件，认为结果太接近，nsimilar++，nsimilar>1就认为有问题了，后面返回false
  int nsimilar = 0;
  if(nGood1>0.7*maxGood)
      nsimilar++;
  if(nGood2>0.7*maxGood)
      nsimilar++;
  if(nGood3>0.7*maxGood)
      nsimilar++;
  if(nGood4>0.7*maxGood)
      nsimilar++;
  

• 判断是否有明显最优的解

  // Step 4.4 四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败
  // 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数（mMinGood），失败
  // 条件2: 如果存在两组及以上的解能三角化出 >0.7*maxGood的点，说明没有明显最优结果，失败
  if(maxGood<nMinGood || nsimilar>1)	
  {
      return false;
  }
  

• 选择最优解

  	    //  Step 4.5 选择最佳解记录结果
      // 条件1: 有效重建最多的3D点，即maxGood == nGoodx，也即是位于相机前方的3D点个数最多
      // 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax，角度越大3D点越稳定
  
      //看看最好的good点是在哪种解的条件下发生的
      if(maxGood==nGood1)
      {
  		//如果该种解下的parallax大于函数参数中给定的最小值
          if(parallax1>minParallax)
          {
              // 存储3D坐标
              vP3D = vP3D1;
  
  			// 获取特征点向量的三角化测量标记
              vbTriangulated = vbTriangulated1;
  
  			// 存储相机姿态
              R1.copyTo(R21);
              t1.copyTo(t21);
  			
              // 结束
              return true;
          }
      }else if(maxGood==nGood2)
      {
          if(parallax2>minParallax)
          {
              vP3D = vP3D2;
              vbTriangulated = vbTriangulated2;
  
              R2.copyTo(R21);
              t1.copyTo(t21);
              return true;
          }
      }else if(maxGood==nGood3)
      {
          if(parallax3>minParallax)
          {
              vP3D = vP3D3;
              vbTriangulated = vbTriangulated3;
  
              R1.copyTo(R21);
              t2.copyTo(t21);
              return true;
          }
      }else if(maxGood==nGood4)
      {
          if(parallax4>minParallax)
          {
              vP3D = vP3D4;
              vbTriangulated = vbTriangulated4;
  
              R2.copyTo(R21);
              t2.copyTo(t21);
              return true;
          }
      }
  
      // 如果有最优解但是不满足对应的parallax>minParallax，那么返回false表示求解失败
      return false;