これは、OpenCV Advent Calendar 2016 17日目の記事です。関連記事は目次にまとめられています。
12/26 肝心のコードの改変部分が根本的に間違っているという超恥ずかしいミスがあったので修正しました．
1. はじめに
昨日はVisualStudio 2015でOpenCV-3.1.0のsfmモジュールをビルドしました．
本日は，OpenCVに付属のsfmのサンプルコードで動作確認をしてみます．というか，例によって実際に試した人にしかわからない落とし穴（ただし，めちゃめちゃ浅い穴）を紹介します．本日の内容はプラットフォーム非依存です．
昨日と同様，検索すれば「opencv_sfmのサンプルを試してみた」という情報はwebでいくつか見つかります．しかし，ここではそれらとは一味違う話をご紹介しますので，ぜひ参考にしていただければと思います．
2. サンプルコードのビルド
では，OpenCVの配布物に含まれているサンプルコードをビルドしましょう．
opencv_contrib-3.1.0/modules/sfm/samples/
に３つのサンプルコードがありますが，これら３つのサンプルファイルは全てcv::sfm::reconstruct()を呼び出してテストするコードで，それぞれの違いは入力データです．
| code | input |
|---|---|
| reconv2v.cpp | 2カメラ間の2D対応点群 |
| scene_reconstruction.cpp | 画像群 |
| trajectory_reconstrucion.cpp | 時系列の2D点群の軌跡 |
という感じです．ここではtrajectory_reconstrucion.cppを使うことにします．
ヘッダやライブラリなどを適切に設定すればビルドできるはずなので，opnencv_sfmをリンクすれば良いだろう…と思ってビルドすると，
trajectory_reconstruccion.cpp: In function 'int main(int, char**)':
trajectory_reconstruccion.cpp:149:77: error: 'reconstruct' was not declared in this scope reconstruct(points2d, Rs_est, ts_est, K, points3d_estimated, is_projective);
みたいに「reconstruct() が見つからへんで」というエラーが出ます．コードを見ても，ちゃんと"#include <opencv2/sfm.hpp>"と書かれています．さて，どうしたことでしょう?
これは実際に<opencv2/sfm.hpp>の中を見ないとわからないのですが，
#ifndef __OPENCV_SFM_HPP__
#define __OPENCV_SFM_HPP__
#include <opencv2/sfm/conditioning.hpp>
#include <opencv2/sfm/fundamental.hpp>
#include <opencv2/sfm/numeric.hpp>
#include <opencv2/sfm/projection.hpp>
#include <opencv2/sfm/triangulation.hpp>
#if CERES_FOUND
#include <opencv2/sfm/reconstruct.hpp>
#include <opencv2/sfm/simple_pipeline.hpp>
#endif
というように，#if CERES_FOUNDというマクロが入っているのが原因です．ですので，例えばコンパイル時に"-DCERES_FOUND=1"とするとか，（行儀は悪いけれども）opencv2/sfm.hppを編集してしまうとか，何等かの対応が必要です．
この問題さえクリアすれば普通にビルドできるはずです．
3. サンプルコードの実行
ビルドしたコードを実行してみましょう．まずは引数無しで実行すると，ヘルプメッセージが出ます．
------------------------------------------------------------------
This program shows the camera trajectory reconstruction capabilities
in the OpenCV Structure From Motion (SFM) module.
Usage:
example_sfm_trajectory_reconstruction <path_to_tracks_file> <f> <cx> <cy>
where: is the tracks file absolute path into your system.
The file must have the following format:
row1 : x1 y1 x2 y2 ... x36 y36 for track 1
row2 : x1 y1 x2 y2 ... x36 y36 for track 2
etc
i.e. a row gives the 2D measured position of a point as it is tracked
through frames 1 to 36. If there is no match found in a view then x
and y are -1.
Each row corresponds to a different point.
f is the focal lenght in pixels.
cx is the image principal point x coordinates in pixels.
cy is the image principal point y coordinates in pixels.
------------------------------------------------------------------
だそうです．2D点群のトラッキング軌跡のファイルと，カメラの内部パラメータ(焦点距離f，光学中心cx, cy)が必要らしいです．ここでイマサラながら公式チュートリアルを見てみると，
./example_sfm_trajectory_reconstruction desktop_tracks.txt 1914 640 360
を実行すれば良いそうです．このdesktop_tracks.txtというサンプルデータは，opencv_contrib-3.1.0/modules/sfm/samples/data/desktop_tracks.txtにあります．
では指示通りに実行してみましょう．
処理にそこそこ時間がかかるので，実行しても最初は何も起きないですが，気長に待ちます．私のPCでは数十秒かかりましたが，以下のような結果が表示されました．
「点群を立方体で表示する」というダサい可視化を見て，ついつい直したくなりますが，今回はアルゴリズムの動作確認が目的なのでそのあたりは無視します．
上図のように，デフォルトでは「カメラが移動している様子を俯瞰的に見た映像」で，視点位置はマウスで移動可能です．コマンド実行したコンソール画面にメッセージが表示されているはずですが，この表示画面で's'を押せばカメラ画面に切り割って以下のような画面になります．
ということで，ちゃんと公式チュートリアルの結果図や，ネットで見つかるサンプル実行例と同じような結果が得られていることが確認できました．めでたし，めでたし．
4. サンプルコードの結果検証
ところで，先ほど「めでたし，めでたし」とか書きましたが，これってホントにうまくいってるんですかねぇ？（疑心暗鬼）
冷静に考えてみましょう．たしかにここではカメラの動きと点群（立方体で表示）が復元されています．その復元結果と入力された2D点群軌跡とがちゃんと辻褄が合っていれば「うまくいった」と言えますが，「実はこの結果は入力データと全然辻褄の合わないでっち上げのデータだった」という可能性もありますよね？
ということで，trajectory_reconstrucion.cppを改変して，2D点群を表示して確認することにしましょう．適当に2D表示のコードを突っ込むことにします．
203 while(!window_est.wasStopped())
204 {
205 /// Render points as 3D cubes
206 for (size_t i = 0; i < point_cloud_est.size(); ++i)
207 {
208 Vec3d point = point_cloud_est[i];
209 Affine3d point_pose(Mat::eye(3,3,CV_64F), point);
ここでは，3D cube表示コードの前に差し込みました．
203 while(!window_est.wasStopped())
204 {
205 /// Render 2D points
206 cv::Mat3b disp_2d = cv::Mat3b::zeros(cy*2, cx*2);
207 for (int i = 0; i < points2d[idx].cols; ++i)
208 {
209 cv::Point pt(points2d[idx].at<double>(0,i), points2d[idx].at<double>(1,i));
210 cv::circle(disp_2d, pt, 3, cv::Scalar(0,192,0), -1);
211 }
212 cv::resize(disp_2d, disp_2d, cv::Size(), 0.5, 0.5);
213 cv::namedWindow("Input 2D points");
214 cv::imshow("Input 2D points", disp_2d);
215 cv::waitKey(1);
216
217 /// Render points as 3D cubes
218 for (size_t i = 0; i < point_cloud_est.size(); ++i)
219 {
220 Vec3d point = point_cloud_est[i];
221 Affine3d point_pose(Mat::eye(3,3,CV_64F), point);
さらに，<opencv2/imgproc.hpp>および<opencv2/highgui.hpp>をincludeしておく必要があります．
では，このコードを実行して，先ほどのカメラ視点からのレンダリング結果と比較して，トラッキング結果の2D情報とSfMの結果がどれくらい一致しているか確認してみましょう．
…おい，全然めちゃくちゃじゃねえか！！！ヽ(`Д´)ﾉ
さて，これはどういうことでしょうか？心を落ち着けて，じっくりとレンダリング結果と2D点群の動きを見比べると，どうも動きが反転しているように見えますね．このあたりがヒントになりそうでしょうか？
結論を書くと，これはSfM処理の問題ではなく，サンプルコードのバグです．（ちなみに，その他２つのサンプルコードにも同じバグがあります．）
5. サンプルコードの修正
では，原因を解説してからバグを修正します．
まず，SfMの結果というのは，時系列の各フレームのカメラキャリブレーションです．
カメラキャリブレーションとは，OpenCVマニュアルの"Camera Calibration and 3D Reconstruction"を見ればわかるように，世界座標系からカメラ座標系への変換処理（に使う行列やパラメータ）であり，回転，並進による座標系の変換（外部キャリブレーション）と，3Dから2Dへの射影変換（内部キャリブレーション）によって構成されます．
一方，サンプルコードでカメラを表示している部分では"cv::viz::WTrajectory"が使われています．OpenCVのマニュアルによると，こちらの第一引数に与えるのは"List of poses on a trajectory."だそうです．つまり，ここではカメラポーズが必要になります．カメラポーズとは，「世界座標系の中で，カメラがどの位置でどの向きにあるか」という情報です．
座標変換についてもう少し具体的に考えると，「カメラ座標系の原点」と「世界座標系のカメラ位置」は対応していますし，同様に「カメラ座標系の光軸ベクトル」と「世界座標系におけるカメラ向き」も対応しています．つまり，カメラ座標系から世界座標系への座標変換は剛体変換であるわけですが，実はこの座標変換（回転，並進）が，そのままカメラポーズ（向き，位置）と一致します．
したがって，カメラポーズ（向き，位置）は，外部キャリブレーションの逆変換ということになります．
サンプルコードを調べると，OpenCVに付属のSfMサンプルコードは全て，SfMの結果をcv::viz::WTrajectoryへそのまま入力してしています．ここがバグというわけです．原因さえわかれば簡単な話で，
119 /// Recovering cameras
120 cout << "Recovering cameras ... ";
121
122 vector<Affine3d> path;
123 for (size_t i = 0; i < Rs_est.size(); ++i)
124 path.push_back(Affine3d(Rs_est[i],ts_est[i]));
を
119 /// Recovering cameras
120 cout << "Recovering cameras ... ";
121
122 vector<Affine3d> path;
123 for (size_t i = 0; i < Rs_est.size(); ++i)
124 path.push_back(Affine3d(Rs_est[i],ts_est[i]).inv());
と，逆行列にしてやれば良いだけの話ですね．（注：初出時に↑に掲載したコード修正が間違っていました）では結果を確認してみましょう．
ちゃんと2D点群位置と矛盾しない，良い感じの結果が得られていますね！
6. まとめ
二日間にわたって，opencv_sfmのビルドと動作確認を紹介しました．
本当は後半部分だけを書くつもりだったのですが，「ついでにVisualStudioでビルドする情報も紹介しておこうか」と思い立ち，文量的に２日間に分けました．（決してAdvent Calendarを埋めるために２つに分割したわけではないですよ！）
公式サンプルコードにバグがあるというのはさすがに想定外の罠でしたが，サンプルを盲信せずにちゃんと検証することは大事ですよね！（←無理やりポジティブな感じの結論を書いた．）
明日はyamada taroさんが担当です．