pose-gauss-newton-opencv.cpp

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

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 pose_gauss_newton(const std::vector< cv::Point3d > &wX,
                       const std::vector< cv::Point2d > &x,
                       cv::Mat &ctw, cv::Mat &cRw)
{
  int npoints = (int)wX.size();
  cv::Mat J(2*npoints, 6, CV_64FC1);
  cv::Mat cX;
  double lambda = 0.25;
  cv::Mat err, sd(2*npoints, 1, CV_64FC1), s(2*npoints, 1, CV_64FC1);
  cv::Mat xq(npoints*2, 1, CV_64FC1);
  // From input vector x = (x, y) we create a column vector xn = (x, y)^T to ease computation of e_q
  cv::Mat xn(npoints*2, 1, CV_64FC1);
  //vpHomogeneousMatrix cTw_ = cTw;
  double residual=0, residual_prev;
  cv::Mat Jp;

  // From input vector x = (x, y, 1)^T we create a new one xn = (x, y)^T to ease computation of e_q
  for (int i = 0; i < x.size(); i ++) {
    xn.at<double>(i*2,0)   = x[i].x; // x
    xn.at<double>(i*2+1,0) = x[i].y; // y
  }

  // Iterative Gauss-Newton minimization loop
  do {
    for (int i = 0; i < npoints; i++) {
      cX = cRw * cv::Mat(wX[i]) + ctw;                      // Update cX, cY, cZ

      double Xi = cX.at<double>(0,0);
      double Yi = cX.at<double>(1,0);
      double Zi = cX.at<double>(2,0);

      double xi = Xi / Zi;
      double yi = Yi / Zi;

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

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

      J.at<double>(i*2+1,0) = 0;                            // 0
      J.at<double>(i*2+1,1) = -1 / Zi;                      // -1/cZ
      J.at<double>(i*2+1,2) = yi / Zi;                      // y/cZ
      J.at<double>(i*2+1,3) = 1 + yi * yi;                  // 1+y^2
      J.at<double>(i*2+1,4) = -xi * yi;                     // -xy
      J.at<double>(i*2+1,5) = -xi;                          // -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_64FC1), dcRw(3,3,CV_64FC1);
    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

  } while (fabs(residual - residual_prev) > 0);
}

int main()
{
  std::vector< cv::Point3d > wX;
  std::vector< cv::Point2d >  x;

  // Ground truth pose used to generate the data
  cv::Mat ctw_truth = (cv::Mat_<double>(3,1) << -0.1, 0.1, 0.5); // Translation vector
  cv::Mat crw_truth = (cv::Mat_<double>(3,1) << CV_PI/180*(5), CV_PI/180*(0), CV_PI/180*(45)); // Rotation vector
  cv::Mat cRw_truth(3,3,CV_64FC1); // Rotation matrix
  cv::Rodrigues(crw_truth, cRw_truth);

  // Input data: 3D coordinates of at least 4 points
  double L = 0.2;
  wX.push_back( cv::Point3d(  -L, -L, 0) ); // wX_0 (-L, -L, 0)^T
  wX.push_back( cv::Point3d( 2*L, -L, 0) ); // wX_1 ( L, -L, 0)^T
  wX.push_back( cv::Point3d(   L,  L, 0) ); // wX_2 ( L,  L, 0)^T
  wX.push_back( cv::Point3d(  -L,  L, 0) ); // wX_3 (-L,  L, 0)^T

  // Input data: 2D coordinates of the points on the image plane
  for(int i = 0; i < wX.size(); i++) {
    cv::Mat cX = cRw_truth*cv::Mat(wX[i]) + ctw_truth; // Update cX, cY, cZ
    x.push_back( cv::Point2d( cX.at<double>(0, 0)/cX.at<double>(2, 0),
                              cX.at<double>(1, 0)/cX.at<double>(2, 0) ) ); // x = (cX/cZ, cY/cZ)
  }

  // Initialize the pose to estimate near the solution
  cv::Mat ctw = (cv::Mat_<double>(3,1) << -0.05, 0.05, 0.45); // Initial translation
  cv::Mat crw = (cv::Mat_<double>(3,1) << CV_PI/180*(1), CV_PI/180*(0), CV_PI/180*(35)); // Initial rotation vector
  cv::Mat cRw = cv::Mat::eye(3, 3, CV_64FC1); // Rotation matrix set to identity
  cv::Rodrigues(crw, cRw);

  pose_gauss_newton(wX, x, ctw, cRw);

  std::cout << "ctw (ground truth):\n" << ctw_truth << std::endl;
  std::cout << "ctw (from non linear method):\n" << ctw << std::endl;
  std::cout << "cRw (ground truth):\n" << cRw_truth << std::endl;
  std::cout << "cRw (from non linear method):\n" << cRw << std::endl;

  return 0;
}