12. Fundamental Matrix and Essential Matrix

ステレオ並行化処理のまとめ

記事10では, 右カメラの画像が左カメラの画像に対してどれたけ回転しているかを示す回転行列が\(R\)であるとき (すなわち\(R_r=RR_l\)のとき), \begin{align} \tilde{\ddot{m_r}}=A_rLA_r^{-1}\tilde{m_r} \label{eq:16} \end{align} \begin{align} \tilde{\ddot{m_l}}=A_rLRA_l^{-1}\tilde{m_l} \label{eq:15} \end{align} と変換した 右画像\(\tilde{\ddot{m_r}}\) と 左画像\(\tilde{\ddot{m_l}}\) が, あるカメラの撮影画像とその同じカメラが, ある方向に並行移動して撮影した画像となっており, 平行移動の方向は回転行列 \(L\)を選ぶことによって任意の方向とすることができることを説明しました. また記事11では, Gaze_line-Depth Modelによるグラフカットを使って, 2枚の画像が、あるカメラの撮影画像とその同じカメラが \(-fu\)軸方向に並行移動して撮影した画像となるための \begin{align} L \end{align} \begin{align} LR \end{align} を求めること(ステレオ並行化)ができることを説明しました.ここではこれらからステレオカメラの基礎行列\(F\) (Fundamental Matrix)と基本行列\(E\)(Essential Matrix)を求めてみたいと思います.OpenCVには 複数の対応点から\(F\)や\(E\)を推定するfindFundamentalMat関数やfindEssentialMat関数がありますが, この新しい方法では全ての画素の辻褄の合う3D表面としての対応から\(F\)や\(E\)を求めることになります. 従来の流れではステレオ処理のために\(F\)や\(E\)が必要だったものを, ステレオ処理を先に行ってから\(F\)や\(E\)を求めることになります. ただし相変わらずカメラ行列\(A\)はZ. Zhangの方法等により求めておく必要がありますが, 後で述べますが,そこそこ正しい値であれば破綻はしないようです.

ステレオカメラの\(F\)と\(E\)

ステレオカメラでは空間上の点\(P\)の左画像座標\(\tilde{m_l}\)と右画像座標\(\tilde{m_r}\)は 基礎行列\(F\)(Fundamental Matrix)によって \begin{align} \tilde{m_l}^T F \tilde{m_r} = 0 \label{eq:5} \end{align} と関係付けられ(拘束され),左カメラ座標\(x_l\)と右カメラ座標\(x_r\)は 基本行列\(E\)(Essential Matrix)によって \begin{align} x_l^T E x_r = 0 \label{eq:6} \end{align} と関係付けられます(拘束されます).\eqref{eq:5}を基礎方程式,\eqref{eq:6}を基本方程式と言います. なぜそのようなことが成立するかはビジョンの教科書を見てください. \(F\)と\(E\)の間には \begin{align} x_l^T E x_r = (A_l^{-1}\tilde{m_l})^T E A_r^{-1}\tilde{m_r} = \tilde{m_l}^T(A_l^{-1})^T E A_r^{-1}\tilde{m_r} = \tilde{m_l}^T F \tilde{m_r} \end{align} ですから \begin{align} F = (A_l^{-1})^T E A_r^{-1} \end{align} の関係があります.

並行ステレオカメラの基礎行列\(F\)

それでは並行化が完了したステレオカメラでは,基礎方程式 \begin{align} (\tilde{\ddot{m_l}})^TF\tilde{\ddot{m_l}}= \begin{pmatrix} u_l & v & 1 \end{pmatrix}F \begin{pmatrix} u_r \\ v \\ 1 \end{pmatrix}=0 \end{align} において,\(v\)が共通で,\(u_l\)と\(u_r\)は点\(P\)の奥行きによって如何様にもなるのですから, \begin{align} \begin{pmatrix} u_l & v & 1 \end{pmatrix} \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & s \\ 0 & -s & 0 \end{pmatrix} \begin{pmatrix} u_r \\ v \\ 1 \end{pmatrix}=0 \end{align} となっていなければなりません.すなわち\(\tilde{\ddot{m_l}}\)と\(\tilde{\ddot{m_l}}\)に対してその基礎行列\(f\)は \begin{align} f = \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & s \\ 0 & -s & 0 \end{pmatrix} \end{align} です.これを元の\(\tilde{m_l}\)と\(\tilde{m_r}\)に戻すと, \begin{align} (A_rLRA_l^{-1}\tilde{m_l})^T \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & s \\ 0 & -s & 0 \end{pmatrix} A_rLA_r^{-1}\tilde{m_r}= (\tilde{m_l})^T(A_l^{-1})^TR^TL^TA_r^T \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & s \\ 0 & -s & 0 \end{pmatrix} A_rLA_r^{-1}\tilde{m_r}=0 \end{align} ですから,\(\tilde{m_l}\)と\(\tilde{m_r}\)に対してその基礎行列\(F\)は \begin{align} F = (A_l^{-1})^TR^TL^TA_r^T \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & s \\ 0 & -s & 0 \end{pmatrix} A_rLA_r^{-1} \end{align} あるいは \begin{align} F = (A_l^{-1})^TR^TL^TA_r^TfA_rLA_r^{-1} \end{align} となります.

