2D-3Dの対応から射影行列を求める方法（DLTアルゴリズム）

2D-3Dの複数の対応から射影行列を求める方法は色々とあるが、Direct Linear Transform(DLT)と呼ばれる方法について紹介する。詳しくは、Multiple view geometryの7章に載っているので、不足している情報は、本を読んで補間して欲しい。

三次元座標を\(X_{i}\)、その画像上の観測座標を\(x_{i}\)(以下の式中の\(x_{i}, y_{i}, w_{i}\)はこのベクトルの各要素を表す)とすると、三次元座標\(X_{i}\)と観測座標\(x_{i}\)は射影行列\(P\)を用いて以下の様な関係を導くことができる。

\[ \begin{bmatrix}0^{T} & -w_{i}X_{i}^{T} & y_{i}X_{i}^T\\ w_{i}X_{i}^{T} & 0^{T} & -x_{i}X_{i}^{T} \end{bmatrix} \left(\begin{array}{c} p^{1^{T}} \\ p^{2^{T}} \\ p^{3^{T}}\end{array}\right) = 0 \]

ここで\( p^{j^{T}}\)は射影行列\(P\)の\(j\)行目の行ベクトルを表す。
\(Ap=0\)の形になっており\(\left|p\right|\neq0\)なので、射影行列\(P\)は行列\(A\)を特異値分解し、最小特異値に対応する右特異ベクトルを求めることで得ることが出来る。
（\bmが使えないとベクトルとスカラーが分からなくなるなぁ。。。）

以下は、三次元座標とその画像上の観測座標から射影行列を求めるための関数。
求められた射影行列にはスケールの不定性があるため、検証する際には射影行列の右下（3行4列の要素）を1.0とするなどの処理が必要になる。

cv::Mat calcProjectionMatrix(std::vector<cv::Point3d> op, std::vector<cv::Point2d> ip)
{
  cv::Mat A;
  A.create(cv::Size(12, op.size()*2), CV_64FC1);

  for (int i = 0, j = 0; i < op.size()*2; i+=2, ++j)
  {
    A.at<double>(i, 0) = 0.0;
    A.at<double>(i, 1) = 0.0;
    A.at<double>(i, 2) = 0.0;
    A.at<double>(i, 3) = 0.0;

    A.at<double>(i, 4) = -op[j].x;
    A.at<double>(i, 5) = -op[j].y;
    A.at<double>(i, 6) = -op[j].z;
    A.at<double>(i, 7) = -1.0;

    A.at<double>(i, 8) = ip[j].y*op[j].x;
    A.at<double>(i, 9) = ip[j].y*op[j].y;
    A.at<double>(i, 10) = ip[j].y*op[j].z;
    A.at<double>(i, 11) = ip[j].y;

    A.at<double>(i+1, 0) = op[j].x;
    A.at<double>(i+1, 1) = op[j].y;
    A.at<double>(i+1, 2) = op[j].z;
    A.at<double>(i+1, 3) = 1.0;

    A.at<double>(i+1, 4) = 0.0;
    A.at<double>(i+1, 5) = 0.0;
    A.at<double>(i+1, 6) = 0.0;
    A.at<double>(i+1, 7) = 0.0;

    A.at<double>(i+1, 8) = -ip[j].x*op[j].x;
    A.at<double>(i+1, 9) = -ip[j].x*op[j].y;
    A.at<double>(i+1, 10) = -ip[j].x*op[j].z;
    A.at<double>(i+1, 11) = -ip[j].x;
  }

  cv::Mat pvect;
  cv::SVD::solveZ(A, pvect);

  cv::Mat projectionMat(3, 4, CV_64FC1);

  for (int i = 0; i < 12; i++)
    projectionMat.at<double>(i/4, i%4) = pvect.at<double>( i );

  return projectionMat;
}