单目初始化（八）：验证位姿的有效性并恢复地图点

本次主要讲解利用已经恢复出来的位姿来三角化地图点的函数CheckRT，相关的数学原理请参考十四讲
首先是函数参数

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t,                                    //恢复出来的位姿（当前帧）
                        const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2, //两帧的特征点
                        const vector<Match> &vMatches12,                                        //两帧特征点的匹配关系
                        vector<bool> &vbMatchesInliers,                                         //特征点对内点标记
                        const cv::Mat &K,                                                       //相机内参矩阵
                        vector<cv::Point3f> &vP3D,                                              //三角化测量之后的特征点的空间坐标
                        float th2,                                                              //重投影误差的阈值
                        vector<bool> &vbGood,                                                   //标记成功三角化点
                        float &parallax)                                                        //计算出来的比较大的视差角

函数中主要进行了以下工作

• 相关变量初始化

  //从相机内参数矩阵获取相机的校正参数
  const float fx = K.at<float>(0,0);
  const float fy = K.at<float>(1,1);
  const float cx = K.at<float>(0,2);
  const float cy = K.at<float>(1,2);
  
  //特征点是否是good点的标记，这里的特征点指的是参考帧中的特征点
  vbGood = vector<bool>(vKeys1.size(),false);
  //重设存储空间坐标的点的大小
  vP3D.resize(vKeys1.size());
  
  //存储计算出来的每对特征点的视差
  vector<float> vCosParallax;
  vCosParallax.reserve(vKeys1.size());
  

• 计算两帧相机的投影矩阵以及相机的光心坐标

      // Step 1：计算相机的投影矩阵  
  // 投影矩阵P是一个 3x4 的矩阵，可以将空间中的一个点投影到平面上，获得其平面坐标，这里均指的是齐次坐标。
  // 对于第一个相机是 P1=K*[I|0]
  
  // 以第一个相机的光心作为世界坐标系, 定义相机的投影矩阵
  cv::Mat P1(3,4,				//矩阵的大小是3x4
  		   CV_32F,			//数据类型是浮点数
  		   cv::Scalar(0));	//初始的数值是0
  //将整个K矩阵拷贝到P1矩阵的左侧3x3矩阵，因为 K*I = K
  K.copyTo(P1.rowRange(0,3).colRange(0,3));
  // 第一个相机的光心设置为世界坐标系下的原点
  cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
  
  // Camera 2 Projection Matrix K[R|t]
  // 计算第二个相机的投影矩阵 P2=K*[R|t]
  cv::Mat P2(3,4,CV_32F);
  R.copyTo(P2.rowRange(0,3).colRange(0,3));
  t.copyTo(P2.rowRange(0,3).col(3));
  //最终结果是K*[R|t]
  P2 = K*P2;
  // 第二个相机的光心在世界坐标系下的坐标
  cv::Mat O2 = -R.t()*t;
  
  //在遍历开始前，先将good点计数设置为0
  int nGood=0;
  

遍历所有的特征点对，并进行以下步骤

• 获取匹配点对的坐标，并进行三角化

      // Step 2 获取特征点对，调用Triangulate() 函数进行三角化，得到三角化测量之后的3D点坐标
      // kp1和kp2是匹配好的有效特征点
      const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
      const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
  	//存储三维点的的坐标
      cv::Mat p3dC1;
  
      // 利用三角法恢复三维点p3dC1
      Triangulate(kp1,kp2,	//特征点
  				P1,P2,		//投影矩阵
  				p3dC1);		//输出，三角化测量之后特征点的空间坐标		
  

  具体的三角化某对匹配点的函数如下

  	void Initializer::Triangulate(
      const cv::KeyPoint &kp1,    //特征点, in reference frame
      const cv::KeyPoint &kp2,    //特征点, in current frame
      const cv::Mat &P1,          //投影矩阵P1
      const cv::Mat &P2,          //投影矩阵P2
      cv::Mat &x3D)               //三维点
  {
      // 原理
      // Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
      // x' = P'X  x = PX
      // 它们都属于 x = aPX模型
      //                         |X|
      // |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|
      // |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|
      // |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|
      // 采用DLT的方法：x叉乘PX = 0
      // |yp2 -  p1|     |0|
      // |p0 -  xp2| X = |0|
      // |xp1 - yp0|     |0|
      // 两个点:
      // |yp2   -  p1  |     |0|
      // |p0    -  xp2 | X = |0| ===> AX = 0
      // |y'p2' -  p1' |     |0|
      // |p0'   - x'p2'|     |0|
      // 变成程序中的形式：
      // |xp2  - p0 |     |0|
      // |yp2  - p1 | X = |0| ===> AX = 0
      // |x'p2'- p0'|     |0|
      // |y'p2'- p1'|     |0|
      // 然后就组成了一个四元一次正定方程组，SVD求解，右奇异矩阵的最后一行就是最终的解.
  
  	//这个就是上面注释中的矩阵A
      cv::Mat A(4,4,CV_32F);
  
  	//构造参数矩阵A
      A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
      A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
      A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
      A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
  
  	//奇异值分解的结果
      cv::Mat u,w,vt;
  	//对系数矩阵A进行奇异值分解
      cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
  	//根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定
  	//别忘了我们更习惯用列向量来表示一个点的空间坐标
      x3D = vt.row(3).t();
  	//为了符合其次坐标的形式，使最后一维为1
      x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
  }
  

