Plane Sweeping Stereo 〜 理論編
ということで，導入編に引き続き理論編です．
０．Plane Sweeping Stereo のざっくり原理
まず，１つのモノを複数のカメラで撮影する状況を考えます．
で，上図を見るとわかると思うのですが，複数カメラで空間中の同一点を撮影した場合，カメラ中心からスクリーンを結んだ半直線（レイ）が実際に点が存在する場所で１つに交わります．この原理を使うと，複数画像に写っている点の３次元位置が分かりそうです．で，これをストレートに実装した場合，下記のようになります．
（前提として，事前に各カメラの相対位置がわかっている必要があります．）
が，実際問題として上記のように実装した場合グリッド数が爆発します．また，１ピクセルの視差の奥行き方向解像度も奥行きが大きくなるに連れて下がってくるので，グリッドの切り方にもかなり工夫が必要そうです．で，もう少し工夫して視差画像が求める方法として考案された方法が「Plane Sweep Stereo」になります．
Plane Sweeping Stereo の大まかなステップ
下図が原理イメージ図になりますが，大まかなステップとしては，
①複数ある画像から１つ Reference 画像を選択．
②Reference 画像の奥行き方向にいくつも仮説平面を設定します．
③他の画像を Source 画像と書いていますが，Source 画像からこの仮説平面へワープするためのホモグラフィー行列を計算します．
④実際にホモグラフィー行列を使ってSource画像を仮説平面にワープさせます．
⑤仮説平面とワープ後Source画像をピクセル単位で比較し，最も一致度が高かった仮説平面の番号をメモしておきます．
⑥すべての計算が終了後に，ピクセルにメモされている仮説平面の番号から，そのピクセルの深度がわかります．
なんだかずらずらと書いてしまったのですが，「もし仮説平面上に物体があったとすると，ワープ後Source画像と仮説平面はその部分に関してはとても良く一致するはずだ！」というのがミソです．
１．もう一度ホモグラフィー！
ということで，実際にミソの部分を実験してみます．奥行きの異なる場所に同じサイズのキャリブボードをおき，２つの視点から撮影します．キャリブボードを正面から撮影した画像がReference画像のイメージで，斜めから撮影した画像がSource画像のイメージです．
で，下記を検証します．
①手前側のキャリブボードの一致を使って計算したホモグラフィー行列による画像変換では，手前側のキャリブボードの部分「のみ」が綺麗に一致する．
②奥側のキャリブボードの一致を使って計算したホモグラフィー行列による画像変換では，奥側のキャリブボード「のみ」が綺麗に一致する．
手前側で計算したホモグラフィー行列とその変換
手前側のキャリブが綺麗に一致してます．
H = [2.919985764518131, 0.04163314453775299, -621.3982383025455; 0.2764719489904646, 1.490676852159184, -37.0904308852414; 0.0009696597430956257, 9.143936486285768e-06, 1]
奥側で計算したホモグラフィー行列とその変換
奥側のキャリブが綺麗に一致してます．
H = [3.069557032214561, 0.2252515263892633, -1225.743495928632; 0.3344542384857155, 1.836184439317147, -176.1551921844029; 0.001130769317868212, 7.994190652823021e-05, 1]
上記ホモグラフィーの実験から，仮想平面上に実際に物体がある場合，Source画像から仮想平面に対してのホモグラフィー変換すれば「該当ピクセルに関してのみ」は変換後画像と仮想平面画像がほぼ一致することがわかります．逆をいえば，「Source画像をホモグラフィー変換して仮想平面画像と比較した場合に，一致度が高ければ実際にそこには物体がある」と言えるのかなと思います．
２．アルゴリズム
ということで，アルゴリズム詳細に入っていきます．
アルゴリズムの入力・出力
まず，入力と出力ですが，上図に書いてあるように，入力が下記になります．
入力
１．異なる視点で撮影した複数枚の画像（ステレオ校正されている必要は無し．）
２．各視点の位置（R, T）
３．各カメラの内部パラメータ
出力
１．Reference画像を元にした視差画像
通常のステレオと違って，このアルゴリズムでは複数枚の画像を使うことができます．なので，２画像のステレオで問題になるオクルージョンの問題も緩和することができます．論文ではこの当たりのアルゴ説明もされているのですが，今回は基本的な原理のみ下記下したいと思います．
二画像間のホモグラフィー
２つの画像の相対的な位置関係がわかっている時のホモグラフィーの導出の仕方です．ここで求めようとしているホモグラフィーによって「Reference画像のある画像座標 (x, y) から対応するSource画像の画像座標 (u, v) 」を求めます．２座標間の回転・並進の関係とホモグラフィー行列を書いてみましたが，下記の２がSource画像，下記の１がReference画像のイメージになります．
ここで，上図の最後にR，Tとホモグラフィー行列の関係を書いてますが，平面の単位法線ベクトルNは座標系１に関しての表現を使います．次に，上図のホモグラフィー行列を使って，奥行きdmの仮想平面に対応する (u, v) の座標を求める式を導出します．
で，論文（Real-time Plane-sweeping Stereo with Multiple Sweeping Directions）に乗っているホモグラフィーの式はグローバル座標系で求めているのでちょっと形が違います．ちょっとこの辺の座標変換がどうも慣れてなくて，だいぶハマったんですが，一応同じような形になりました．（が，ちょっと自信無しです．座標系の定義って，もっとはっきり書いてなくてもわかる人にはわかるもんなんですかね．．．）
ということで，ひとまず主要部分はまとまったかと思います．次は実装編にいきます！
Plane Sweeping Stereo 〜 導入編
いろいろと順番が前後してるんですが，キャリブもSFMも後でまとめるとして， Plane Sweeping Stereo を勉強＆コード解剖したのでまとめておきたいと思います．
０．普通のステレオカメラ
SGMとか普通のステレオアルゴリズムって，基本的には校正済みの２画像に対して視差を求めると思います．細かい話は以前ブログで書いた下記のエントリに書いてあるんですが，
もう一度簡単に復習しておくと，，，，
上のメモに書いたんですが，ステレオカメラの原理的な？特性は２つあって
１．マッチングの探索候補が常に同じ行上にある．
これは演算速度のために仕方がないことですが，ステレオカメラを使う場合事前に必死でキャリブレーションして，左右のカメラでとった映像が完全に平行になるようにします．ここでいう「平行」というのが，「マッチングの探索候補が常に同じ行上にある」ということでして，例えば右の画像の（30，50）にあるピクセルの一致点を探すときに，左の画像では同じ行上（つまり（x，50））の点を探索します．
２．観測可能な奥行きに限度がある．（奥行きが大きくなるに従って，精度が著しく下がる．）
メモに書いてあるとおり，奥行きは視差に反比例する形で大きくなっていくので，ある程度奥行きが大きくなってしまうと視差がでなくなってしまいます．下記２つの要素を調整することである程度遠方まで見れるようにはなるのですが，，，
- 基線長（左カメラと右カメラの距離）を長くする．
- カメラの画角を小さくする．
ただ，上記の２つは既成品のステレオカメラを買うと変更できず．．．．というところで，自作しないことにはちょっとつらいです．あと自動車の場合と違って，自律走行ロボットは割とうねうねと曲がりながら動くので，カメラのFOVを小さくすると横方向が殆どみえなくなってしまうという難点もあります．
ちなみに，自分がつくばチャレンジで使っているステレオカメラは下記のZedというカメラなんですが，このカメラで大体13m ~ 15mくらいまで見れます．（一応20mくらいまでは視差は出るのですが，ノイズとの切り分けが難しくなってきます．）この，「13m ~ 15m」ってとこがなかなか絶妙なところでして，屋外でロボットを走らせようと思うと，13m ~ 15m 周辺になにもモノがないエリアは無数に有ります．一方でLidarを使うと40mくらいまでビームが届くので，安定して自己位置推定できるようになります．
１．Plane Sweeping Stereoって？
で，「Plane Sweeping Stereo」ってナンジャラホイ？って話なんですが，校正していない複数の画像から視差を生成するためのアルゴリズムです．なんでこんなこと勉強してんの？って理由は２つ有りまして，，，
１．国際通りの三次元復元に SFM + MVS を使いたい．
SFMだけだと点群でしか復元できないので，MVSを使ってテクスチャを作ってあげればちょっとキャッチーになるかな？と思ってまして，それで勉強してます．
２．つくばチャレンジのロボットの自己位置推定用として．
「普通のステレオカメラ」の項目で，「ステレオの奥行きを伸ばすためには基線長を長くしてやればいい」と書いたのですが，Plane Sweep Stereo を使って一定距離ロボットが移動するたびにMVSを実行すれば奥行きが伸びるのでは？とおもって，こっちでも使えないかな．．．という淡い期待もあります．
２．参考文献
この「Plane Sweeping Stereo」，原理自体は結構昔（1996年）に考案されたみたいなんですが，VSLAMと組み合わせたりして若干ホットになりつつあるような気がしてます．
A Space-Sweep Approach to True Multi-Image Matching
原理を考案した論文です．必読です．
Real-time Plane-sweeping Stereo with Multiple Sweeping Directions
上の論文を実装して，Cudaを使うことでリアルタイムで回してます．
Real-Time Direct Dense Matching on Fisheye Images Using Plane-Sweeping Stereo
ピンホールモデルでなく魚眼モデルを直接使って，Plane-Sweeping Stereoを実施してます．画素の損失が少なくなったことで，３次元復元力が向上してます．
3D Modeling on the Go: Interactive 3D Reconstruction of Large-Scale Scenes on Mobile Devices
まだ詳しく読めてないですが，上の論文の原理を元に画素のマッチングを取る方法を工夫して精度の向上を図ってます．Project TangoっていうGoogleのプロジェクトがあったみたいなんですが，それの論文みたいですね．
で，最近気づいたんですけど，論文読んでて実装見たくなったら，「論文名＋Github」ってググるとでてきますね（笑）この実装も有りました！で，コード読むだけだと結局読み流してしまうので，リファクタ＋（写経？（笑））しました．
github.com
ということで，Plane Sweeping Stereoを解剖日記を「導入編」「理論編」「実装編」の３つに分けて残しておきたいと思います．
２画像間のホモグラフィー行列の計算
ということで，下記の魚眼カメラのキャリブレーションエントリからの「スピンオフ？」としてホモグラフィー行列の計算エントリを書き残します．
０．ホモグラフィー行列とは何か？
平面を撮影した２つの画像は射影変換と呼ばれる関係で結ばれます．って，これだけ書くと意味わかんないんで図を書いてみます．
３年前に画像処理を勉強し始めて本を読んだ時には，「で，それがなんなの？」と思ってたんですが，これって結構役に立ちます．大事なことは上述の「ホモグラフィー行列Hがわかっていれば，カメラAに写っているキャリブボードと同一平面上にある点のカメラBでの座標を計算できる」というところで，自動車に搭載されているアラウンドビューモニタなんかもこれを使ってます．（ちなみに，”同一平面に”というのがポイントで，同一平面上にない点は変なところに変換されます．）
日産HPのリンクを貼ってますが，この例だとカメラAが車両に搭載されているカメラで，カメラBが地面を真上から見ているカメラです．この場合にはカメラBはもちろん実際には存在しないので，カメラAとカメラBの位置関係から事前にホモグラフィー行列Hを計算しておいて，Hを使ってカメラAのビューを変換して車内に写してます．
１．ホモグラフィー行列の計算方法
で，どうやってHを計算すんのか？って話なんですが，毎度のことながらたくさんの線形方程式を解いて求めます．Hを求めるときにはカメラAで撮影した点の画像座標と，それに対応するカメラBで撮影した点の画像座標が必要になります．（つまり，上図のキャリブボード上の赤点のそれぞれの写真内での座標．）
ということで，スラスラーと書いてみたんですが，スラスラーと頭から出てきたわけではなく本にかじりついてます（笑）．ホモグラフィー行列をかけた後に，ベクトルの外積が０になるところから方程式を導き出して．．．みたいなとこって数学得意な人ならすぐにおもいつくんですかね．ひとまず，上の導出でよく見る方程式の形 ”Ah = 0” が出てきました．これをもう少し具体的に書き下すと．．．
で，最後に書き下した数式なんですが，XB = [XB, YB, ZB] の z 座標であるZBでAを割ってあげても方程式が満たされます．なので，計算の単純化のためにXB = [XB / ZB, YB / ZB, 1] とします．こうすることで，取得した点 XB を正規化画像座標として扱います．
次に Ah = 0 の解である h を求めますが，求める解 h には定数倍の不定性があるので h33 = 1とするなり，|h| = 1 とするなりして適当に制約条件をつけてあげます．こうすることで，最終的に方程式は下記のようになります．
で，最後の方に Ah = 0 の最終的な形を書いていると思うんですが，４点あったらホモグラフィー行列自体は求まりますが，画像Aと画像Bの対応点自体に誤差が含まれるので実際にはキャリブレーションの時みたいにもっとたくさんの点を入力します．この点を元に行列式 A を作って，後は入力点に対して Ah がもっとも小さくなるような h を特異値分解で求めます．
実装（線形方程式を解いてホモグラフィー行列を求める．）
下記，実際の C++ での実装です．
Mat ComputeHomographyViaDLT(const Mat& pnt_to_be_projected, const Mat& pnt_destnation) { LOG(INFO) << "Inside \"ComputeHomographyViaDLT\""; // Build linear system L. int point_num = pnt_to_be_projected.cols; Mat L = Mat::zeros(2 * point_num, 9, CV_64FC1); for (int i = 0; i < point_num; ++i) { for (int j = 0; j < 3; j++) { L.at<double>(2 * i, j + 3) = pnt_to_be_projected.at<double>(j, i); L.at<double>(2 * i, j + 6) = -pnt_destnation.at<double>(1, i) * pnt_to_be_projected.at<double>(j, i); L.at<double>(2 * i + 1, j) = pnt_to_be_projected.at<double>(j, i); L.at<double>(2 * i + 1, j + 6) = -pnt_destnation.at<double>(0, i) * pnt_to_be_projected.at<double>(j, i); } } // Solve H via SVD. Mat H; { if (point_num > 4) { L = L.t() * L; } SVD svd(L); Mat h_vector = svd.vt.row(8) / svd.vt.row(8).at<double>(8); H = h_vector.reshape(1, 3); } return H; }
２．再投影誤差の最小化
１のホモグラフィー行列の計算でホモグラフィー行列自体は求まったのですが，線形の方程式を解いてあげるだけだとまだ投影誤差が少し大きいです．なので，非線形最適化を使って最終仕上げをします．
再投影誤差
で，最適化をするには誤差関数を作ってあげないといけないですが，今回の場合は下記のようになります．
ヤコビアンの計算
ヤコビアンの計算ですが，今回の最適化では変動成分が H の各要素になります．これを一方で誤差関数の各要素 (ek) は個々のベクトル要素の差分になります． なので，これに対してヤコビアンを計算します．
で，上記の最後で誤差関数の各要素 ek が出てきましたが，結局ヤコビアンの要素は写像後の座標 xb, yb のそれぞれをホモグラフィー行列の要素で微分したものになります．
実装（ガウスニュートン法によるホモグラフィー行列の最適化）
void RefineHomographyViaGaussNewton(const Mat& pnt_to_be_projected, const Mat& pnt_destnation, const int iteration_num, Mat& H) { LOG(INFO) << "Inside \"RefineHomographyViaGaussNewton\""; int point_num = pnt_to_be_projected.cols; if (point_num > 4) { for (int iter = 0; iter < iteration_num; iter++) { Mat m_proj = H * pnt_to_be_projected; Mat m_reproj_err_vec, m_proj_normalized; double reproj_err; // Calculate Reprojection Error. { // Scale Adjustment. Dividing both side of eqn by m_proj.z. divide(m_proj, Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 2, m_proj.cols, 1)), m_proj_normalized); m_reproj_err_vec = m_proj_normalized(Rect(0, 0, m_proj_normalized.cols, 2)) - pnt_destnation(Rect(0, 0, pnt_destnation.cols, 2)); m_reproj_err_vec = Mat(m_reproj_err_vec.t()) .reshape(1, m_reproj_err_vec.cols * m_reproj_err_vec.rows); reproj_err = cv::norm(m_reproj_err_vec) / (double)point_num; } // Create Jacobian. Mat J = Mat::zeros(2 * point_num, 8, CV_64FC1); { Mat m_proj_z = m_proj(Rect(0, 2, m_proj.cols, 1)); Mat m_proj_z_2; multiply(m_proj_z, m_proj_z, m_proj_z_2); Mat MMM, MMM2, MMM3; divide(pnt_to_be_projected, Mat::ones(3, 1, CV_64FC1) * m_proj_z, MMM); multiply(Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 0, m_proj.cols, 1)), pnt_to_be_projected, MMM2); divide(MMM2, Mat::ones(3, 1, CV_64FC1) * m_proj_z_2, MMM2); multiply(Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 1, m_proj.cols, 1)), pnt_to_be_projected, MMM3); divide(MMM3, Mat::ones(3, 1, CV_64FC1) * m_proj_z_2, MMM3); for (int i = 0; i < point_num; ++i) { for (int j = 0; j < 3; ++j) { J.at<double>(2 * i, j) = MMM.at<double>(j, i); J.at<double>(2 * i + 1, j + 3) = MMM.at<double>(j, i); } for (int j = 0; j < 2; ++j) { J.at<double>(2 * i, j + 6) = -MMM2.at<double>(j, i); J.at<double>(2 * i + 1, j + 6) = -MMM3.at<double>(j, i); } } } // Update Homography Matrix H. { Mat h_vector = H.reshape(1, 9)(Rect(0, 0, 1, 8)).clone(); Mat dh_vec = -(J.t() * J).inv() * (J.t()) * m_reproj_err_vec; h_vector = h_vector + dh_vec; Mat tmp; vconcat(h_vector, Mat::ones(1, 1, CV_64FC1), tmp); H = tmp.reshape(1, 3); } LOG(INFO) << "Number of iteration : " << iter << ", Reproj Error : " << reproj_err << std::endl; } } }
実験結果
うーん．ちょっとDLTで求めたホモグラフィー行列の誤差がとても大きいです．ただ，最適化を実行すればすぐに誤差は収束したのですが，ひょっとするとDLTのあたりにバグがあるかもです．最終的な結果としては，，，
オリジナル画像１（この画像が変換する画像です．）
オリジナル画像２（この画像が変換が目指すべき画像です．）
最終的な全実装
結構気合を入れてエントリを作ったので，一式Githubにも上げておきます．ただ，１つのファイルでほぼすべてが完結するように書いてあるので，時間があればライブラリのリンクを通してあげれば下記でも動くかも．．．
/*M///////////////////////////////////////////////////////////////////////////// // // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. // // By downloading, copying, installing or using the software you agree to this // license. If you do not agree to this license, do not download, install, // copy or use the software. // // // License Agreement // For Open Source Computer Vision Library // // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. // Third party copyrights are property of their respective owners. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // // * Redistribution's of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // // * Redistribution's in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // * The name of the copyright holders may not be used to endorse or promote // products derived from this software without specific prior written // permission. // // This software is provided by the copyright holders and contributors "as is" // and any express or implied warranties, including, but not limited to, the // implied warranties of merchantability and fitness for a particular purpose // are disclaimed. In no event shall the Intel Corporation or contributors be // liable for any direct, indirect, incidental, special, exemplary, or // consequential damages (including, but not limited to, procurement of // substitute goods or services; // loss of use, data, or profits; or business interruption) however caused // and on any theory of liability, whether in contract, strict liability, // or tort (including negligence or otherwise) arising in any way out of // the use of this software, even if advised of the possibility of such damage. // //M*/ // System #include <algorithm> #include <fstream> #include <iostream> #include <vector> // Glog #include <glog/logging.h> // Gflag #include <gflags/gflags.h> // Boost #include <boost/algorithm/string.hpp> // OpenCV #include <opencv2/calib3d.hpp> #include <opencv2/core.hpp> #include <opencv2/highgui.hpp> #include <opencv2/imgproc.hpp> using namespace cv; namespace { void CreateHomogeneousPoints(const Mat& m, Mat& m_homo) { int point_num = m.cols; if (m.rows < 3) { vconcat(m, Mat::ones(1, point_num, CV_64FC1), m_homo); } else { m_homo = m.clone(); } divide(m_homo, Mat::ones(3, 1, CV_64FC1) * m_homo.row(2), m_homo); } void NormalizePoints(const Mat& pnt_homo, Mat& pnt_normalized, Mat& norm_mat, Mat& inv_norm_mat) { { Mat mx_row = pnt_homo.row(0).clone(); Mat my_row = pnt_homo.row(1).clone(); double mx_mean = mean(mx_row)[0]; double my_mean = mean(my_row)[0]; Mat normalized_mx_row = mx_row - mx_mean; Mat normalized_my_row = my_row - my_mean; double x_scale = mean(abs(normalized_mx_row))[0]; double y_scale = mean(abs(normalized_my_row))[0]; norm_mat = cv::Mat(Matx33d(1 / x_scale, 0.0, -mx_mean / x_scale, 0.0, 1 / y_scale, -my_mean / y_scale, 0.0, 0.0, 1.0)); inv_norm_mat = Mat(Matx33d(x_scale, 0, mx_mean, 0, y_scale, my_mean, 0, 0, 1)); pnt_normalized = norm_mat * pnt_homo; } } Mat ComputeHomographyViaDLT(const Mat& pnt_to_be_projected, const Mat& pnt_destnation) { LOG(INFO) << "Inside \"ComputeHomographyViaDLT\""; // Build linear system L. int point_num = pnt_to_be_projected.cols; Mat L = Mat::zeros(2 * point_num, 9, CV_64FC1); for (int i = 0; i < point_num; ++i) { for (int j = 0; j < 3; j++) { L.at<double>(2 * i, j + 3) = pnt_to_be_projected.at<double>(j, i); L.at<double>(2 * i, j + 6) = -pnt_destnation.at<double>(1, i) * pnt_to_be_projected.at<double>(j, i); L.at<double>(2 * i + 1, j) = pnt_to_be_projected.at<double>(j, i); L.at<double>(2 * i + 1, j + 6) = -pnt_destnation.at<double>(0, i) * pnt_to_be_projected.at<double>(j, i); } } // Solve H via SVD. Mat H; { if (point_num > 4) { L = L.t() * L; } SVD svd(L); Mat h_vector = svd.vt.row(8) / svd.vt.row(8).at<double>(8); H = h_vector.reshape(1, 3); } return H; } void RefineHomographyViaGaussNewton(const Mat& pnt_to_be_projected, const Mat& pnt_destnation, const int iteration_num, Mat& H) { LOG(INFO) << "Inside \"RefineHomographyViaGaussNewton\""; int point_num = pnt_to_be_projected.cols; if (point_num > 4) { for (int iter = 0; iter < iteration_num; iter++) { Mat m_proj = H * pnt_to_be_projected; Mat m_reproj_err_vec, m_proj_normalized; double reproj_err; // Calculate Reprojection Error. { // Scale Adjustment. Dividing both side of eqn by m_proj.z. divide(m_proj, Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 2, m_proj.cols, 1)), m_proj_normalized); m_reproj_err_vec = m_proj_normalized(Rect(0, 0, m_proj_normalized.cols, 2)) - pnt_destnation(Rect(0, 0, pnt_destnation.cols, 2)); m_reproj_err_vec = Mat(m_reproj_err_vec.t()) .reshape(1, m_reproj_err_vec.cols * m_reproj_err_vec.rows); reproj_err = cv::norm(m_reproj_err_vec) / (double)point_num; } // Create Jacobian. Mat J = Mat::zeros(2 * point_num, 8, CV_64FC1); { Mat m_proj_z = m_proj(Rect(0, 2, m_proj.cols, 1)); Mat m_proj_z_2; multiply(m_proj_z, m_proj_z, m_proj_z_2); Mat MMM, MMM2, MMM3; divide(pnt_to_be_projected, Mat::ones(3, 1, CV_64FC1) * m_proj_z, MMM); multiply(Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 0, m_proj.cols, 1)), pnt_to_be_projected, MMM2); divide(MMM2, Mat::ones(3, 1, CV_64FC1) * m_proj_z_2, MMM2); multiply(Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 1, m_proj.cols, 1)), pnt_to_be_projected, MMM3); divide(MMM3, Mat::ones(3, 1, CV_64FC1) * m_proj_z_2, MMM3); for (int i = 0; i < point_num; ++i) { for (int j = 0; j < 3; ++j) { J.at<double>(2 * i, j) = MMM.at<double>(j, i); J.at<double>(2 * i + 1, j + 3) = MMM.at<double>(j, i); } for (int j = 0; j < 2; ++j) { J.at<double>(2 * i, j + 6) = -MMM2.at<double>(j, i); J.at<double>(2 * i + 1, j + 6) = -MMM3.at<double>(j, i); } } } // Update Homography Matrix H. { Mat h_vector = H.reshape(1, 9)(Rect(0, 0, 1, 8)).clone(); Mat dh_vec = -(J.t() * J).inv() * (J.t()) * m_reproj_err_vec; h_vector = h_vector + dh_vec; Mat tmp; vconcat(h_vector, Mat::ones(1, 1, CV_64FC1), tmp); H = tmp.reshape(1, 3); } LOG(INFO) << "Number of iteration : " << iter << ", Reproj Error : " << reproj_err << std::endl; } } } cv::Mat ComputeHomography(const Mat& m, const Mat& M, bool run_optimization, const int iteration_num) { // Convert input to homogeneous Mat. Mat m_homo, M_homo; CreateHomogeneousPoints(m, m_homo); CreateHomogeneousPoints(M, M_homo); // Calculation down here will be done based on normalized coord. Mat H_m_to_M; { // Normalize x and y coordinate to make homogeneous. Mat m_normalized, m_norm_mat, inv_m_norm_mat; NormalizePoints(m_homo, m_normalized, m_norm_mat, inv_m_norm_mat); Mat M_normalized, M_norm_mat, inv_M_norm_mat; NormalizePoints(M_homo, M_normalized, M_norm_mat, inv_M_norm_mat); // Compute initial homography matrix. Mat H_m_to_M_normalized = ComputeHomographyViaDLT(m_normalized, M_normalized); H_m_to_M = inv_m_norm_mat * H_m_to_M_normalized * M_norm_mat; H_m_to_M = H_m_to_M / H_m_to_M.at<double>(2, 2); { cv::Mat m_reproj; cv::Mat m_proj = H_m_to_M * m_homo; divide(m_proj, Mat::ones(3, 1, CV_64FC1) * m_proj(Rect(0, 2, m_proj.cols, 1)), m_proj); LOG(INFO) << "After DLT Cauclation. Reproj Error : " << cv::norm(M_homo.reshape(1, 1) - m_proj.reshape(1, 1)) / (double)m_proj.cols; } } if (run_optimization) { // Refine homography matrix via optimization. RefineHomographyViaGaussNewton(m_homo, M_homo, iteration_num, H_m_to_M); } return H_m_to_M; } void LoadDetectPointsFromFile(const std::string& path, std::vector<cv::Point2d>& detected_points) { detected_points.clear(); std::ifstream reading_file(path, std::ios::in); CHECK(reading_file.is_open()) << "File at given path can not be opened." << path; while (!reading_file.eof()) { std::string line; std::getline(reading_file, line); std::vector<std::string> results; boost::split(results, line, [](char c) { return c == ','; }); if (results.size() != 2) { break; } detected_points.push_back( cv::Point2d(std::stod(results[0]), std::stod(results[1]))); } } void ConvertPoint2dToPoint2fVector(const std::vector<cv::Point2d>& src, std::vector<cv::Point2f>& dst) { dst.clear(); for (auto elem : src) { dst.push_back(cv::Point2f(elem.x, elem.y)); } } void ApplyHomographyMatToPoint2fVector(const std::vector<cv::Point2f>& src, std::vector<cv::Point2f>& dst, cv::Mat& H) { dst.clear(); for (auto elem : src) { cv::Mat pnt = (cv::Mat_<double>(3, 1) << elem.x, elem.y, 1.0); cv::Mat trans = H * pnt; dst.push_back(cv::Point2f(trans.at<double>(0, 0) / trans.at<double>(2, 0), trans.at<double>(1, 0) / trans.at<double>(2, 0))); } } } // namespace DEFINE_string(picture_path1, "/home/koichi/Desktop/Blog/Pic20190616/raw/DSC02719.jpg", "Path to the source picture."); DEFINE_string(picture_path2, "/home/koichi/Desktop/Blog/Pic20190616/raw/DSC02721.jpg", "Path to the source picture."); DEFINE_string(detection_result1, "/home/koichi/Desktop/Blog/Pic20190616/raw/DSC02719.txt", "Path to the detected corner location of image 1."); DEFINE_string(detection_result2, "/home/koichi/Desktop/Blog/Pic20190616/raw/DSC02721.txt", "Path to the detected corner location of image 2."); int main(int argc, char** argv) { google::ParseCommandLineFlags(&argc, &argv, true); FLAGS_alsologtostderr = 1; FLAGS_stderrthreshold = google::GLOG_INFO; google::InitGoogleLogging(argv[0]); // Read Picture. cv::Mat img1, img2; { img1 = cv::imread(FLAGS_picture_path1, CV_LOAD_IMAGE_COLOR); CHECK(!img1.empty()) << "Image 1 cannot be opened. " << FLAGS_picture_path1; img2 = cv::imread(FLAGS_picture_path2, CV_LOAD_IMAGE_COLOR); CHECK(!img2.empty()) << "Image 2 cannot be opened. " << FLAGS_picture_path2; } // Read std::vector<cv::Point2d> detected_point1, detected_point2; { LoadDetectPointsFromFile(FLAGS_detection_result1, detected_point1); LoadDetectPointsFromFile(FLAGS_detection_result2, detected_point2); } // Convert vector<cv::Point2f> to Mat cv::Mat point1_1ch, point2_1ch; { cv::Mat point1 = cv::Mat(detected_point1); cv::Mat temp[2]; cv::split(point1, temp); point1_1ch = cv::Mat::zeros(cv::Size(point1.rows, 2), CV_64FC1); point1_1ch.row(0) += temp[0].t(); point1_1ch.row(1) += temp[1].t(); cv::Mat point2 = cv::Mat(detected_point2); cv::split(point2, temp); point2_1ch = cv::Mat::zeros(cv::Size(point2.rows, 2), CV_64FC1); point2_1ch.row(0) += temp[0].t(); point2_1ch.row(1) += temp[1].t(); } // Compute homography. cv::Mat H_1_to_2_wo_opt, H_1_to_2_w_opt; { H_1_to_2_wo_opt = ComputeHomography(point1_1ch, point2_1ch, false, 0); H_1_to_2_w_opt = ComputeHomography(point1_1ch, point2_1ch, true, 50); LOG(INFO) << "H_1_to_2_wo_opt : " << H_1_to_2_wo_opt; LOG(INFO) << "H_1_to_2_w_opt : " << H_1_to_2_w_opt; } // Calculate Homography { cv::Mat warped_img_wo_opt, warped_img_w_opt; cv::Mat img2_with_corner = img2.clone(); cv::Mat img2_with_corner_wo_opt = img2.clone(); cv::Mat img2_with_corner_w_opt = img2.clone(); cv::warpPerspective(img1, warped_img_wo_opt, H_1_to_2_wo_opt, cv::Size(img1.cols, img1.rows)); cv::warpPerspective(img1, warped_img_w_opt, H_1_to_2_w_opt, cv::Size(img1.cols, img1.rows)); std::vector<cv::Point2f> detected_point1f, detected_point2f, warped_point2f_wo_opt, warped_point2f_w_opt; ConvertPoint2dToPoint2fVector(detected_point2, detected_point2f); cv::drawChessboardCorners(img2_with_corner, cv::Size(7, 10), detected_point2f, true); ConvertPoint2dToPoint2fVector(detected_point1, detected_point1f); ApplyHomographyMatToPoint2fVector(detected_point1f, warped_point2f_wo_opt, H_1_to_2_wo_opt); cv::drawChessboardCorners(img2_with_corner_wo_opt, cv::Size(7, 10), warped_point2f_wo_opt, true); ApplyHomographyMatToPoint2fVector(detected_point1f, warped_point2f_w_opt, H_1_to_2_w_opt); cv::drawChessboardCorners(img2_with_corner_w_opt, cv::Size(7, 10), warped_point2f_w_opt, true); cv::imshow("Image 1", img1); cv::imshow("Without Optimization", warped_img_wo_opt); cv::imshow("With Optimization", warped_img_w_opt); cv::imshow("Image 2 with DETECTED CORNER", img2_with_corner); cv::imshow("Image 2 with WARPED CORNER WITHOUT optimization", img2_with_corner_wo_opt); cv::imshow("Image 2 with WARPED CORNER WITH optimization", img2_with_corner_w_opt); cv::waitKey(0); } return 0; }
Insta 360 One X
ここ何回かのエントリで書いている魚眼レンズキャリブレーションの件なんですが，Insta 360 One X っていう 360°カメラをキャリブレーションするのに使いました．国際通りの三次元復元に向けて以前購入した Sony の α7 2 で撮影しようとてたんですが，画角が狭く撮影が終わらないのと，ガジェット欲しさに買ってしまいました．
Appleのウェブサイトから購入．
いつもの如くAmazonでポチッとしかけていたんですが，AppleのWebサイトでも販売していることを発見．自分が購入した時はAppleのサイトが一番安かったです．この値段でバッテリーが二個，SDカード32GB，撮影用の自撮り棒もついてます．
www.apple.com
出力ファイルフォーマット
映像・ビデオともにDual Fisheyeの形で出てきます．で，フォーマットに関しては，
写真
・RAW撮影オプションON：”.dng”というアドビのイメージフォーマット，".insp" という Insta360のイメージフォーマットの2つのファイルが出力されます．
・RAW撮影オプションOFF：”.insp” というフォーマットのみ出てきます．
ビデオ
・LOG撮影オプションON/OFFともに：".insv" というフォーマットで出てきます．
".insp" にしても，".insv" にしても，拡張子を ".insp" -> ".jpg", ".insv" -> ".mp4" にしてあげれば普通のソフトで見ることができました．が，ビデオに関しては流石に写真よりはだいぶ画質が落ちていると思います．あと，UVCデバイスとしてロボコンで使えないかな〜とちょっと淡い期待を抱いていたのですが，USB経由で画像を送信することはできなさそうでした．USBのストレージとしてしか認識されてないように思います．
uvcdynctrl --list
で確認しましたが，認識されてませんでした．
撮影画像
自分は国際通りを撮影することが目的だったので魚眼のままの画像が必要だったんですが，前述の通り，写真もビデオも魚眼の映像のまま出てきます．おそらくカメラでは撮影しているだけで，その後の処理はPCやスマホですることを前提としているのだと思います．下の写真が編集ソフトです．
撮影した写真
下記，Insta 360 One X で撮影した写真をそのまま載せてます．これをPCで事後処理して全天球画像に変換します．
本当はビデオを撮影していたら You Tube にアップして 360° ビデオみたいにできたんですが，ちょっと手持ちが写真だけだったので．．．．Insta 360 Studio を使えば，自由に視点変換した写真を作ることができます．
画角の比較
Sony α7 2 に付属していたレンズと Insta でとった写真の比較です．Insta でとった写真は画角が 180°以上有ります．α2 は一眼レフなんで，「広角レンズ買えばええやん？」と言われればそれはそのとおりなんですけども（笑）
Insta360 One X で撮影した写真
手持ちカメラ with デフォルトレンズで撮影した写真
.dng ファイルの変換コード
で，せっかく Raw の画像ファイルを出力してくれるので， Raw のファイルを読み込んでくれるライブラリを探してたんですが，有りました．
www.libraw.org
Ubuntu のパッケージとしてインストールして使えます．
sudo apt install libraw-dev
下記，.dng ファイルをlibraw でデコードして jpg で保存します．1つの .dng ファイルに2つの魚眼カメラの画像が含まれているので，分割してます．
// Standard #include <iostream> #include <string> // Boost #include <boost/filesystem.hpp> // Gflags #include <gflags/gflags.h> // Glog #include <glog/logging.h> // OpenCV #include <opencv2/core.hpp> #include <opencv2/highgui.hpp> #include <opencv2/imgproc.hpp> // LibRaw #include <libraw/libraw.h> // Original #include <fileutil/filesystem_util.h> DEFINE_string(picture_input_dir, "", "Path to the directory containing the insta \".dng\" files."); DEFINE_string(picture_output_dir, "", "Output directory for converted \".jpg\" files"); using namespace std; namespace { bool ConvertDngFileToJpgFile(const std::string &path, const std::string &save_dir_path) { LibRaw raw_processor; // Setting raw_processor.imgdata.params.use_camera_matrix = 1; raw_processor.imgdata.params.use_camera_wb = 1; CHECK(raw_processor.open_file(path.c_str()) == LIBRAW_SUCCESS) << "LibRaw can not open file. : " << path; CHECK(fileutil::CheckDirectory(save_dir_path)) << "Directory specified for output is not valid. : " << save_dir_path; raw_processor.unpack(); raw_processor.dcraw_process(); libraw_processed_image_t *img = raw_processor.dcraw_make_mem_image(); cv::Mat insta_img(raw_processor.imgdata.sizes.height, raw_processor.imgdata.sizes.width, CV_8UC3, img->data); cv::Mat insta_rgb; cv::cvtColor(insta_img, insta_rgb, cv::COLOR_RGB2BGR); int width = insta_img.cols, height = insta_img.rows / 2; int lower_start_row = height; cv::Mat insta_rgb_upper(insta_rgb, cv::Rect(0, 0, width, height)); cv::Mat insta_rgb_lower(insta_rgb, cv::Rect(0, lower_start_row, width, height)); std::string filename; bool result = fileutil::ExtractFilename(path, filename); CHECK(result) << "Invalid file name."; size_t n = filename.rfind(".dng"); std::string save_filename_f = filename + " "; std::string save_filename_b = filename + " "; save_filename_f.replace(n, 6, "_f.jpg"); save_filename_b.replace(n, 6, "_b.jpg"); std::string up_path = save_dir_path + "/" + save_filename_f; std::string low_path = save_dir_path + "/" + save_filename_b; vector<int> compression_params; compression_params.push_back(CV_IMWRITE_JPEG_QUALITY); compression_params.push_back(100); cv::imwrite(up_path, insta_rgb_upper, compression_params); cv::imwrite(low_path, insta_rgb_lower, compression_params); return true; } } // namespace int main(int argc, char **argv) { google::ParseCommandLineFlags(&argc, &argv, true); FLAGS_alsologtostderr = 1; google::InitGoogleLogging(argv[0]); CHECK_NE(FLAGS_picture_input_dir, "") << "Please specify the directory containing the .dng files."; CHECK(fileutil::CheckDirectory(FLAGS_picture_input_dir)) << "Given input directory is not valid!"; std::string output_dir = FLAGS_picture_output_dir != "" ? FLAGS_picture_output_dir : FLAGS_picture_input_dir + "/../converted"; CHECK(fileutil::CreateDirectoryIfNotExist(output_dir)) << "Output directory is not valid or able to be created."; vector<string> dng_filelist; LOG(INFO) << "Collecting all \".dng\" files in the specified directory..."; fileutil::RaiseAllFilesInDirectory(FLAGS_picture_input_dir, dng_filelist, vector<string>{".dng"}); LOG(INFO) << "Converting \".dng\" file to \".jpg\" files. Total files to " "convert is : " << dng_filelist.size(); for (size_t idx = 0; idx < dng_filelist.size(); idx++) { if (idx % 10 == 0) { LOG(INFO) << "Processing : " << to_string(idx) << " / " << dng_filelist.size(); } ConvertDngFileToJpgFile(dng_filelist[idx], output_dir); } return 0; }
ということで，明日は久々の晴れっぽいので，撮影してきます(ﾉ´∀｀*)
魚眼カメラのキャリブレーション 〜 実装編１（ホモグラフィー行列と外部パラメータの計算）
ここでは，ホモグラフィー行列の計算と外部パラメータの計算を行っている下記の関数を見ていきます．（実装編０のコードにあった，Step2の CalibrateExtrinsicsが該当します．）
０．処理の流れ
具体的にやっていることとしては，キャリブボード上の点と画像に写っている点の対応関係からキャリブボードの回転・位置を計算します．この関数では，キャリブボードを撮影した一つ一つの写真が個別に処理されていきます．大きな処理の流れとしては，下記のようになっています．
１．ConvertVector2dToMat
ただのフォーマット変換なので，飛ばします．
２．ComputeExtrinsicsBasedOnHomography
ホモグラフィー行列を計算し，それを元に外部パラメータを計算します．多分全体のキモです．
該当部コード
下記，コードです．
void CalibrateExtrinsics(const vector<vector<cv::Point3f>>& objectPoints, const vector<vector<cv::Point2f>>& imagePoints, const IntrinsicParams& param, const int check_cond, const double thresh_cond, vector<cv::Vec3d>& board_rotations, vector<cv::Vec3d>& board_translations) { CHECK(objectPoints.size() == imagePoints.size()); CHECK(!objectPoints.empty() && !imagePoints.empty()); int image_num = imagePoints.size(); board_rotations.clear(); board_rotations.resize(image_num); board_translations.clear(); board_translations.resize(image_num); for (int image_idx = 0; image_idx < image_num; ++image_idx) { Mat board_rot, board_trans, jacobi_extrinsic; // Convert vector structure to cv:Mat structure. cv::Mat object_points_mat, image_points_mat; ConvertVector2dToMat(objectPoints[image_idx], imagePoints[image_idx], object_points_mat, image_points_mat); // Compute Extrisics Based on Homography. ComputeExtrinsicsBasedOnHomography(image_points_mat, object_points_mat, param, board_rot, board_trans); // Refine Extrinsics calculated in the above function. RefineExtrinsicsViaOptimization(image_points_mat, object_points_mat, board_rot, board_trans, jacobi_extrinsic, param, thresh_cond); // Check condition number for convergence. if (check_cond) { SVD svd(jacobi_extrinsic, SVD::NO_UV); if (svd.w.at<double>(0) / svd.w.at<double>((int)svd.w.total() - 1) > thresh_cond) { CV_Error(cv::Error::StsInternal, format("CALIB_CHECK_COND - Ill-conditioned " "matrix for input array %d", image_idx)); } } board_rot.copyTo(board_rotations[image_idx]); board_trans.copyTo(board_translations[image_idx]); } }
このエントリでは，ComputeExtrinsicsBasedOnHomographyを説明していきます．
１．ComputeExtrinsicsBasedOnHomographyの処理
おそらくキャリブレーション全体のロジックのキモになると思います．
１．TransformToNormalizedFrame
ホモグラフィー行列を実際に計算するにあたって，数値計算上の観点で正規化してます．ここも読んでいく中で初めて知ったんですが，点群位置の共分散をとって特異値分解することで正規化座標系をもとめることができるんですね．サラッと実装されてたんで調べるの大変でしたが．．．
２．ComputeHomography
ホモグラフィー行列を計算します．この関数内でも最適化計算をしてまして，，，ちょっと理解するのに時間がかかったんですが，ちょっとこのエントリに書くには大きいので，別のエントリを作ってリンクをここに持ってきます．
３．外部パラメータの計算部分
論文の主要部分です．
４．正規化した座標系からカメラ座標系への引き戻し
board_rot, board_transはカメラ座標系から見た書くキャリブボードの座標系でなければいけないので，座標を戻します．
該当部コード
void ComputeExtrinsicsBasedOnHomography(const Mat& image_points_mat, const Mat& object_points_mat, const IntrinsicParams& param, Mat& board_rot, Mat& board_trans) { CV_Assert(!object_points_mat.empty() && object_points_mat.type() == CV_64FC3); CV_Assert(!image_points_mat.empty() && image_points_mat.type() == CV_64FC2); // Transform points to the normalized coordinate on calibration board. Mat object_points_in_normalized_frame, rep_rot, rep_trans; TransformToNormalizedFrame( object_points_mat, object_points_in_normalized_frame, rep_rot, rep_trans); // Compute Homography. Mat H; { Mat imagePointsNormalized = NormalizePixels(image_points_mat, param).reshape(1).t(); H = ComputeHomography( imagePointsNormalized, object_points_in_normalized_frame( Rect(0, 0, object_points_in_normalized_frame.cols, 2))); // Norm of basis vector should be one. (r1, r2) double scale = .5 * (norm(H.col(0)) + norm(H.col(1))); H = H / scale; } // Axis and Rotation Matrix generation. cv::Mat rep_rot_in_cam; { cv::Mat u1, u2, u3; u1 = H.col(0) / norm(H.col(0)); u2 = H.col(1).clone() - u1.dot(H.col(1).clone()) * u1; u2 = u2 / norm(u2); u3 = u1.cross(u2); hconcat(u1, u2, rep_rot_in_cam); hconcat(rep_rot_in_cam, u3, rep_rot_in_cam); } // Compute calibration origin from representative origin. { cv::Mat rep_trans_in_cam = H.col(2).clone(); board_trans = rep_rot_in_cam * rep_rot.t() * (-rep_trans) + rep_trans_in_cam; cv::Mat board_rot_mat = rep_rot_in_cam * rep_rot.t(); Rodrigues(board_rot_mat, board_rot); } }
２．TransformToNormalizedFrameの処理
キャリブボード上の点座標を正規化しています．DLT法でホモグラフィー，基礎，基本行列を求める際のノウハウだと思うのですが，数値計算の誤差を抑えるために，点の重心を座標系の中心に持ってくるようにします．下記，点群座標の平均と共分散を求めて，共分散行列を特異値分解して回転行列を求めているんですが，このテクニックは知りませんでした．
該当部コード
void TransformToNormalizedFrame(const Mat& object_points_mat, Mat& normalized_object_points_mat, cv::Mat& rep_rot, cv::Mat& rep_trans) { // Compute COG and Angle via SVD. { Mat objectPoints = object_points_mat.reshape(1).t(); Mat objectPointsMean, covObjectPoints; calcCovarMatrix(objectPoints, covObjectPoints, objectPointsMean, COVAR_NORMAL | COVAR_COLS); SVD svd(covObjectPoints); rep_rot = svd.vt; if (norm(rep_rot(Rect(2, 0, 1, 2))) < 1e-6) { rep_rot = Mat::eye(3, 3, CV_64FC1); } if (determinant(rep_rot) < 0) { rep_rot = -rep_rot; } // rep_trans = rep_rot * objectPointsMean; rep_trans = objectPointsMean; int Np = objectPoints.cols; // Subtract translation from origin to normalize points. normalized_object_points_mat = rep_rot.t() * objectPoints + (rep_rot.t() * -rep_trans) * Mat::ones(1, Np, CV_64FC1); } }
３．ComputeHomography
Homography のトピックは大きいので，別のトピックに切り出しました．下記を参照あれ！
daily-tech.hatenablog.com
４．外部パラメータの計算部分
論文の主要部分ですが，キャリブボード上の点のZ座標が０であることを活かして，ホモグラフィー行列から外部パラメータを求めてます．u3 は u1 と u2 の外積から計算してます．
// Axis and Rotation Matrix generation. cv::Mat rep_rot_in_cam; { cv::Mat u1, u2, u3; u1 = H.col(0) / norm(H.col(0)); u2 = H.col(1).clone() - u1.dot(H.col(1).clone()) * u1; u2 = u2 / norm(u2); u3 = u1.cross(u2); hconcat(u1, u2, rep_rot_in_cam); hconcat(rep_rot_in_cam, u3, rep_rot_in_cam); }
５．正規化した座標系からカメラ座標系への引き戻し
数値計算上都合のいい座標系から，カメラ座標系から見た座標系に戻してあげます．参考までに座標系のイメージを書いておきます．ホモグラフィーの計算とか重要なことはすべて正規化座標で実施して，最後に引き戻してます．
// Compute calibration origin from representative origin. { cv::Mat rep_trans_in_cam = H.col(2).clone(); board_trans = rep_rot_in_cam * rep_rot.t() * (-rep_trans) + rep_trans_in_cam; cv::Mat board_rot_mat = rep_rot_in_cam * rep_rot.t(); Rodrigues(board_rot_mat, board_rot); }
ということで，次は最適化の部分です！なんだか，思っていたよりとっても読みにくいエントリになってしまいました．．．魚眼のキャリブで困った人がいた時の助けになれば．．．
リファクタしたコードは下記のGithubにあげておきます．もちろん本家はOpenCVなので，あくまでも流れをつかむ用途でしか使えないですが．．．
github.com
魚眼カメラのキャリブレーション 〜 実装編０（全体構成）
前回までの理論編に引き続き，実装編に入っていきます．
ちなみに，「沖縄三次元復元プロジェクト！」をやっていく中で，オープンソースのSFMソフトをいろいろと試して感じてるんですが，やっぱり人のコード読まないとダメですね．．．「今更なにいってんの？」という話ではあると思うんですが，どうしても論文とか本とかを読んで「なんとなく雰囲気がわかった状態」で終えてしまうことがいままで多かったんですが，これだと本質的なとこが理解できてないですね．．．ということもあって，OpenCVのコードとかも今後読んでいかないといけないなと思い，まずは手始めにキャリブレーションのコードに突撃してみました．やったこととしては，
１．実装の元になった論文を読む．
２．結果が大体同じことを確認しながら，OpenCVのコードをリファクタ．
３．実装でわからないとこにぶつかったら，ググる．論文読む．
というステップでやりました．一通りやっていく中で，
・最適化（最急降下法）
・ホモグラフィー行列の計算
・ホモグラフィー行列最適化（ガウスニュートン法）
・主成分分析を使った点群の正規化
・再投影誤差の最小化（ガウスニュートン法）
・ヤコビアンの計算方法
．．．
と無数のアイテムに出会いました．特に最適化なんかはいまいちしっくり来てなくて困ってたんですが，生きたコンテキストで勉強したことでちょっとは理解が深まったと思います．今後もこのやり方を続けて行きたいんですが，これ，恐ろしく時間がかかるんですよね．．．というのと，大きな関数に対してこれをするのはなかなか大変です．．．まあ，あせらずちょっとづつですかね．
ということで，ここから本題です．
０．全体構成
今回は，OpenCVのcv::fisheye::calibrateの関数をそのままパクってリファクタしたんですが，論文の構成にできるだけ近づくようにリファクタしました．大きな関数のフローとしては，下記のようになります．（ちなみに，OpenCVはvectorでもMatでもできるだけ動くようにIFが抽象化されてますが，このコードがなかなか理解を妨げたので，IFを簡素化してます．）
１．InitializeParamsBasedOnFlagSetting
ユーザから指定されたフラグを元に，キャリブの初期値を決定します．ここはそんなに大したことはやってないかと思います．
２．CalibrateExtrinsics
入力された画像上の点とキャリブボード上の点の対応関係から，下記を実施します．
①DLTを使ってホモグラフィー行列を求める．
②①で得た初期値を元に，ガウスニュートン法でホモグラフィー行列の精度を向上させる．
③キャリブボード上の点のZ座標が０であることを利用して，ホモグラフィー行列から外部パラメータを計算．
３．MinimizeReprojectionError
①ヤコビアンを計算し，ガウスニュートン法を使ってパラメータの更新ステップを計算．
②パラメータの更新ステップをパラメータに反映
③パラメータの更新率がしきい値を下回ったら終了．
４．EstimateUncertainties
①最終決定した内部・外部パラメータを使ってボード上の点を画像に投影．
②①の投影点と実際に撮影されている点を比較して，再投影誤差を計算．
５．ConvertFormat
出力のフォーマットに合わせるための関数です．ここはそんなに大したことはやってないかと思います．
該当部コード
double CalibrateFisheye( const std::vector<std::vector<cv::Point3f>>& objectPoints, const std::vector<std::vector<cv::Point2f>>& imagePoints, const cv::Size& image_size, const int flags, const cv::TermCriteria criteria, cv::Matx33d& K, std::vector<double>& D, std::vector<cv::Vec3d>& board_rotations, std::vector<cv::Vec3d>& board_translations) { IntrinsicParams final_param; // Step 1. Initailization of the parameter int check_cond, recompute_extrinsic; InitializeParamsBasedOnFlagSetting(flags, K, D, image_size, final_param, check_cond, recompute_extrinsic); // Step 2. Calculate Homography and Extrinsics const double thresh_cond = 1e6; CalibrateExtrinsics(objectPoints, imagePoints, final_param, check_cond, thresh_cond, board_rotations, board_translations); // Step 3. Minimize Reprojection Error. cv::Vec2d err_std; MinimizeReprojectionError(criteria, objectPoints, imagePoints, check_cond, thresh_cond, recompute_extrinsic, final_param, board_rotations, board_translations); // Step 4. Calib Result Summary. double rms; EstimateUncertainties(objectPoints, imagePoints, final_param, board_rotations, board_translations, err_std, rms); // Step 5. Format Conversion. ConvertFormat(final_param, K, D); return rms; }
ということで，次のエントリでは "CalibrateExtrinsics" の中身を見ていきます．
リファクタしたコードは下記のGithubにあげておきます．もちろん本家はOpenCVなので，あくまでも流れをつかむ用途でしか使えないですが．．．
github.com
ジモトモと沖縄探索 〜 前半戦
どうもどうも，島人です（笑）．ちょっとしばらくブログをサボり気味でしたが，頻度を上げます！
GWの旅行日記を今更書くなんて．．．という感じはありますが，せっかく写真もとったし，「ブログ書くから！」って言ってたので思い出しながらアップします．ということで，沖縄旅行１日目，２日目，３日目午前までの内容です＼(^o^)／
アメリカ食堂（那覇空港出迎えからの，国際通りで安焼き肉）
相方の沖縄到着一発目の食事はハンバーガーでした．このお店，店頭に巨大なハンバーガーの看板を出してます．友達が来たら絶対食べに行こうと思ってたので，このチャンスを逃すまい！と早速連れて行きました．が，値段（３８００円）が結構高く．．．．諦めました（笑）相方は昔，「ハンバーガー何個食えるか記録を作る！」とかわけわからんこと言い出して，マックで２０個くらいハンバーガーを買ってたくらいだったので挑戦するかな？と思ってたんですが，お互い年をとったということですね（笑）
東京に帰るまでに挑戦するかどうか．．．ちょっと悩んでます．
ただ，ハンバーガー自体はとっても美味しかったです！
ちなみに，このあとの夕食に「焼き肉を食おう！」という話になったんですが，昼過ぎにハンバーガーを食べてたこともあってそんなに食欲もないだろうと思い，金欠時のみんなの味方「安安」に連れて行ったんですが，どうやら相方の舌は僕よりも肥えていたようで．．．「せっかく沖縄来たのに，こんなわけわからんとこ連れてくんな！」と一蹴されてしまいました．．．すいませんねえ．神奈川ではよく一人で行ってます（笑）
２日目（慶良間諸島でダイビング）
自分はこの旅のメーンイベントだと思ってるんですが，スキューバダイビングしました．相方はすでに経験済だったんですが，自分は初めてで．．．．
あれですね，「タンクから空気補充されるし，なんも怖いことないやん．」と思ってたんですけど，耳抜きしないといけないんですね．．．自分はどうも耳抜きがあんまりうまくできず，最初の方は結構パニックでした．しかもこの耳抜き，一回やったらいいというわけではなく，深度が１ｍくらい変わるたびにやらないと耳が痛くなるので，大忙しでした．自分が初心者だからか，耳が痛いのに耳抜きがスカぶりすることが結構あって，力ずくで耳抜きしたら，鼻血で顔面真っ赤になってしまいました．なんでも必死感を出さずに，「サラッとこなすいい男」を目指してるんですが，どうやらキャラ的も身体的にも無理なようです（笑）
でも毎回思うんですけど，こういうアクティビティのインストラクターやってるお兄ちゃんて，頼りがいがあってめっちゃかっこいいですよね．．．船から降りて海中に潜って行く時にインストラクターのお兄ちゃんが用意してくれた鎖を辿って潜っていくんですけど，ダイビングのタンクって結構重たくて（20kgくらい）初心者はなかなか前傾姿勢が保てないらしいんです．で，その説明をみんなの前（含む美女）している時に，「頑張って前傾姿勢を保ってください．上向き姿勢になりそうだったら，前にいる自分の太ももを頼ってもらって（挟んでもらって）大丈夫です！」って言う説明聞いて，思いっきり足で挟んだろうかと思ったのはここだけの話です（笑）
今回のダイビングは相方が手配してくれたんですが，下記の業者さんでした．ほかを試してないので比較できないですが，よかったです！
www.reeffers.com
３日目午前（久高島にて神々との遭遇）
今日から車生活開始です．GWギリギリにレンタカーを予約しようとしたこともあって普通のレンタカーは見事にすべて貸出中で，，，だいぶ焦ったんですがTimes カーシェアを探してみると空車を発見！なんとか車なしの状態を避けることができました．
久高島
神々の島こと久高島です．ググるとWikipediaの説明が出てくるんですが，琉球王国時代に国王が礼拝していたこともあり神聖な島として扱われています．４日目のナイトカヤックのインストラクターのお兄ちゃんから教えてもらったんですが，久高島では琉球の時代に作られた神女組織「祝女」制度を継承していて，女性を守護神とする文化が継承されているようです．新正月（１月１日）でなく，旧正月（１月２８日？）に島を上げて新年のお祝いをするようで，旧正月には多くの人が久高島にやってくるとのことでした．ちなみに，久高島へは安座間港というところから高速船とフェリーが出てまして，高速船で２５分くらい，フェリーで５０分くらいで行けます．
久高島について
ja.wikipedia.org
で，相方の提案でレンタサイクルして久高島を一周することになりました．レンタサイクルとか初めてだったんですけど，これがなかなか当たりでした．帰るまでに石垣島とか西表島に行こうと思ってるんですが，その時もレンタサイクルしたいと思います．
島は一周４キロくらいなんですが，木々が覆い茂っている砂利道を駆け抜けます！ググると下みたいな写真がいっぱい出てくると思います．
最後はランチを食べて，久高島を後にしました．この食堂ですが，食べログにものってます！
で，ちなみに，沖縄ではイラブー汁っていう海蛇のお汁があるんですが，ここでも有りました！一度は勇気を振り絞って食べなければ，，，，と思っているのですが，この時は日和りました（笑）帰るまでには，，，，勇気をだして．．．
ちなみに，日和った僕は日替わり定食（笑）を頼みましたが，なかなか美味しかったです．
ということで，後半戦は次のエントリで！
