ORB-SLAM2代码阅读笔记：PnPSolver

• PnPsolver::PnPsolver(const Frame &F, const vector &vpMapPointMatches):
  pws(0), us(0), alphas(0), pcs(0), //这里的四个变量都是指针啊,直接这样子写的原因可以参考函数 set_maximum_number_of_correspondences()
  maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),
  mnIterations(0), mnBestInliers(0), N(0)
  构造函数取出2维特征点，世界坐标系下的3D点，只记录坐标，特征点在哪一层取出来的，调用SetRansacParameters()，设置默认的RANSAC参数,这个和Sim3Solver中的操作是相同的
• void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
  设置RANSAC迭代的参数：
  • 获取给定的参数
  • 计算理论内点数
  • 调整内点数/总体数比例
  • 计算理论迭代次数
  • 计算不同图层上的特征点在进行内点检验时使用的不同判断误差阈值
• double PnPsolver::compute_pose(double R[3][3], double t[3])
  根据类成员变量中给出的匹配点,计算相机位姿
  • 获取EPnP算法中的四个控制点
  • 计算世界坐标系下每个3D点用4个控制点线性表达时的系数alphas
  • 构造M矩阵，EPnP原始论文中公式(3)(4)-->(5)(6)(7); 矩阵的大小为 2n*12 ,n 为使用的匹配点的对数
  • 求解Mx = 0,x是控制点在相机坐标系下组成的向量
  • ,是M特征向量，需要分N=1、2、3、4计算
  • 根据匹配点在相机坐标系下的坐标以及在世界坐标系下的坐标，然后根据ICP算法求得相机位姿。
• void PnPsolver::choose_control_points(void)
  从给定的匹配点中计算出四个控制点
  • 第一个控制点为所有3D点几何中心

    // 遍历每个匹配点中的3D点，然后对每个坐标轴加和
  for(int i = 0; i < number_of_correspondences; i++)
    for(int j = 0; j < 3; j++)
      cws[0][j] += pws[3 * i + j];
  // 再对每个轴上取均值
  for(int j = 0; j < 3; j++)
    cws[0][j] /= number_of_correspondences;

- 其它三个控制点，C1, C2, C3通过PCA分解得到

// 将所有的3D参考点写成矩阵，(number_of_correspondences * ３)的矩阵
  CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);

  double pw0tpw0[3 * 3], dc[3], uct[3 * 3];         // 下面变量的数据区
  CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);     // 如变量名所示.其实这里使用cv::Mat格式主要是为了进行SVD分解
  CvMat DC      = cvMat(3, 1, CV_64F, dc);          // 分解上面矩阵得到的奇异值组成的矩阵
  CvMat UCt     = cvMat(3, 3, CV_64F, uct);         // 分解上面矩阵得到的左奇异矩阵

  // step 2.1：将存在pws中的参考3D点减去第一个控制点的坐标（相当于把第一个控制点作为原点）, 并存入PW0
  for(int i = 0; i < number_of_correspondences; i++)
    for(int j = 0; j < 3; j++)
      PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];

  // step 2.2：利用SVD分解P'P可以获得P的主分量
  cvMulTransposed(PW0, &PW0tPW0, 1);
  cvSVD(&PW0tPW0,                         // A
        &DC,                              // W
        &UCt,                             // U
        0,                                // V
        CV_SVD_MODIFY_A | CV_SVD_U_T);    // flags

  cvReleaseMat(&PW0);

  // step 2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量
  // 讲道理这里的条件不应写成4,而应该是变量 number_of_correspondences 啊
  for(int i = 1; i < 4; i++) {
    // 这里也是只有前三个奇异值 
    double k = sqrt(dc[i - 1] / number_of_correspondences);
    for(int j = 0; j < 3; j++)
      //? 但是这里为什么要乘k？
      cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];

• void PnPsolver::compute_barycentric_coordinates(void)
  求解四个控制点的系数alphas
  每个三维点可以通过下面式子算出和控制点之间的关系:

  
  
  image

具体推导过程见 https://blog.csdn.net/qq_30356613/article/details/80588134

• void PnPsolver::fill_M(CvMat * M,
  const int row, const double * as, const double u, const double v)
  填充最小二乘的M矩阵，对每一个3D参考点，Mx=0，x为四个控制点组成的向量1*12：

  
  
  image

M推导过程见 https://blog.csdn.net/qq_30356613/article/details/80588134

double PnPsolver::compute_R_and_t(const double * ut, const double * betas,double R[3][3], double t[3])
根据已经得到的控制点在当前相机坐标系下的坐标来恢复出相机的位姿