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

./プログラム名 右画像 左画像 最大視差 最小視差 penalty値 inhibit値 divisor値 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に対応します. またステレオ並行化処理の時にはピクセルを間引いてグラフカットを行います. divisor値を4とすると縦横ともに4ピクセルを1ピクセルに間引いてグラフカットを行います. このときグラフのサイズは1/(4*4*4)=1/64となっています. これだけ間引いても正解が得られるというのは面白いことです.

端末のシェル(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を使っています.またカメラ行列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);
次に良い対応を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によるグラフカットを使ったステレオ処理は 画像処理(2D処理)を全く使わない3D空間での表面抽出処理なので, 別の道を選択したということだと思います.

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

最後にカメラ行列や歪係数を抽出するプログラムを置いておきます. Z. Zhangの手法が組み込まれたOpenCVのライブラリ関数cv::calibrateCamera()を使ったプログラムです. 基本的にはOpenCVの例(calibration.cpp)と変わりませんが,OpenCVの例は余りに読みにくかったので, 自分好みに作ってみたものです. calib.cppcalib_png.cppcalib_cam.cppnear.pngfar.pngboard_ng.pngboard_ok.pngcircles.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.cpp)を起動すると,near.pngに写っている平面上の円の配列 を元にカメラ内部パラメータを計算します. その後, 再投影誤差を出力し, 焦点距離と中心座標を出力し, 歪係数を出力し, キャリブレーションパターンがどう回転しているかの情報を出力し, キャリブレーションパターンがどう移動しているかの情報を出力し, 歪を正した画像をboardという名前のウィンドウに表示して キー入力待ちとなります. 任意のキーを押下すると終了します. 文字がwの時は歪を正した画像をundistort.pngとして出力して終了します. キャリブレーションボードが検出できないときは cannot find calibration boardと出力して終了します. calib_png.cppの起動形式は
./プログラム名 特徴点の横の数 特徴点の縦の数 特徴点の間隔 画像ファイル名
となっています.チェスボードパターンと円の格子状配列パターン(circles grid)が扱えます.チェスボードでは交差する点が特徴点, 格子状に置かれた円では円(斜めから見た場合は楕円)の中心が特徴点となります. 特徴点の間隔は「キャリブレーションパターンがどう移動しているかの情報」以外にはほとんど影響を与えません.
make far
と比べてみてください.こちらでは焦点距離が長くなって,移動情報も遠くになっています. near.pngとfar.pngは同じカメラのズームを変えて撮影したものです. 詳しくは記事11を見てください. 次に
make ng
とすると同じプログラム(calib_png.cpp)に対して画像ファイルとしてboard_ng.pngが使われますが, 歪を正した画像として
board_ng.pngを処理した結果
がウィンドウに表示されます. 次に
make ok
では同じプログラム(calib_png.cpp)に対して画像ファイルとしてboard_ok.pngが使われますが, まともな画像がウィンドウに表示されます.

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

make cam
と入力してプログラム(calib_cam.cpp)を起動すると カメラが起動して カメラに写っている内容が ディスプレーのウィンドウに動画表示され, キー入力待ちとなります. 文字qは終了となります. 文字dはキャリブレーションの結果と蓄積画像をクリアします. 文字cは押下時点のカメラ画像を蓄積画像に追加して, 蓄積されている全ての画像を使ってキャリブレーション(カメラの内部パラメータの抽出)を行います. キャリブレーションボードが検出できないときは cannot find calibration boardと出力されてキャリブレーションは実行されず, 画像の追加も行われません. 文字oは直前の文字cコマンドで追加された画像をboard_ok.pngとして出力します. 文字nは直前の文字cコマンドで追加された画像をboard_ng.pngとして出力します. 文字sはその時点のカメラの内部パラメータ(カメラ行列と歪係数)をcamera.ymlとして出力します. キャリブレーションを実行すると得られたカメラの内部パラメータからカメラ入力の歪が修正されて動画表示されるようになります. このプログラムにより複数枚の画像からカメラパラメータを求めることができます. しかしどのような位置角度で何枚撮影すれば良いかの正解は見つかりませんでした. ステレオ処理を使ったステレオ並行化処理(3次元復元法)ではカメラ行列の内容に関し, オーダー程度の精度があれば十分なのでそれでも困ることはありません.

プログラムについて簡単に説明します.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つのコマンドは動画表示をおかしくした画像やそうでない画像を調べるためのものです.