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

記事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と同じです． 起動形式は

./rectify right_image left_image max_disparity min_disparity 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<cv::KeyPoint> lkeypoints;
  std::vector<cv::KeyPoint> 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<cv::DMatch> matches;
  matcher.match(ldescriptor, rdescriptor, matches);

  std::vector<cv::DMatch> g_matches;
  std::vector<cv::KeyPoint> g_lkeypoints;
  std::vector<cv::KeyPoint> 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<cv::Point2f> lkeypoints_2f;
  std::vector<cv::Point2f> rkeypoints_2f;
  cv::KeyPoint::convert(g_lkeypoints, lkeypoints_2f);
  cv::KeyPoint::convert(g_rkeypoints, rkeypoints_2f);
  std::vector<cv::Vec3f> llines;
  std::vector<cv::Vec3f> 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， far.png， board_ng.png， board_ok.png， circles.pdfをダウンロードしてください．

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

端末のシェル(shell)に

make near

./calib_png nof_features_in_horizontal nof_features_in_vertical distance_between_features image_file

make far

make ng

make ok

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

make cam

プログラムについて簡単に説明します．main()を持っているのはcalib_png.cppとcalib_cam.cppです． calib.cppはcalib_png.cppとcalib_cam.cppから共用される関数を集めたものです．

calib.cpp内のcalib()は1枚または複数枚の画像から単一カメラのキャリブレーションを行います． まず与えられた1枚の画像に円の格子状配列パターン(circles grid)が存在するかがチェックされ， 見つからないときはチェスボードパターンが存在するかがチェックされます． これも見つからないときはfalseでリターンします． 与えられた画像が何枚目の画像であるかをpictureという引数が示しています． pictureが0のときは与えられた画像が最初の画像であることを示しています． このときはこの1枚の画像でキャリブレーションを行います． pictureが3のときは与えられた画像が4枚目の画像であることを示しています． このときは4枚の画像でキャリブレーションを行います． この場合3枚目までの画像の情報は引数のpointsが持っています． このpointsはcalib()を呼ぶ側で定義され， calib()を呼ぶたびに情報が蓄積されていきます． pointsは画像から検出された特徴点のピクセル位置を収めるベクターのベクターとなっています． 上位のベクターは画像枚数に対応します． 一方calib()内で定義されるPointsは検出すべきキャリブレーションボードの特徴点の座標を計算で与えたものです． 特徴点のz座標はすべて0.0とされ，特徴点が平面上にあるとされます． 特徴点が平面上にあるということはZ. Zhangの手法の重要な前提です． 特徴点間の間隔（実距離）がこのPointsに埋め込まれますが，移動情報tvecsにしか影響を与えません． これは移動情報tvecsだけが実距離で与えられるからです． 回転情報rvecsは角度を表すものですからピクセルとも実距離とも独立です． それ以外のカメラ行列MMなどはピクセルを単位とする値です． Pointsとpoints、そして画像のピクセルサイズを入力情報としてOpenCVのcalibrateCamera()を呼び出します． 結果は最初は全く値を持たない， カメラ行列MM， 歪ベクターDD， 回転情報rvecs， 移動情報tvecs， 再投影誤差rmsに書き込まれます． calibrateCamera()をどう実行するかのオプション指定はすべての項目を列挙してコメントアウトしています． calib()に渡した画像には検出された特徴点と特徴点が線で結ばれた画像が付け加えられます．

次にcalib.cpp内のmap12_make()はOpenCVのremap()で使う変換テーブル(map1, map2)をつくる関数です． 最初にOpenCVのgetOptimalNewCameraMatrix()を呼び出して， 歪を正した場合にすべてのピクセルが画像に含まれる様な新しいカメラ行列を求めています． 次に元々のカメラ行列と歪ベクターから， この新しいカメラ行列を前提として歪を除いた画像を得る変換テーブルを， OpenCVのinitUndistortRectifyMap()で作成しています． このとき回転は必要ありませんから， 回転無しを意味する回転行列をロドリゲスのゼロ回転ベクターから作っています．

次にcalib.cpp内のmap12_init()はとりあえず表示を行うための変換テーブル(map1, map2)をつくる関数です． あとで正しい値に書き換えるのが前提です．

次にcalib_png.cppのmain()はファイルから読み込んだ1枚の画像で単一カメラのキャリブレーションを行います．

次にcalib_cam.cppのmain()はとりあえずの変換テーブルを使って， カメラからの画像をウィンドウに表示するループに入ります． これで動画表示となります． qキーはループを出て終了となります． dキーは最初にループに入ったときの状況に戻します． cキーは押すたびにその時点の画像を追加して複数画像による単一カメラのキャリブレーションを行います． 最初のcキーでは1枚の画像でのキャリブレーションとなります． とりあえずの変換テーブルはキャリブレーション結果によって正しい変換テーブルに更新されます． sキーはその時点のカメラ行列と歪ベクターをファイルに出力します． nキーは直前のcキー時の画像をboard_ng.pngという名前でファイルに書き出します． oキーは直前のcキー時の画像をboard_ok.pngという名前でファイルに書き出します． 最後の2つのコマンドは動画表示をおかしくした画像やそうでない画像を調べるためのものです．