Ransac算法优化的PnP算法-随机抽样一致性算法

       在基于DLT直接线性变换求解的PnP算法中，我们通过建立一个超定方程来获得一个最小二乘解，在这个超正定方程中，我们考虑了所有的特征匹配点，这显然可能会带来误差，基于这样的思路我们引入Ransac（Random sample consensus，随机抽样一致性）算法来进行优化求解。

         Ransac算法的核心在于对现有数据进行多次随机抽样，通过划定内点和外点来衡量算法所建立模型的好坏，最终留下内点最多的模型。具体到PnP算法中，就是对特征匹配点进行多次的抽样，利用抽样到的若干特征匹配点计算出相机位姿（R和T），利用计算出的相机位姿对每个点计算重投影误差，对误差小于某个阈值的点，我们认为其为当前抽样计算结果的内点，误差小于某个阈值的点，我们认为为外点。显然内点的个数可以体现当前抽样计算结果的好坏。通过多次迭代抽样，获得内点最多的计算结果。下图为程序流程图：（这里的Ransac取消了原来按照阈值计算分数的方法，转为直接比较error大小进行迭代，便于调节参数）

下面为基于C++语言对基于Ransac算法优化PnP算法的一段示例程序：

    vector<cv::Point2f> undis_pts_2;
    cv::undistortPoints(pts_2, undis_pts_2, K, D);
    // Step 1: Work out the initial pose estimates using the linear solution.
    int points_num = pts_3.size();
    MatrixXd A(2 * 5, 9);
    VectorXd sub_A1(9);
    VectorXd sub_A2(9);
    int ransac_num;
    int ransac_times = 100;
    int ransac_score_last = 0;
    int ransac_score = 0;
    cv::Mat R_ransac_out;
    cv::Mat T_ransac_out;
    MatrixXd T_out(3, 1);
    Matrix3d R_out;
    double error_last = 0.0;
    vector<cv::Point2f> reproj_pts_2_ransac;
    for (int i = 0; i < ransac_times; i++){
      //cout << "ransac iteration: " << i << endl;
      for (int j = 0; j < 5; j++){
      ransac_num = rand() % points_num;
      cv::Point3f p_3 = pts_3[ransac_num];
      cv::Point2f p_2 = undis_pts_2[ransac_num];
      double x = p_3.x;
      double y = p_3.y;
      //double z = p_3.z;
      double u = p_2.x;
      double v = p_2.y;
      sub_A1 << x, y, 1, 0, 0, 0, -x * u, -y * u, -u;
      sub_A2 << 0, 0, 0, x, y, 1, -x * v, -y * v, -v;

      A.block<1, 9>(2 * j, 0) = sub_A1.transpose();
      A.block<1, 9>(2 * j + 1, 0) = sub_A2.transpose();
      }
      JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
      VectorXd H_vector(9);
      H_vector = svd.matrixV().col(8);
      Eigen::Matrix3d H;
      H.row(0) << H_vector(0), H_vector(1), H_vector(2);
      H.row(1) << H_vector(3), H_vector(4), H_vector(5);
      H.row(2) << H_vector(6), H_vector(7), H_vector(8);

      Matrix3d K_eigen;
      cv2eigen(K, K_eigen);
      //H = K_eigen.inverse() * H;
      Vector3d h3 = H.col(2);
      if (h3(2) < 0)
      {
          H = -H;
      }
      Vector3d h1 = H.col(0);
      Vector3d h2 = H.col(1);
      h3 = H.col(2);
      Matrix3d H_new;
      H_new.col(0) = h1;
      H_new.col(1) = h2;
      H_new.col(2) = h1.cross(h2);
      JacobiSVD<MatrixXd> svd_H(H_new, ComputeThinU | ComputeThinV);
      Matrix3d R_H;
      cv::Mat R_init;
      R_H = svd_H.matrixU() * (svd_H.matrixV().transpose());
      eigen2cv(R_H, R_init);
      MatrixXd T_r(3, 1);
      cv::Mat T_init;
      T_r = h3 / h1.norm();
      eigen2cv(T_r, T_init);
      cv::projectPoints(pts_3, R_init, T_init, K, D, reproj_pts_2_ransac);
      // Calculate the reprojection error
      double error = 0.0;
      for (int m = 0; m < points_num; m++)
      {
        cv::Point2f diff = pts_2[m] - reproj_pts_2_ransac[m];
        error += diff.x * diff.x + diff.y * diff.y;
        //error /= 20000;
        //cout << "error: " << error << endl;
        if (error < 10)
        {
          ransac_score++;
        }
      }
      //cout << "ransac score: " << ransac_score << endl;
      //cout << "ransac score: " << ransac_score_last << endl;
      if (error < error_last || error_last == 0.0)
      {
        R_ransac_out = R_init;
        T_ransac_out = T_init;
        ransac_score_last = ransac_score;
        error_last = error;
        T_out(0) = T_r(0);
        T_out(1) = T_r(1);
        T_out(2) = T_r(2);
        R_out = R_H;
      }
      ransac_score = 0;
    }
    
    cv::Mat R_init = R_ransac_out;
    cv::Mat T_init = T_ransac_out;