姿势优化的高斯牛顿实现错误

Question

我正在使用修改版的高斯-牛顿法来改进使用 OpenCV 的姿势估计。未修改的代码可以在这里找到：http://people.rennes.inria.fr/Eric.Marchand/pose-estimation/tutorial-pose-gauss-newton-opencv.html

相应论文中概述了此方法的详细信息：

Marchand, Eric, Hideaki Uchiyama, and Fabien Spindler. "Pose estimation for augmented reality: a hands-on survey." IEEE transactions on visualization and computer graphics 22.12 (2016): 2633-2651.

可在此处找到 PDF：https://hal.inria.fr/hal-01246370/document

相关部分（第 4 页和第 5 页）截屏如下：

这是我所做的。首先，我（希望）“纠正”了一些错误：(a) dt 和 dR 可以通过引用传递给 exponential_map()（即使 cv::Mat 本质上是一个指针）。 (b) 每个 2x6 雅可比矩阵 J.at<double>(i*2+1,5) 的最后一个条目是 -x[i].y 但应该是 -x[i].x。 (c) 我也尝试过使用不同的投影公式。具体来说，包括焦距和主点的：

xq.at<double>(i*2,0)   = cx + fx * cX.at<double>(0,0) / cX.at<double>(2,0);
xq.at<double>(i*2+1,0) = cy + fy * cX.at<double>(1,0) / cX.at<double>(2,0);

这是我正在使用的相关代码的全部（控制从 optimizePose3() 开始）：

void exponential_map(const cv::Mat &v, cv::Mat &dt, cv::Mat &dR)
{
  double vx = v.at<double>(0,0);
  double vy = v.at<double>(1,0);
  double vz = v.at<double>(2,0);
  double vtux = v.at<double>(3,0);
  double vtuy = v.at<double>(4,0);
  double vtuz = v.at<double>(5,0);
  cv::Mat tu = (cv::Mat_<double>(3,1) << vtux, vtuy, vtuz); // theta u
  cv::Rodrigues(tu, dR);
  double theta = sqrt(tu.dot(tu));
  double sinc = (fabs(theta) < 1.0e-8) ? 1.0 : sin(theta) / theta;
  double mcosc = (fabs(theta) < 2.5e-4) ? 0.5 : (1.-cos(theta)) / theta / theta;
  double msinc = (fabs(theta) < 2.5e-4) ? (1./6.) : (1.-sin(theta)/theta) / theta / theta;
  dt.at<double>(0,0) = vx*(sinc + vtux*vtux*msinc)
                     + vy*(vtux*vtuy*msinc - vtuz*mcosc)
                     + vz*(vtux*vtuz*msinc + vtuy*mcosc);
  dt.at<double>(1,0) = vx*(vtux*vtuy*msinc + vtuz*mcosc)
                     + vy*(sinc + vtuy*vtuy*msinc)
                     + vz*(vtuy*vtuz*msinc - vtux*mcosc);
  dt.at<double>(2,0) = vx*(vtux*vtuz*msinc - vtuy*mcosc)
                     + vy*(vtuy*vtuz*msinc + vtux*mcosc)
                     + vz*(sinc + vtuz*vtuz*msinc);
}


