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

記事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.cu， rectify.cpp， makefile， 2_R.png， 2_L.pngをダウンロードしてください． rectify.cppは記事11のrectify.cppにfundamental()関数を追加するとともに，起動形式を少し改良したものになっています． ですので，このrectify.cppが本流の最新ということになります． fundamental()関数はMキーコマンド（ステレオ並行化の実行(do calibration)）の中で実行されるようにしました． 使える1文字キーコマンドの種類と数は記事11のrectify.cppと同じです． 起動形式は

./プログラム名 右画像 左画像 最大視差 最小視差 penalty値 inhibit値 divisor値 option値

端末のシェル(shell)に

make rec

fundamental()関数の内容

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

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);

  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);

  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によるグラフカットを使ったステレオ処理は 画像処理（2D処理）を全く使わない3D空間での表面抽出処理なので， 別の道を選択したということだと思います．

Z. Zhangの手法によるカメラ内部パラメータの抽出

最後にカメラ行列や歪係数を抽出するプログラムを置いておきます． Z. Zhangの手法が組み込まれたOpenCVのライブラリ関数cv::calibrateCamera()を使ったプログラムです． 基本的にはOpenCVの例（calibration.cpp）と変わりませんが，OpenCVの例は余りに読みにくかったので， 自分好みに作ってみたものです． calib.cpp， calib_png.cpp， calib_cam.cpp， near.png， board_ng.png， board_ok.png， circles.pdfをダウンロードしてください．

near.pngは記事11の「遠近のキャリブレーションボード（左が遠い，右は近い）」の近い方の画像です． board_ng.pngは同じキャリブレーションボードをロジクールのC270で最近撮影したものです． このキャリブレーションボードは発砲スチロールの板にcircles.pdfを印刷した紙を貼っているのですが， 作ってから2年近く経過していますので，near.pngの時に比べて貼った紙に皺が寄ってしまっています． 発砲スチロールの板の平面度はかなり良いと思います． board_ok.pngはcircles.pdfをディスプレーに表示させたものをロジクールのC270で最近撮影したものです． board_ng.pngは目で見ても何がまずいのか分かりませんが結果がおかしくなります． ディスプレーに写っているキャリブレーションパターンを撮影した場合の方が結果が安定するようです．

端末のシェル(shell)に

make near

./プログラム名 特徴点の横の数 特徴点の縦の数 特徴点の間隔 画像ファイル名

make ng

make ok

今度は端末のシェル(shell)に

make cam