2D-3D対応からのカメラ位置・姿勢の推定

画像からの三次元復元処理や拡張現実感のための位置合わせ処理において、画像を撮影したカメラの位置・姿勢が必要になることがある。ここでは、カメラの内部パラメータ（焦点距離、画像中心座標、アスペクト比など）は既知とし、カメラの外部パラメータ（並進と回転）を求める方法を紹介する。
カメラの位置・姿勢は、三次元位置が既知の点Pとその画像上での観測座標pの複数の対応から求めることが出来る。このような問題はPerspective-n-Pont Problem（PnP問題）として知られている。OpenCVにはPnP問題を解くための関数cv::SolvePnPが用意されている。この関数の入力は三次元位置が既知の点の配列(objectPoints)とその画像上での観測座標の配列(imagePoints)、カメラの内部パラメータ行列、歪係数を入力とし、カメラの外部パラメータ（回転ベクトル、並進ベクトル）を推定する。
対応点の組み合わせに誤対応が含まれている場合は、cv::SolvePnPRansacを用いることでロバスト推定手法であるRANSACを用いた誤対応の排除が可能である。
また、推定手法としてLevenberg-Marquardt法による最適化(cv::ITERATIVE)や高速な推定手法であるEPnP(cv::EPNP)を選択することができる。経験的にはcv::ITERATIVEの方が安定した推定を実現できるように思う。
さらに、回転ベクトル、並進ベクトルの初期値が得られている場合には、第7引数をtrueにし、出力用のrvec, tvecに値を設定しておくことで、最適化の際の初期値として使用することができる。拡張現実感などの場合には、直前のフレームでの推定結果などを初期値として用いることで推定結果の安定性を向上させることができる。

#include <iostream>
#include <vector>

#include <opencv2/opencv.hpp>

void genRT(cv::Mat& rvect, cv::Mat& tvect);
void getCorrespondingPairs(cv::Mat K, cv::Mat d, cv::Mat rvec, cv::Mat tvec, std::vector<cv::Point3d>& op, std::vector<cv::Point2d>& ip);

int main(int argc, char** argv)
{
  cv::Mat K = (cv::Mat_<double>(3, 3) << 300, 0, 0, 0, 300, 0, 0, 0, 1);
  cv::Mat distortion = (cv::Mat_<double>(4, 1) << 0, 0, 0, 0);
  cv::Mat rvec(3, 1, CV_64FC1);
  cv::Mat tvec(3, 1, CV_64FC1);

  std::vector<cv::Point3d> objectPoints;
  std::vector<cv::Point2d> imagePoints;

  //カメラ位置・姿勢、対応点情報の生成
  genRT(rvec, tvec);
  getCorrespondingPairs(K, distortion, rvec, tvec, objectPoints, imagePoints);

  cv::Mat rvec_est;
  cv::Mat tvec_est;

  //誤対応の排除なし
  cv::solvePnP(objectPoints, imagePoints, K, distortion, rvec_est, tvec_est);
  //RANSACによる誤対応の排除あり
  //cv::solvePnPRansac(objectPoints, imagePoints, K, distortion, rvec_est, tvec_est, false, 100, 3.0, 100);

  cv::Mat rmat;
  cv::Rodrigues(rvec, rmat);
  std::cout << "rmat(ground truth): " << rmat << "\n" << "tvec(ground truth): " << tvec << std::endl;
  cv::Mat rmat_est;
  cv::Rodrigues(rvec_est, rmat_est);
  std::cout << "rmat(estimated): " << rmat_est << "\n" << "tvec(estimated): " << tvec_est << std::endl;

  return 0;
}

//カメラの位置・姿勢を生成
void genRT(cv::Mat& rvect, cv::Mat& tvect)
{
  rvect.at<double>(0) = (double)rand()/RAND_MAX;
  rvect.at<double>(0) = (double)rand()/RAND_MAX;
  rvect.at<double>(0) = (double)rand()/RAND_MAX;

  tvect.at<double>(0) = (double)rand()/RAND_MAX*10;
  tvect.at<double>(1) = (double)rand()/RAND_MAX*10;
  tvect.at<double>(2) = (double)rand()/RAND_MAX*10;
}
//2D-3Dの対応関係を生成
void getCorrespondingPairs(cv::Mat K, cv::Mat d, cv::Mat rvec, cv::Mat tvec, std::vector<cv::Point3d>& op, std::vector<cv::Point2d>& ip)
{
  cv::Mat rmat(3, 3, CV_64FC1);
  cv::Mat ep = cv::Mat::eye(4, 4, CV_64FC1);

  cv::Rodrigues(rvec, rmat);
  for(int i = 0; i < 3; ++i)
    for(int j = 0; j < 3; ++j)
    {
      ep.at<double>(i, j) = rmat.at<double>(i, j);
    }
  ep.at<double>(0, 3) = tvec.at<double>(0);
  ep.at<double>(1, 3) = tvec.at<double>(1);
  ep.at<double>(2, 3) = tvec.at<double>(2);

#define NUMBER_OF_POINTS 30
  for(int i = 0; i < NUMBER_OF_POINTS; ++i)
  {
    cv::Point2d p;
    p.x = (double)rand()/RAND_MAX*640;
    p.y = (double)rand()/RAND_MAX*480;
    ip.push_back( p );
  }
  cv::convertPointsToHomogeneous(ip, op);

  for(int i = 0; i < NUMBER_OF_POINTS; ++i)
  {
    cv::Mat p( op[i] );
    p = K.inv()*p;

    double dist = (double)rand()/RAND_MAX*1000;
    p *= dist;

    cv::Mat p_homo(4, 1, CV_64FC1);
    p_homo.at<double>(0) = p.at<double>(0);
    p_homo.at<double>(1) = p.at<double>(1);
    p_homo.at<double>(2) = p.at<double>(2);
    p_homo.at<double>(3) = 1.0;

    cv::Mat p_w(4, 1, CV_64FC1);
    p_w = ep.inv()*p_homo;

    op[i].x = p_w.at<double>(0);
    op[i].y = p_w.at<double>(1);
    op[i].z = p_w.at<double>(2);
  }
}

結果
rmat(ground truth): [-0.9688404116280623, -0.1751403106031944, 0.175140310603194
4;
 0.1751403106031944, 0.01557979418596911, 0.9844202058140315;
 -0.1751403106031944, 0.9844202058140315, 0.01557979418596911]
tvec(ground truth): [8.087405011139255;  5.850093081453903;  4.798730430005799]
rmat(estimated): [-0.9688404116280625, -0.1751403106031941, 0.1751403106031939;
 0.1751403106031939, 0.01557979418596855, 0.9844202058140312;
 -0.1751403106031941, 0.9844202058140312, 0.015579794185969]
tvec(estimated): [8.087405011139273;  5.850093081453918;  4.798730430005823]