Simple Structure from Motion 〜 実装編１（全体構成）
ということで，下記エントリの続きです．ここから中身に入っていきます．
（が，そろそろつくばチャレンジに本腰を入れないといけないので，息長く書いていきます．）
daily-tech.hatenablog.com
０．３種類のSFMと今回の実装
現状，SFMには大きく分けて３つの種類があります．
1. Incremental SfM
最初にシード（種）となる二枚の画像ペアを選び，その二枚に関してF行列を計算してカメラ位置推定を実施し，３次元点群を生成．３枚目以降は点群と画像のマッチングからPNP問題を解くことによってカメラ位置推定を実施し，３次元点群生成．
2. Global SfM
それぞれの画像ペアの相対角度，相対位置を求め，Rotation Average，Translation Average をもとめてカメラ位置推定をしてから３次元点群生成を実施．
3. Hybrid SfM
Incremental と Global の中間的な手法で，ある一定サイズのクラスタ内では Incremental SfM を使って，それらのクラスタ間をつなぐときに Global SfM を使う．
で，Incremental SfM に関しては10年くらい前に盛んに研究が行われてました．ただ，この方法だと画像を一枚加える毎にバンドル調整をしないといけないので，画像が増えてくると計算時間が発散してしまいます．この欠点を補うために，ここ数年は Global SfM が盛んに研究されています．ただ，まだあまりロバストではなく，実用観点では現状 Incremental SfM のほうが性能がいいようです．（自分も試して見ましたが，画像が多くなってくると Global SfM ではうまく行きませんでした．）で，それでは両者のいいところどりをしよう！ということで考えだされたのが Hybrid SfM になります．この方法ではスケールの大きい問題（＝復元するものの規模が大きい）をいくつかのクラスタに分割して，それぞれのクラスタに対しては Incremental SfM を実施します．で，クラスタ間の関係を決めるために Global SfM を使います．Global SfM と Hybrid SfM に関しては細かい理解はまだ十分でないので，興味がある方は下記の論文を見てみてください．
1. Z. Cui and P. Tan. Global structure-from-motion by similarity averaging. In ICCV, pages 864–872. IEEE, 2015.
2. K. Wilson and N. Snavely. Robust global translations with 1dsfm. ECCV, 2014
3. Cui HN, Gao X, Shen SH, Hu ZY. HSfM: hybrid structure-from-motion. In: Proceedings of 2017 IEEE conference on computer vision and pattern recognition. Honolulu: IEEE; 2017. p. 2393–402.
１．SFM観点での全体構成
SfM観点で重要なステップを書き出してみました．次の項（２．全体構成）で実際の実装と照らしあわせた全体構成を書いているんですが，Incremental SfMの原理とフローを簡単にイラスト化しておきます．
SfM Step 1. 各画像からの特徴点抽出（実装 Step. 3）
SfM実行対象となるすべての画像から特徴点・特徴量を抽出します．今回の実装ではSIFTを使ってますが，SIFTだろうが，SURFだろうが，FASTだろうが，「複数の写真に写っている同一点を認識する．」ということが目的なので，この目的が達成されるならどの特徴量でもいいです．が，これが実際問題として結構難しいです．フレームレートの大小や実行時間の制約，ロバスト性，精度等を考慮して適切なものを選ぶ必要があります．一般論として，SfMの場合はWebから検索した写真を使ったりするので，画像間関係にあまり強い前提を置くことができず，特徴量はおそらくSIFT一択になると思います．VSLAMの場合はフレームレートが小さくなるとフレーム間の差分が大きくなって，同一点でも写り方がかなり変わってしまうので，FASTとかだと同一点として認識することがかなり難しくなります．が，かといってSIFTを使うとリアルタイム実行が難しくなると思います．．．
SfM Step 2. 抽出した特徴点を用いた各画像の特徴点マッチング（実装 Step. 4）
SfM Step 1 で計算した各写真毎の特徴点をマッチングします．今回はOpenCVの関数を使ってますが，各特徴点にて計算された特徴量を逐一比較することで画像ペア間の特徴点マッチングリストを作ります．
SfM Step 3. フィルタリングして誤マッチングを除去．（実装 Step. 5）
SfM Step 2 では，単純に特徴点・特徴量の観点でマッチングをしただけなので，当然のことながら誤マッチングが存在します．なので，エピポーラ拘束などの幾何学的な制約を使って誤マッチングをできるだけ取り除きます．今回の実装では，マッチングリストからRANSACで２画像間の基礎行列を求めて，RANSACでアウトライア判定されたものを誤マッチングとして取り除いています．ちなみに，ここで誤マッチングをある程度取れないと後がグダグダになってしまいます．オープンソースのSfMはこのあたりをとても工夫してました．
SfM Step 4. 最も一致度の高い画像ペアを用いて，カメラ位置を推定．（実装 Step. 7）
Incremental SfMの種（シード）を決定します．この画像ペアによって生成される点群を使って残りの画像とのマッチングを取っていくので，こいつがとても重要です．今回の実装では単純に，「計算された基礎行列（F）に対するインライアの数」が一番多いものをシードとして選んでますが，オープンソースのSfMとかだと，「２カメラの視線ベクトルの角度があまりに小さいと三角測量が難しいので除く」とか，いろいろと工夫が入ってました．
SfM Step 5. 推定されたカメラ位置を使って，点群生成．（実装 Step. 7）
SfM Step 4 で 基礎行列（F）が求まりましたが，今回のケースでは内部パラメータは既知という前提でSfMを実施しているのでここから簡単にE行列が求まります．この E 行列を R, T に分解してカメラ行列を求め，２つのカメラ行列から SfM Step 3 で計算したインライアの点に対して三角測量を実施して点群を生成します．
SfM Step 6. SfM Step 4 で生成された点群に対して，バンドル調整．（実装 Step. 8）
ここはまだ実装出来てませんが，ここまでで生成した点群とカメラ行列に対してバンドル調整を実施します．
SfM Step 7. 生成された点群に対して，最もマッチする画像を選択．PNPによりカメラの位置推定．（実装 Step. 11.1）
残りの画像の中で，現時点で生成されている３D点群をもっとも多く含んでいる画像を選択します．で，2D点群（このステップで選択した画像） vs 3D点群（すでに復元されている点群）の対応関係を使って PNP 問題を解くことによってこのステップで選択した画像を撮影したカメラの位置を推定します．
SfM Step 8. 推定されたカメラ位置を使って，点群生成．（実装 Step. 11.1）
SfM Step 7 で選択した画像と，それまでに処理が終了している画像の ２D点群のマッチングペアのなかで，「まだ３D復元されてないもの」を拾い上げます．で，この点に対して三角測量を実施して新たな点群を生成します．（ちょっと絵の辻褄が合わなくなっちゃったんですが，もう夜も更けてきたので，ごまかします（笑）ただ，言わんとしていることはわかっていただけるかと．．．）
SfM Step 9. ここまでに生成した点群に対して，バンドル調整．（実装 Step. 11.2）
ここはまだ実装出来てませんが，ここまでで生成した点群とカメラ行列に対してバンドル調整を実施します．
２．全体構成
で，どうやったら「スッキリ説明＆自分の記憶定着」につながるかな〜と思いつつ手順を分けて行ったんですが，結果的に下記のようになりました．実際のコードは下につけてあります．なんだかとっても手続き的になってしまってて，オブジェクト指向ができない人みたいな感じになってしまってるんですが，大きい流れはやっぱり手続きっぽく書いた方が頭に入って着やすいんですよね．．．．
Step 0. System Setup
Glog, Gflag の読み取りとか．
Step 1. Prepare PCL Viewer.
PCL のビューワー作成と描画スレッドのディスパッチ．
Step 2. Feature Extraction
OpenCV を使った特徴点の抽出．今回は CPU & SIFT でやってます．
Step 4. Feature Matching
OpenCV を使った抽出特徴点のマッチング処理．テスト画像が 11 枚しかないこともあり，今回は OpenCV の Brute Force Matcher を使ってます．画像枚数が多くなってくると組み合わせが爆発するので，他の方法を考えないといけないですが，このあたりはもっと洗練されたオープンソースの SFM にはある程度入ってました．
Step 5. Filter by Fundatmental Matrix Calculation.
特徴量を使ってマッチングした点ペアを，幾何拘束をつかってフィルタリング（＝誤マッチ除去）します．
Step 6. Visualize Matching Result
画像ペア毎の特徴点マッチング結果の可視化．
Step 7. Compute Fundamental Matrix and Decide Scale.
マッチング計算した画像ペアの中で最もスコアの高いものを選択し，その２枚を使って３次元復元．ちなみに，二枚の画像からカメラ位置を求める場合，スケールは不定なので２つのカメラ間距離は「エイヤ！」で決めてあげないといけません．ここで決めた２カメラ間距離（スケール）を基準にしてこれ以降点群座標が生成されることになります．
Step 8. Bundle Adjust
Step 7 で生成した点群とカメラポジションに対して，バンドル調整を実施．
Step 9. Visualizew Two View Pose.
生成点群とカメラ位置の可視化．
Step 10. Sleep for visualization.
少しの観察時間．．．のためのスリープ．．．
Step 11. Loop for all remaining views.
ここからは，残りの画像を一枚づつ選択肢，Step 11.1 ~ Step 11.4 を繰り返します．
Step 11.1. Compute Pose Via PNP
すでに生成されている点群に対して，最もマッチしている特徴点を持つ画像を選択し，PNPによりカメラの位置推定，新たな点群生成を実施．
Step 11.2. Bundle Adjust
ここまでに生成した点群に対して，バンドル調整．
Step 11.3. Visualize Generated Point Cloud and Camera Poses.
生成点群とカメラ位置の可視化．
Step 11.4. Sleep for visualization.
少しの観察時間．．．のためのスリープ．．．
Step 12. Wait till thread joins.
ビューワの終了まち．
該当部コード
int main(int argc, char **argv) { // Step 0. System Setup SystemSetup(argc, argv); // Step 1. Data Preparation Database db; LoadData(FLAGS_directory, FLAGS_k_mat_file, FLAGS_scale, db); // Step 2. Prepare PCL Viewer. PCLViewer pcl_viewer("Sfm Result"); pcl_viewer.RunVisualizationAsync(); // Step 3. Feature Extraction ExtractSIFTFeature(FLAGS_num_features, db); // Step 4. Feature Matching MatchFeature(db); // Step 5. Filter by Funcatmental Matrix Calculation. FilterMatchedFeatureViaGeometricConstraints(db); // Step 6. Visualize Matching Result VisualizeMatchingResult(0, db); // Step 7. Compute Fundamental Matrix and Decide Scale. StructureFromMotionViaTwoViewGeometry(db); // Step 8. Bundle Adjust BundleAdjust(db); // Step 9. Visualizew Two View Pose. pcl_viewer.Update(GenerateCloudPointVectorFromMap(db.GetCloudPoints()), GenerateRecoveredPoseVector(db)); // Step 10. Sleep for visualization. std::this_thread::sleep_for(std::chrono::seconds(5)); // Step 11. Loop for all remaining views. while (db.status.processed_view.size() != db.GetViews().size()) { // Step 11.1 Compute Pose Via PNP StructureFromMotionViaPNP(db); // Step 11.2 Bundle Adjust BundleAdjust(db); // Step 11.3 Visualize Generated Point Cloud and Camera Poses. pcl_viewer.Update(GenerateCloudPointVectorFromMap(db.GetCloudPoints()), GenerateRecoveredPoseVector(db)); // Step 11.4 Sleep for visualization. std::this_thread::sleep_for(std::chrono::seconds(5)); } // Step 12. Wait till thread joins. pcl_viewer.WaitVisThread(); return 0; }
ということで，SfM Step の一つ一つに対してエントリを書いていきます！（思いのほか時間がかかるので，ゆっくり行きます（笑））