void optimizePose3(const PoseEstimation &pose, 
                   std::vector<FeatureMatch> &feature_matches,
                   PoseEstimation &optimized_pose) {

  //Set camera parameters
  double fx = camera_matrix.at<double>(0, 0); //Focal length
  double fy = camera_matrix.at<double>(1, 1);
  double cx = camera_matrix.at<double>(0, 2); //Principal point
  double cy = camera_matrix.at<double>(1, 2);


  auto inlier_matches = getInliers(pose, feature_matches);

  std::vector<cv::Point3d> wX;
  std::vector<cv::Point2d> x;

  const unsigned int npoints = inlier_matches.size();
  cv::Mat J(2*npoints, 6, CV_64F);
  double lambda = 0.25;
  cv::Mat xq(npoints*2, 1, CV_64F);
  cv::Mat xn(npoints*2, 1, CV_64F);
  double residual=0, residual_prev;
  cv::Mat Jp;


  for(auto i = 0u; i < npoints; i++) {

    //Model points
    const cv::Point2d &M = inlier_matches[i].model_point();

    wX.emplace_back(M.x, M.y, 0.0);


    //Imaged points
    const cv::Point2d &I = inlier_matches[i].image_point();

    xn.at<double>(i*2,0)   = I.x; // x
    xn.at<double>(i*2+1,0) = I.y; // y

    x.push_back(I);
  }


  //Initial estimation
  cv::Mat cRw = pose.rotation_matrix;
  cv::Mat ctw = pose.translation_vector;

  int nIters = 0;

  // Iterative Gauss-Newton minimization loop
  do {
    for (auto i = 0u; i < npoints; i++) {

      cv::Mat cX = cRw * cv::Mat(wX[i]) + ctw;            // Update cX, cY, cZ


      // Update x(q)
      //xq.at<double>(i*2,0)   = cX.at<double>(0,0) / cX.at<double>(2,0);    // x(q) = cX/cZ
      //xq.at<double>(i*2+1,0) = cX.at<double>(1,0) / cX.at<double>(2,0);    // y(q) = cY/cZ
      xq.at<double>(i*2,0)   = cx + fx * cX.at<double>(0,0) / cX.at<double>(2,0);
      xq.at<double>(i*2+1,0) = cy + fy * cX.at<double>(1,0) / cX.at<double>(2,0);

      // Update J using equation (11)
      J.at<double>(i*2,0) = -1 / cX.at<double>(2,0);        // -1/cZ
      J.at<double>(i*2,1) = 0; 
      J.at<double>(i*2,2) = x[i].x / cX.at<double>(2,0);    // x/cZ
      J.at<double>(i*2,3) = x[i].x * x[i].y;                // xy
      J.at<double>(i*2,4) = -(1 + x[i].x * x[i].x);         // -(1+x^2)
      J.at<double>(i*2,5) = x[i].y;                         // y

      J.at<double>(i*2+1,0) = 0;
      J.at<double>(i*2+1,1) = -1 / cX.at<double>(2,0);      // -1/cZ
      J.at<double>(i*2+1,2) = x[i].y / cX.at<double>(2,0);  // y/cZ
      J.at<double>(i*2+1,3) = 1 + x[i].y * x[i].y;          // 1+y^2
      J.at<double>(i*2+1,4) = -x[i].x * x[i].y;             // -xy
      J.at<double>(i*2+1,5) = -x[i].x;                      // -x
    }



    cv::Mat e_q = xq - xn;                     // Equation (7)


    cv::Mat Jp = J.inv(cv::DECOMP_SVD);        // Compute pseudo inverse of the Jacobian

    cv::Mat dq = -lambda * Jp * e_q;           // Equation (10)

    cv::Mat dctw(3, 1, CV_64F), dcRw(3, 3, CV_64F);
    exponential_map(dq, dctw, dcRw);

    cRw = dcRw.t() * cRw;                      // Update the pose
    ctw = dcRw.t() * (ctw - dctw);

    residual_prev = residual;                  // Memorize previous residual
    residual = e_q.dot(e_q);                   // Compute the actual residual

    std::cout << "residual_prev: " << residual_prev << std::endl;
    std::cout << "residual: " << residual << std::endl << std::endl;

    nIters++;

  } while (fabs(residual - residual_prev) > 0);
  //} while (nIters < 30);

  optimized_pose.rotation_matrix = cRw;
  optimized_pose.translation_vector = ctw;
  cv::Rodrigues(optimized_pose.rotation_matrix, optimized_pose.rotation_vector); 
}

即使我使用给定的函数，也不会产生正确的结果。我的初始姿势估计非常接近最佳，但是当我尝试运行该程序时，该方法需要很长时间才能收敛 - 当它收敛时，结果是非常错误的。我不确定哪里出了问题，而且我没有主意。我相信我的内点实际上是内点（它们是使用 M 估计器选择的）。我将指数映射的结果与其他实现的结果进行了比较，它们似乎是一致的。

那么，这个高斯-牛顿姿势优化实现的错误在哪里？对于任何愿意伸出援手的人，我都尽量让事情变得简单。让我知道是否还有我可以提供的信息。任何帮助将不胜感激。谢谢。

Answer 1

编辑：2019/05/13

OpenCV 中现在有 solvePnPRefineVVS 函数。

此外，您应该使用根据当前估计姿势计算的 x 和 y。

在引用的论文中，他们在标准化相机框架（在 z=1 处）表达了测量值 x。

在处理真实数据时，您有：

(u,v)：二维图像坐标（如关键点、角点位置等）
K：内参数（标定相机后得到）
D：畸变系数（标定相机后得到）

要计算归一化相机帧中的 2D 图像坐标，您可以在 OpenCV 中使用函数 cv::undistortPoints() (link to my 关于 cv::projectPoints() 和 cv::undistortPoints())。

没有失真时，计算（也称为"reverse perspective transformation"）为：

x = (u - cx) / fx
y = (v - cy) / fy

姿势优化的高斯牛顿实现错误

Error in gauss-newton implementation for pose optimization

c++

opencv

computer-vision

nonlinear-optimization

pose-estimation