软件工程应用与实践第十三篇

ORB-SLAM3 PnPSolver.cc相关代码分析
2021SC@SDUSC

---

前言

接上一篇的内容

---

1.通过之前求解的位姿来进行3D-2D投影，统计内点数目

void PnPsolver::CheckInliers()
{
    mnInliersi=0;

    // 遍历当前帧中所有的匹配点
    for(int i=0; i<N; i++)
    {
        // 取出对应的3D点和2D点
        cv::Point3f P3Dw = mvP3Dw[i];
        cv::Point2f P2D = mvP2D[i];

        // 将3D点由世界坐标系旋转到相机坐标系
        float Xc = mRi[0][0]*P3Dw.x+mRi[0][1]*P3Dw.y+mRi[0][2]*P3Dw.z+mti[0];
        float Yc = mRi[1][0]*P3Dw.x+mRi[1][1]*P3Dw.y+mRi[1][2]*P3Dw.z+mti[1];
        float invZc = 1/(mRi[2][0]*P3Dw.x+mRi[2][1]*P3Dw.y+mRi[2][2]*P3Dw.z+mti[2]);

        // 将相机坐标系下的3D进行针孔投影
        double ue = uc + fu * Xc * invZc;
        double ve = vc + fv * Yc * invZc;

        // 计算特征点和投影点的残差大小
        float distX = P2D.x-ue;
        float distY = P2D.y-ve;

        float error2 = distX*distX+distY*distY;

        // 判定
        if(error2<mvMaxError[i])
        {
            mvbInliersi[i]=true;
            mnInliersi++;
        }
        else
        {
            mvbInliersi[i]=false;
        }
    }
}

2.从给定的匹配点中计算出四个控制点

void PnPsolver::choose_control_points(void)
{
  // Take C0 as the reference points centroid:
  // Step 1：第一个控制点：参与PnP计算的参考3D点的质心（均值）
  // cws[4][3] 存储控制点在世界坐标系下的坐标，第一维表示是哪个控制点，第二维表示是哪个坐标(x,y,z)
  // 计算前先把第1个控制点坐标清零
  cws[0][0] = cws[0][1] = cws[0][2] = 0;

  // 遍历每个匹配点中世界坐标系3D点，然后对每个坐标轴加和
  // number_of_correspondences 默认是 4
  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;


  // Take C1, C2, and C3 from PCA on the reference points:
  // Step 2：计算其它三个控制点，C1, C2, C3通过特征值分解得到
  // ref: https://www.zhihu.com/question/38417101
  // ref: https://yjk94.wordpress.com/2016/11/11/pca-to-layman/

  // 将所有的3D参考点写成矩阵，(number_of_correspondences * 3)的矩阵
  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);     // PW0^T * PW0，为了进行特征值分解
  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：利用特征值分解得到三个主方向
  // cvMulTransposed(A_src,Res_dst,order, delta=null,scale=1): 
  // Calculates Res=(A-delta)*(A-delta)^T (order=0) or (A-delta)^T*(A-delta) (order=1)
  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++) {
    // 这里只需要遍历后面3个控制点
    double k = sqrt(dc[i - 1] / number_of_correspondences);
    for(int j = 0; j < 3; j++)
      cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
  }
}

3.计算在给定位姿的时候的3D点投影误差

double PnPsolver::reprojection_error(const double R[3][3], const double t[3])
{
  // 统计误差的平方
  double sum2 = 0.0;

  // 遍历每个3D点
  for(int i = 0; i < number_of_correspondences; i++) {
    // 指针定位
    double * pw = pws + 3 * i;
    // 计算这个3D点在相机坐标系下的坐标,逆深度表示
    double Xc = dot(R[0], pw) + t[0];
    double Yc = dot(R[1], pw) + t[1];
    double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
    // 计算投影点
    double ue = uc + fu * Xc * inv_Zc;
    double ve = vc + fv * Yc * inv_Zc;
    // 计算投影点与匹配2D点的欧氏距离的平方
    double u = us[2 * i], v = us[2 * i + 1];
    // 得到其欧式距离并累加
    sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) );
  }
  // 返回平均误差
  return sum2 / number_of_correspondences;
}

---