プログラムの配布

それではステレオ処理により基礎行列を求めるプログラムについて説明します. graph_cut.curectify.cppmakefile2_R.png2_L.pngをダウンロードしてください. rectify.cppは記事11のrectify.cppにfundamental()関数を追加するとともに,起動形式を少し変更したものになっています. fundamental()関数はMキーコマンド(ステレオ並行化の実行(do calibration))の中で実行されるようにしました. 使える1文字キーコマンドの種類と数は記事11のrectify.cppと同じです. 起動形式は

./プログラム名 右画像 左画像 最大視差 最小視差 penalty値 inhibit値 option値
となっています.option値でMキーコマンドの実行内容を指定します. option値が1の時は動画(result.avi)が出力されます. option値が2の時はグラフカット値の変化がgnuplot用のデータ(l??.out,r??.out)として出力されます. option値が4の時はステレオ並行化された画像(rec_L.png,rec_R.png)が出力されます. option値が8の時はfundamental()関数が実行され, matches.png,lkeypoints.png,rkeypoints.png,lepilines.png,repilines.pngが出力されます. このoption値は2のべき乗になっていますので,合計値で平行指定が可能です. 例えばoption値を3とすると最初の2つ,15とするとすべてが選択されます. ですのでoption値は0から15までの値が意味を持ちます. 記事11のrectify.cppではoption値は0か1しかなく,0は今回の0に,1は今回の7に対応します.

端末のシェル(shell)に

make rec
と入力してプログラムを起動し,Mキーを押下してステレオ並行化処理を実行して, Calibration completedが表示されたら qキー押下で終了してください.makefileを見ると分かりますが,option値は8が指定されていますので, matches.png,lkeypoints.png,rkeypoints.png、lepilines.png,repilines.png の5つのファイルができているはずです.それらを以下に示します.まずmatches.pngは
対応する特徴点
のようになります.次にlkeypoints.pngとrepilines.pngは
左の特徴点とそのエピ極線
のようになります.また,rkeypoints.pngとlepilines.pngは
右の特徴点とそのエピ極線
のようになります. 対応する点がエピ極線の上にあることが確認できると思います.

fundamental()関数の内容

まず関数の最初で基礎行列Fを求めます. 関数の入力引数はグラフカットによる並行化処理で決定された 左カメラと右カメラのロドリゲス回転ベクトルの成分です. またMはカメラ行列です. このMの内容は真値に近い値であれば結果にほとんど影響を与えません. たとえば焦点距離が数倍違っても目で見て分かる違いは発生しません. もちろん100倍違うとグラフカットを使った並行化処理がうまく行かなくなります. これは重要なことだと思われますが,詳細な検討は行っていません. それでカメラ行列は皆同じMを使っています.

