5点アルゴリズムによるカメラ位置・姿勢の推定

2枚の画像間のカメラ位置・姿勢を推定する際に、それぞれの画像を撮影したカメラの内部パラメータが既知の場合には5点アルゴリズムが利用できる。OpenCVにも5点アルゴリズムでカメラ位置・姿勢を推定するための関数cv::findEssentialMatが用意されている。

使い方は、対応点の座標を以下のように指定するだけ。

cv::Mat essentialMat = cv::findEssentialMat(p1, p2);

ただし、デフォルトでは焦点距離1.0、画像中心座標(0, 0)で計算するようになっているので、下記のコードのように内部パラメータ行列を用いて座標変換を行うか焦点距離、画像中心座標を指定する必要がある。座標変換を行わずに利用したい場合は、焦点距離、画像中心座標を第3,4引数に設定すれば良い。

推定したEssential Matrixからカメラの並進と回転を求めるためには、cv::recoverPoseで並進ベクトルと回転ベクトルに分解することができる。

以下のように、2枚の画像から対応点を求めて、5点アルゴリズムでカメラ位置・姿勢を推定することができる。

#include <iostream>
#include <vector>

#include <opencv2/opencv.hpp>
#include <opencv2/nonfree.hpp>

int main(int argc, char** argv)
{
  cv::initModule_nonfree(); //モジュールの初期化

  //カメラパラメータの読み込みとレンズ歪の除去
  cv::Mat img1; //入力画像1
  cv::Mat img2; //入力画像2
  cv::Mat K;
  cv::Mat distCoeffs;
  cv::FileStorage fs("camera.xml", CV_STORAGE_READ);
  fs["intrinsicMat"] >> K;
  fs["distCoeffs"] >> distCoeffs;

  cv::undistort(cv::imread("image1.jpg"), img1, K, distCoeffs);
  cv::undistort(cv::imread("image2.jpg"), img2, K, distCoeffs);

  //特徴抽出
  cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create( "SURF" ); //検出器
  cv::Ptr<cv::DescriptorExtractor> descriptorExtractor = cv::DescriptorExtractor::create( "SURF" ); //特徴量
  cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce"); //対応点探索方法の設定

  std::vector<cv::KeyPoint> keypoints1, keypoints2;
  cv::Mat descriptor1, descriptor2;
  detector->detect(img1, keypoints1);
  descriptorExtractor->compute(img1, keypoints1, descriptor1);

  detector->detect(img2, keypoints2);
  descriptorExtractor->compute(img2, keypoints2, descriptor2);

  //対応点の探索
  std::vector<cv::DMatch> dmatch;
  std::vector<cv::DMatch> dmatch12, dmatch21;

  matcher->match(descriptor1, descriptor2, dmatch12); //img1 -> img2
  matcher->match(descriptor2, descriptor1, dmatch21); //img2 -> img1

  for (size_t i = 0; i < dmatch12.size(); ++i)
  {
    //img1 -> img2 と img2 -> img1の結果が一致しているか検証
    cv::DMatch m12 = dmatch12[i];
    cv::DMatch m21 = dmatch21[m12.trainIdx];

    if (m21.trainIdx == m12.queryIdx)
      dmatch.push_back( m12 );
  }

  //十分な数の対応点があれば基礎行列を推定
  if (dmatch.size() > 5)
  {
    std::vector<cv::Point2d> p1;
    std::vector<cv::Point2d> p2;
    //対応付いた特徴点の取り出しと焦点距離1.0のときの座標に変換
    for (size_t i = 0; i < dmatch.size(); ++i)
    {
      cv::Mat ip(3, 1, CV_64FC1);
      cv::Point2d p;

      ip.at<double>(0) = keypoints1[dmatch[i].queryIdx].pt.x;
      ip.at<double>(1) = keypoints1[dmatch[i].queryIdx].pt.y;
      ip.at<double>(2) = 1.0;
      ip = K.inv()*ip;
      p.x = ip.at<double>(0);
      p.y = ip.at<double>(1);
      p1.push_back( p );

      ip.at<double>(0) = keypoints2[dmatch[i].trainIdx].pt.x;
      ip.at<double>(1) = keypoints2[dmatch[i].trainIdx].pt.y;
      ip.at<double>(2) = 1.0;
      ip = K.inv()*ip;
      p.x = ip.at<double>(0);
      p.y = ip.at<double>(1);
      p2.push_back( p );
    }

    cv::Mat essentialMat = cv::findEssentialMat(p1, p2);
    std::cout << "Essential Matrix\n" << essentialMat << std::endl;

    cv::Mat r, t;
    cv::recoverPose(essentialMat, p1, p2, r, t);
    std::cout << "R:\n" << r << std::endl;
    std::cout << "t:\n" << t << std::endl;
  }
  return 0;
}