• 检查地图点的坐标是否为有限值

  	// Step 3 第一关：检查三角化的三维点坐标是否合法（非无穷值）
      // 只要三角测量的结果中有一个是无穷大的就说明三角化失败，跳过对当前点的处理，进行下一对特征点的遍历 
      if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
      {
  		//其实这里就算是不这样写也没问题，因为默认的匹配点对就不是good点
          vbGood[vMatches12[i].first]=false;
  		//继续对下一对匹配点的处理
          continue;
      }
  

• 检查视差和地图点是否有正深度

      // Step 4 第二关：通过三维点深度值正负、两相机光心视差角大小来检查是否合法 
  
      //得到向量PO1
      cv::Mat normal1 = p3dC1 - O1;
  	//求取模长，其实就是距离
      float dist1 = cv::norm(normal1);
  
  	//同理构造向量PO2
      cv::Mat normal2 = p3dC1 - O2;
  	//求模长
      float dist2 = cv::norm(normal2);
  
  	//根据公式：a.*b=|a||b|cos_theta 可以推导出来下面的式子
      float cosParallax = normal1.dot(normal2)/(dist1*dist2);
  
      // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
      // 如果深度值为负值，为非法三维点跳过该匹配点对
      // ?视差比较小时，重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗？
      // ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998 
      // !可能导致初始化不稳定
      if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
          continue;
  
      // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
      // 讲空间点p3dC1变换到第2个相机坐标系下变为p3dC2
      cv::Mat p3dC2 = R*p3dC1+t;	
  	//判断过程和上面的相同
      if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
          continue;
  
  

  注意，作者这里视差判断写反了，如果是小于号的就是视差大于阈值就抛弃，这明显是不符合常理的

• 检查地图点在两帧上的重投影误差

      // Step 5 第三关：计算空间点在参考帧和当前帧上的重投影误差，如果大于阈值则舍弃
      // Check reprojection error in first image
      // 计算3D点在第一个图像上的投影误差
  	//投影到参考帧图像上的点的坐标x,y
      float im1x, im1y;
  	//这个使能空间点的z坐标的倒数
      float invZ1 = 1.0/p3dC1.at<float>(2);
  	//投影到参考帧图像上。因为参考帧下的相机坐标系和世界坐标系重合，因此这里就直接进行投影就可以了
      im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
      im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
  
  	//参考帧上的重投影误差，这个的确就是按照定义来的
      float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
  
      // 重投影误差太大，跳过淘汰
      if(squareError1>th2)
          continue;
  
      // Check reprojection error in second image
      // 计算3D点在第二个图像上的投影误差，计算过程和第一个图像类似
      float im2x, im2y;
      float invZ2 = 1.0/p3dC2.at<float>(2);
  
      im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
      im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
  
  	// 计算重投影误差
      float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
  
      // 重投影误差太大，跳过淘汰
      if(squareError2>th2)
          continue;
  

• 记录3d点和视差

      // Step 6 统计经过检验的3D点个数，记录3D点视差角 
      // 如果运行到这里就说明当前遍历的这个特征点对靠谱，经过了重重检验，说明是一个合格的点，称之为good点 
      vCosParallax.push_back(cosParallax);
  	//存储这个三角化测量后的3D点在世界坐标系下的坐标
      vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
  	//good点计数++
      nGood++;
  
  	//判断视差角，只有视差角稍稍大一丢丢的才会给打good点标记
  	//? bug 我觉得这个写的位置不太对。你的good点计数都++了然后才判断，不是会让good点标志和good点计数不一样吗
      if(cosParallax<0.99998)
          vbGood[vMatches12[i].first]=true;
  

  注意，这里最后一句判断的位置是有问题的，会导致视差不够的点也被认为三角化成功了

• 获取较大的视差值

  // Step 7 得到3D点中较大的视差角，并且转换成为角度制表示
  if(nGood>0)
  {
      // 从小到大排序，注意vCosParallax值越大，视差越小
      sort(vCosParallax.begin(),vCosParallax.end());
  
      // !排序后并没有取最小的视差角，而是取一个较小的视差角
  	// 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)
  	// 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 
      size_t idx = min(50,int(vCosParallax.size()-1));
  	//将这个选中的角弧度制转换为角度制
      parallax = acos(vCosParallax[idx])*180/CV_PI;
  }
  else
  	//如果没有good点那么这个就直接设置为0了
      parallax=0;
  
  //返回good点计数
  return nGood;