void fundamental(int l0, int l1, int l2, int r0, int r1, int r2) {
  cv::Mat lV = cv::Mat::zeros(1, 3, CV_64F);
  cv::Mat rV = cv::Mat::zeros(1, 3, CV_64F);
  lV.at(0, 0) = 0.00001*l0;
  lV.at(0, 1) = 0.00001*l1;
  lV.at(0, 2) = 0.00001*l2;
  rV.at(0, 0) = 0.00001*r0;
  rV.at(0, 1) = 0.00001*r1;
  rV.at(0, 2) = 0.00001*r2;
  cv::Mat lR;
  cv::Mat rR;
  Rodrigues(lV, lR);
  Rodrigues(rV, rR);
  cv::Mat f = cv::Mat::zeros(3, 3, CV_64F);
  f.at(1, 2) =  1.0;
  f.at(2, 1) = -1.0;
  cv::Mat F = M.inv().t()*lR.t()*M.t()*f*M*rR*M.inv();
次に特徴点の検出と特徴量の計算を行います.ここから後の部分は 石立 喬さんのホームページ が大変参考になりました.
  cv::Mat lcolor = lframe.clone();
  cv::Mat rcolor = rframe.clone();
  cv::Mat lgray;
  cv::Mat rgray;
  cv::cvtColor(lcolor, lgray, CV_BGR2GRAY);
  cv::cvtColor(rcolor, rgray, CV_BGR2GRAY);
  std::vector lkeypoints;
  std::vector rkeypoints;
  cv::Mat ldescriptor;
  cv::Mat rdescriptor;
  auto detector = cv::ORB::create();
  detector->detectAndCompute(lgray, cv::Mat(), lkeypoints, ldescriptor);
  detector->detectAndCompute(rgray, cv::Mat(), rkeypoints, rdescriptor);
次に対応を計算します.
  cv::BFMatcher matcher(cv::NORM_HAMMING, true);
  std::vector matches;
  matcher.match(ldescriptor, rdescriptor, matches);
次に良い対応を5個程度選択します.
  std::vector g_matches;
  std::vector g_lkeypoints;
  std::vector g_rkeypoints;
  for (int t = 1; ; t++) {
    int n = 0;
    for (int i = 0; i < matches.size(); i++) if (matches[i].distance < t) n++;
    if (n > 5) {
      for (int i = 0; i < matches.size(); i++) {
        if (matches[i].distance < t) {
          g_matches.push_back(matches[i]);
          g_lkeypoints.push_back(lkeypoints[matches[i].queryIdx]);
          g_rkeypoints.push_back(rkeypoints[matches[i].trainIdx]);
        }
      }
      break;
    }
  }
次に全ての特徴点と選択された対応を画像として出力します.
  cv::Mat image_matches;
  drawMatches(lcolor, lkeypoints, rcolor, rkeypoints, g_matches, image_matches);
  cv::imwrite("matches.png", image_matches);
次に選択された特徴点を画像として出力します.
  cv::Mat image_lkeypoints;
  cv::Mat image_rkeypoints;
  cv::drawKeypoints(lcolor, g_lkeypoints, image_lkeypoints, cv::Scalar(0, 0, 255));
  cv::drawKeypoints(rcolor, g_rkeypoints, image_rkeypoints, cv::Scalar(0, 0, 255));
  cv::imwrite("lkeypoints.png", image_lkeypoints);
  cv::imwrite("rkeypoints.png", image_rkeypoints);
次に最初に求めた基礎行列Fを使ってエピ極線を計算します.
  std::vector lkeypoints_2f;
  std::vector rkeypoints_2f;
  cv::KeyPoint::convert(g_lkeypoints, lkeypoints_2f);
  cv::KeyPoint::convert(g_rkeypoints, rkeypoints_2f);
  std::vector llines;
  std::vector rlines;
  cv::computeCorrespondEpilines(rkeypoints_2f, 1, F, llines);
  cv::computeCorrespondEpilines(lkeypoints_2f, 2, F, rlines);
最後にエピ極線を画像として出力して終了です.
  float a, b, c;
  for (auto it = llines.begin(); it != llines.end(); it++) {
    a = (*it)[0];
    b = (*it)[1];
    c = (*it)[2];
    cv::line(lcolor, cv::Point(0, -c/b),
                     cv::Point(lcolor.cols-1, -(a/b*(lcolor.cols-1)+c/b)),
                     cv::Scalar::all(255));
  }
  for (auto it = rlines.begin(); it != rlines.end(); it++) {
    a = (*it)[0];
    b = (*it)[1];
    c = (*it)[2];
    cv::line(rcolor, cv::Point(0, -c/b),
                     cv::Point(rcolor.cols-1, -(a/b*(rcolor.cols-1)+c/b)),
                     cv::Scalar::all(255));
  }
  cv::imwrite("lepilines.png", lcolor);
  cv::imwrite("repilines.png", rcolor);
}

感想

今回は基礎行列が正しく得られているかの確認のため, ORB(Oriented FAST and Rotated BRIEF)という名称の特徴点抽出器を使いましたが, 見る位置や方向が異なる2枚の画像から対応する点を画像処理で見つけるというのは やはり間違っていると感じました.このホームページで紹介している Gaze_line-Depth Modelによるグラフカットを使ったステレオ処理は 最初から3D空間での表面抽出処理なので, 画像処理(2D処理)は全く使っていません. それでステレオ処理の結果を使って基礎行列を求めてどうするのかということですが, 初めからカメラ間の回転と平行移動の方向は分かっているのですから, 基礎行列や基本行列の意義は余りないのではと思います.

カメラ内部パラメータの抽出

最後にカメラ行列や歪ベクトルを測定するプログラムを置いておきます. Z. Zhangの手法が組み込まれたOpenCVのライブラリ関数を使ったプログラムです. 基本的にはOpenCVの例(calibration.cpp)と変わりませんが,OpenCVの例は余りに読みにくかったので, 自分好みに作ってみたものです. calib.cppcalib_png.cppcalib_cam.cppnear.pngfar.pngcalibboard.pngcircles.pdfをダウンロードしてください. 端末のシェル(shell)に

make png
と入力してプログラムを起動するとnear.pngに写っているcircles.pdf を元にカメラ内部パラメータを計算して焦点距離を出力してキー入力待ちとなります. 任意のキーを押下すると終了します.

次に,端末のシェル(shell)に

make cam
と入力してプログラムを起動するとカメラが起動してキー入力待ちとなります. 文字qは終了となります. 文字dは画像蓄積をクリアします. 文字oは直前の文字cコマンド