Stixelを用いた視差画像からのフリースペース計算 〜（理論編１）
ということで，今年のつくばチャレンジのマイテーマ「視差画像を用いた障害物検知・回避」です．「障害物検知・回避」のフローを下記の４ステップに分けるとすると，このテーマは下記の２番に該当します．
１．ステレオ画像から視差を計算する．
２．視差画像をセグメンテーションする．<- ココ
３．セグメンテーションした結果をセンシング情報としてグリッドに反映する．
４．更新されたグリッドをもとに障害物を回避する経路を生成する．
１．参考文献
Stixelという概念を作ったのはDaimlerの研究所なんですが，下記の２つがDaimlerのVision Groupから出ているものです．
- The Stixel World - A Compact Medium LevelRepresentation of the 3D-World
- Towards a Global Optimal Multi-Layer Stixel Representation of Dense 3D Data
あと，つくばチャレンジで実際に使うにあたってCuda実装されたソースが必要だったのですが，その関連論文として下記のものを読みました．
- GPU-accelerated real-time stixel computation
２．Stixelって？
で，Stixel って何ぞや？って話なんですが，「ステレオから得た画像を適度にセグメンテーションしたもの」です．．．．っていっても説明になってないので，描画します！
１．まず，ステレオ画像を取得します．
２．次にステレオ画像から視差画像を計算します．
ここで視差画像が求まって，なんとなーくダンボールとか，テレビとかそれらしい感じで視差（≒距離）が出ていることがわかると思うのですが，視差画像って結局ピクセルに割り当てられた情報でしかないので，情報が多すぎます．このケースだと 1280 x 750 ≒ 100万点くらいの情報量になります．ただ，実際にロボットで使うときって，どこが走行できそうかどうか（フリースペース）が一番大切なので，もう少し抽象化してデータ量を減らせればステキです．下記のようなイメージです．
３．Stixel の計算
ということで，実際に Stixel を計算した結果です．結果の絵ではフリースペースではなくて，オブジェクトがあるところに Stixel がでているので，「フリースペースのイメージ」と逆になってしまってます．あと，パラメータチューニングとかがまだ十分でないので，オブジェクトの足元がちゃんと出てませんが，これはおいおい改善していきます．
このエントリでは，参考文献として挙げた論文の理論的な部分を解剖していきます．
３．Stixel の計算フロー
「Towards a Global Optimal Multi-Layer Stixel Representation of Dense 3D Data」を読み解いていきます．で
３．１．Stixel モデル
アルゴリズムとしては視差画像を入力として受け取り， Stixel を出力するわけですが，式展開をしていく中で D, L, Lu, Sn という４つのデータ単位が出てきます．
D : もとの視差画像．
L : D に対する Stixel の集合．
Lu : 列 u の Stixel の集合．
Sn : 列 u の Stixel の集合のうちの n 番目の Stixel．
ちょっと言葉だけだと味気ないので，簡単にこれを図示してみます．
ということで，このアルゴリズムの IN & OUT としては，視差画像 D を入力としてもらったときに，最適な Stixel の集合が詰まった L を計算することです．で，最適な Stixel の集合 L は列毎の最適な Stixel の集合 Lu からなっており，Lu はその列の最適な Stixel から構成されてます．
３．２．ベイズの定理を用いた Stixel の計算
で，「じゃあ，どうやって最適な Stixel を計算するの？」という話なんですが，ここではベイズの定理を用いて最適化する確率式を構築していきます．確率的に考えると，視差画像 D が求まったときに，最適な Stixel の集合 L を求めるということは下記の式を解くことになります．
で，SLAMのときにもやったようにベイズの定理を使って式変形していきます．
この式変形，ロボットの自己位置推定のときにもいっぱい使いましたが，条件付き確率の条件付けする変数を入れ替えることができました．ロボットの自己位置推定のときにも思ったんですが，実際の Input を条件付き確率の条件付けされる側にすることで，入力データを用いて答え合わせ（尤度計算）ができるようになるのがミソなんですかね．
それぞれの項の役割
ベイズの定理を用いて，P (L | D) ~ P(D | L) * P(L) と式展開できることがわかりましたが，各項にはそれぞれ役割があります．
P(D | L) （条件付き確率項）
この項は，仮定された Stixel に対して，与えられた視差画像を用いて ”答え合わせ” をする役割を担っています．
P(L)（事前確率項）
この項は，物理的な条件・仮定を加味した上で，当該の Stixel が起こりうる確率を計算する役割を担っています．見たまんまですが，この項は視差画像には依存していません．
４．条件付き確率項 P(D | L) の計算
うーん．ここからだいぶややこしくなってきますが，まずは条件付き確率項の詳細です．下記の２条件によって，条件付き確率項がある程度展開できることを見ました．
・隣り合う列は互いに独立である．
・それぞれのピクセルの視差の値は互いに独立である．
ここで，PD(dv | sn, v) の項をもう少し具体的に見ていきますが，この項は一つの Stixel を構成する一つのピクセルに対しての確率を計算します．ちょっと図示すると．．．
で，図の中に書いてしまいましたが，ステレオカメラの地表に対する傾きが決まれば，行の位置（v）によって視差がある程度決まります．例えば，下記のような感じです．
Stixel が路面だったとき：視差値 = α (v - v hor)
（α はステレオカメラの傾きによって決まる値，v hor画像中のは水平線の行座標）
Stixel が物体だったとき：視差値 = μ
（μ は物体の視差値．）
ということで，Stixel を仮定してしまえば尤度が求まることがわかります．で，ここからどうやって尤度を計算していくのかという話ですが，この論文では，確率を次のように定義していました．
で，上式では下記の２つの場合を考慮してモデルに組み込んでいます．
「１．ある物体が存在するときに，得られる視差値が有効である確率」
「２．ある物体が存在するときに，得られる視差値が無効である確率」
無効な場合というのは，おそらくオクルージョンとかの場合ですかね．で，ここからはエイヤで１の確率には正規分布を用いて，２の確率には一様分布を用いてモデリングしています．
イメージは上図の感じですかね．仮定した Stixel からあるべき視差値 fn(v) が求まり，これを実際の視差画像と比較することで１ピクセルずつ尤度を計算していく．．．と．地道ですね．ちなみに，正規化項 Anorm の理解は諦めました（笑）
５．事前確率項 P(L) の計算
次に事前確率項 P(L) の計算です．この項で，仮定した Stixel に対してモデリングの仮定を反映し，尤度を計算します．ここで，具体的に「モデリングの仮定」とは，，
- ベイズ情報量規準：１つの列中において，検知される物体は比較的すくない．
- 重力拘束：浮遊している物体はなく，オブジェクト・地表の境界部は地表上に存在する．
- 順序拘束：１つの列中において，上下隣接したセグメントがある場合，上側のセグメントの視差が大きいことが期待される．
やっぱり文章だと味気ないので，図示します．
上記の３つの拘束を尤度計算に取り込むため，隣接する２つの Stixel を使って尤度計算を実施します．隣接する２つの Stixel のうち，上の Stixel が常に下側の Stixel に条件付けされるような形になります．具体的には，，，，
５．１．隣接する Stixel を用いた尤度計算
次に，実際に P(Sn | Sn-1) をどのように計算していくのか見ていきます．Stixel Sn を構成する要素は Sn = {vbn, vtn, cn, fn(v)} であるので，P(Sn | Sn-1) = P(vbn, vtn, cn, fn(v) | vbn-1, vtn-1, cn-1, fn-1(v)) と書けます．論文では，この条件付き確率を下記のように分解していました．
P(Sn | Sn-1)
=
P(vbn, vtn, cn, fn(v) | vbn-1, vtn-1, cn-1, fn-1(v))
=
P(vbn | vtn-1) * P(vtn | vtn-1) * P(cn | vtn-1, cn-1) * P(fn(v) | cn, vtn-1, cn-1, fn-1(v))
うーん．この式変換が完全に数学的に出てくるのか，ちょっと仮定が入っているのかはわからないんですが，４つある項の意味合いは下記のようになります．
４つの項のそれぞれの確率的意味
１．P(vbn | vtn-1) :
隣接する Stixel を考えた場合に，下の Stixel の上端と上の Stixel の下端が隣り合っていること．
２．P(vtn | vtn-1) :
隣接する Stixel を考えた場合に，上の Stixel の長さを評価する項．
３．P(cn | vtn-1, cn-1) :
隣接する Stixel を考えた場合に，下の Stixel のカテゴリを踏まえた上で上の Stixel のカテゴリを評価する項．（下がオブジェクトなら，上に地面が来る確率は高くない etc...）
４．P(fn(v) | cn, vtn-1, cn-1, fn-1(v)) :
重力拘束，順序拘束，地面より下にオブジェクトがくる可能性を評価する項．
この問題では，事前確率項 P(L)，条件付き確率項 P(D | L) ともに事前計算して Look Up Table として保存しておくことで実行時間の増加を抑えることができます．（ただ，カメラの路面に対する角度が変わると，テーブルを再計算しないといけません．坂道になったりするとこの角度は変わってしまうので，都度計算がさけられないような気もします．）
論文では，P(fn(v) | cn, vtn-1, cn-1, fn-1(v)) の項を下記表にテーブルとしてまとめてました．ちなみに，ここで６つ（実質５つ）の変数が新たに導入されてますが，意味合いとしては，，，
⊿Z（⊿d） : オブジェクトの分離条件
２つの異なるオブジェクトとして分離するための条件変数です．「別の」Stixel として評価されるためには，下の Stixel から ±⊿Z 奥行きが離れている必要があります．⊿d は ⊿Zから計算されます．
ε : 重力拘束・順序拘束の緩和条件
例えば重力拘束を考えると，「浮遊している物体はなく，オブジェクト・地表の境界部は地表上に存在する．」になりますが，視差にはノイズも乗ったりするのでちょっとだけ緩和させてあげるために ε を導入します．
pord: 順序拘束を満たさない確率
「隣接する Stixel があった場合，基本的に上の Stixel のほうが遠方にある」というのが順序拘束ですが，この拘束を満たさない確率をこの変数で表現します．
pgrav: 重力拘束を満たさない確率
「地表面と Stixel 下端は接している」というのが重力拘束ですが，この拘束を満たさない確率をこの変数で表現します．
pblg: 地表面よりも下にオブジェクトがある確率
基本的に地表面よりも下にオブジェクトは無いはず．．．ですが，マンホールの蓋が取れてたりとかそういうケースを表現する確率です．
上記２つの変数を導入すると，P(fn(v) | cn, vtn-1, cn-1, fn-1(v)) のテーブルは下記のようになります．ちなみに，Sn は上側の Stixel，Sn-1 は下側の Stixel です．
ふー．．．．いつものことながら時間がかかるなあ．．．次は実装編です！
Ubuntu & ROS & Cuda インストール
引き続きつくばチャレンジネタです．
以前（１月位？）エントリに書いた，下記のロボットトレーニングなのですが，
はや８ヶ月．子分（笑）とのロボットトレーニングは進んでおりまして，現状の進捗はといいますと．．．．
子分：PCにUbuntuがインストールできません(´・ω・`)
自分：おいっ(；´Д｀)
という状況です（笑）実は二人とも仕事を変わって，特に子分に関しては新婚ホヤホヤという状態で，，，二人ともいろんなライフイベントがあった半年だったんです．「どんな凸凹コンビだよ」って話なんですが，あと２ヶ月，ここからがんばります．．．
PC購入
子分はもともとハードウェアが専門でして，まともなPCをもっていなかったので，奥さんを説得してPCを購入するところから始まりました．購入するときに考えた条件は．．．
- 持ち運びできる．
- ロボットに載せられる．（制御用PCとして使える）
- 画像処理をするかもしれないから，GPUがほしい．
- できるだけ安く．．．
という条件で探した結果，G-Tuneを購入することにしました．同一スペックのゲーミングPCで比較すると，やっぱりG-Tuneが一番安いように思います．
CPU ： インテル® Core™ i7-8700
グラフィックス ： GeForce® GTX 1050
メモリ ： 16GB PC4-19200
M.2 SSD ： 256GB
ハードディスク ： 1TB
購入した時は119,800円だったんですけど，このスペックでこの値段．．．パソコンもどんどん安くなりますね．．．
Ubuntu のインストール
で，Ubuntuのインストールに入ったんですが，ここで早くもトラブル．．．昨年に引き続いて Ubuntu16.04 & ROS Kinetic を使おうと思っていたんですが，新しいPCにUbuntu がインストールできず．．．orz Grubのメニュー画面で "Install Ubuntu" を選択して実行するも，USBがフリーズしてそれから全くインストールが進まないという状況になってしまいました．結構ハマったんですが，下記のサイトを参考にGRUBの起動オプションを変更してやればインストールできるということがわかり，ようやくインストール完了．
このPCの場合はgrubの起動オプションにacpi=offをつけてやればOKでした．で，acpi=offをつける場所なんですが，イマイチコマンドの意味を理解せずにやってたのでここでもはまったのですが，下記のように追記すればOKです．
ということで， Ubuntu のインストール完了．めでたしめでたし．とはいかず．．．acpi=off の状態ではグラボがまともに使えないことが発覚．ちょっと粘ってみたものの，解決方法がわからなかったので Ubuntu 18.04 に乗り換えることにしました．．．この時点で日曜が終了（笑）OSのインストールが秋分の日に持ち越されることとなりました．
ちなみにUbuntu 18.04でもいまいちスッキリインストールが進まなかったので，同じく grub の起動画面で nomodeset というオプションをつけてインストールをすすめました．（上記写真の acpi=off の部分を nomodeset に置き換えた感じです．）
nomodeset の詳細は下記の通りで，BootのときにVideo Driver を読み込まないようにするみたいですね．確かに，nomodeset をつけて起動するとBoot 画面が解像度が荒い状態で立ち上がりました．
nomodeset
The newest kernels have moved the video mode setting into the kernel. So all the programming of the hardware specific clock rates and registers on the video card happen in the kernel rather than in the X driver when the X server starts.. This makes it possible to have high resolution nice looking splash (boot) screens and flicker free transitions from boot splash to login screen. Unfortunately, on some cards this doesnt work properly and you end up with a black screen. Adding the nomodeset parameter instructs the kernel to not load video drivers and use BIOS modes instead until X is loaded.
もっと詳細が知りたい方はこちらを！
https://ubuntuforums.org/showthread.php?t=1613132
Nvidiaのドライバインストール
せっかくGPU付きのPCを買ったので，Nvidia Driver と Cudaのインストールです．
Nvidia Driver の ppa 追加
sudo add-apt-repository ppa:graphics-drivers/ppa
搭載グラフィックドライバの確認
lspci | grep -i nvidia
で
01:00.0 VGA compatible controller: NVIDIA Corporation GP107M [GeForce GTX 1050 Mobile] (rev a1) 01:00.1 Audio device: NVIDIA Corporation GP107GL High Definition Audio Controller (rev a1)
と表示されます．
対応ドライバの確認
今までは Cuda 9.0 を使っていたのですが，Ubuntu も変えたので Cuda も変えることにしました．Cuda10 の対応ドライバは下記のサイトを見た感じ「>= 410.48」となっていたので，このバージョンより新しいドライバならOKそうです．
docs.nvidia.com
nouveauドライバのblacklist化
nouveau というオープンソースのドライバと NVIDIA のドライバがぶつかってしまうらしいので，これを立ち上げないようにします．
gedit /etc/modprobe.d/blacklist-nouveau.conf
で
blacklist nouveau options nouveau modeset=0
を書き込みます．
ドライバのインストール
ここまでくれば，あとはインストールです．最後に，下記のコマンドで推奨ドライバを確認します．
ubuntu-drivers devices
で
vendor : NVIDIA Corporation model : GP107M [GeForce GTX 1050 Mobile] driver : nvidia-driver-415 - third-party free driver : nvidia-driver-430 - third-party free driver : nvidia-driver-435 - third-party free recommended driver : nvidia-driver-390 - third-party free driver : nvidia-driver-410 - third-party free driver : xserver-xorg-video-nouveau - distro free builtin
と表示されます．で，435が推奨とのことなのですが，435は新しすぎるような気がしたので，430をインストールすることにしました．
sudo apt install nvidia-driver-430
で再起動して，下記のコマンドが使えるようになっていればOKです．
nvidia-smi
で，下記のような表示が出ればOK．
| -----------------------------------------------------------------------------+ | ||
| NVIDIA-SMI 430.50 Driver Version: 430.50 CUDA Version: 10.1 | ||
| -------------------------------+----------------------+----------------------+ | ||
| GPU Name Persistence-M | Bus-Id Disp.A | Volatile Uncorr. ECC |
| Fan Temp Perf Pwr:Usage/Cap | Memory-Usage | GPU-Util Compute M. |
| ===============================+======================+====================== | ||
| 0 GeForce GTX 1080 Off | 00000000:65:00.0 On | N/A |
| 28% 53C P0 49W / 200W | 794MiB / 8116MiB | 0% Default |
| -------------------------------+----------------------+----------------------+ |
| -----------------------------------------------------------------------------+ |
| Processes: GPU Memory |
| GPU PID Type Process name Usage |
| ============================================================================= |
| 0 1626 G /usr/lib/xorg/Xorg 40MiB |
| 0 1673 G /usr/bin/gnome-shell 49MiB |
| 0 2045 G /usr/lib/xorg/Xorg 383MiB |
| 0 2174 G /usr/bin/gnome-shell 141MiB |
| 0 2512 G ...quest-channel-token=5223014432444062971 176MiB |
| -----------------------------------------------------------------------------+ |
Cudaのインストール
Cudaは下記の Nvidia のサイトから持ってきてインストールします．ここまで来ていれば Cuda のインストールはすんなり行くと思うのですが，GPUのドライバはすでにインストール済なので，「インストールしますか？」と聞かれた時点で No と回答します．
Cuda のダウンロード先
作業完了！！！
というわけで，PCインストールに熱狂した充実のシルバーウィーク後半戦でした（笑）頑張ったご褒美に，子分が牛角焼肉をおごってくれることでしょう！
ということで，ここから頑張るぞ！(；・∀・)
つくばチャレンジ2019 今年の目標！
ご無沙汰しております．前回のブログアップからだいぶ時間が空いてしまったのですが，結論からいうと元気にやってます（笑）
ちょっとした近況報告
ブログのこと
ちょっと仕事との兼ね合いもあってブログをどう書くか悩んでいたのですが，ありがたいことにクライアントさん（って書くとなんかコンサルみたいですが（笑））にも理解をいただくことができ，ついに４年目を迎えた本ブログですが，無事に継続できそうです＼(^o^)／
つくばチャレンジ2019
で，ようやく本題なのですが，沖縄やら引っ越しやらでバタバタしている間に，ついに本番まで２ヶ月を切ってしまいました，，今年のつくばチャレンジ．カメラを使った自己位置推定は間に合いそうもないので，来年の目標とするとして，今年は下記の３つをテーマにやりたいと思います．
今年の目標（３つ）
１．Stereo Camera を用いて障害物検知・回避を実装する．（LIDARは自己位置推定のみ！にできればいいなあ．．．）
2018年はお借りしたLidarを使って ROS & 2D Lidar の威力を実感しましたが，ここから少しづつ画像処理の比率を上げていければと思っています．具体的には，今年は SGBM と Stixel を使って障害物検知・回避を実装しようと思います．いつもの如く，解剖日記を後々アップします．
２．move_base, teb_local_planner の内部（アーキテクチャ・アルゴリズム）を理解して，手を入れられるようにする．
１の実現に際して，おそらくそのまま ROS のパッケージを使うのは難しいような気がしていることと，Planner 周りの勉強を兼ねて，move_base と teb_local_planner を大解剖したいと思います．
３．子分の理解の促進．
１月に書いた下記エントリにあるとおり，子分（笑）が増えて今年は２人のチームになりました．ただ子分が新婚ホヤホヤであまり時間が取れないこと，住んでいるとこが離れている（栃木，千葉）ということもあって，ちょっと進め方を工夫しないといけないかなと思っています．本走行まで一緒に集まれそうな日程も限られてますが，うまいこと進めて少しは学びを得てくれればと．（我ながら関心するんですが，この後輩思いな僕のスタンス！本当に聖人君子ですよね（笑）おい！そこのお前，嫁の相手ばっかせずちょっとはロボットの勉強もしろ！）
ということで，本番までに何回か集まって合宿もすると思うので，その様子も面白おかしくブログにアップできればと思っています．今年の子分の目標は「コード変更を伴わない範囲で，一通りROSを使えるようになる．」です．「自分でデータとって，マップ作って，ロボットを自動走行させられる．」くらいまで行けば，今年の学びとしてはいい感じかなと．これにプラスαでちょっとした Python のスクリプトも作れるようになれば，尚良ですかね．
リポジトリ
毎年つくばチャレンジ前に「ソフト整理」と題して結構な時間を使っていたのですが，今年からはなんとかうまくできないかなあ．．．と検討中です．要件としては，，，
１．不定期で作る小さなソフトをリポジトリに残して，別の用途でも使いまわしたい．
２．新しく小さなソフトを作るときに，大きなツリーを持ってくるのは煩わしいので避けたい．
３．個別のソフト・アルゴリズムにROSとかのフレームワークを依存させたくない．
ということで，Git の Submodule を使ってみることにしました．それぞれ別のリポジトリとして開発して，必要なときに既存のリポジトリを Submodule として持ってくるようにします．個々のアルゴリズムに対するROSのラッパーとかは，つくばチャレンジのツリーで作ることにします．
github.com
日程感
ということで本番までの日程感ですが，うーんいつものことながら時間がない．．．が，がんばります！
９/２８：勉強会
１０/６：勉強会
１０/１９＆１０/２０：勉強会（合宿）
１０/２１：試走会３回目
１０/２２：試走会４回目
１０/２６＆１０/２７：勉強会（合宿）
１１/２ ：試走会５回目
１１/８ ：試走会６回目
１１/９ ：試走会７回目
１１/１０：本走行
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 の一つ一つに対してエントリを書いていきます！（思いのほか時間がかかるので，ゆっくり行きます（笑））
港区民
前回のエントリからちょっと時間が空いてしまってたんですが，，，実は突貫で引っ越ししてました．
引っ越し
８月からのフリーランス開始に伴って，「最初の３ヶ月はマンスリーマンションと横浜宅の２軒持ちで凌いで，契約更新してもらえそうだったらいい塩梅のところに引っ越す．」という目論見だったんですが，これだと，最初の６ヶ月でかかる費用が，，，
横浜宅 ：90000円 x 3ヶ月
マンスリー ：100000円 x 3ヶ月
引っ越し ：100000円
新居 ：80000円 x 3ヶ月
敷金 ：150000円
合計 ：1060000円
（値段・計算は大体です（笑））
で，家周りだけで100万以上かかってまうやんけ．．．．ということに愕然としたのが 7/19，ここから悩んで家探し＆引っ越しすることにしました．契約更新してもらえなかったら，，，その時はその時ですね！で，引っ越しの方はといいますと，荷物搬入＆搬出が7/30で，やっとこさ部屋が住めるレベルになってきました．ただ，いろいろとガラクタを集めすぎたせいか，開かずの技術書を購入しすぎたせいか，思った以上に荷物が多く，死ぬほど引っ越し大変でした（笑）住む場所変えると気分転換になるので，定期的に引っ越しするのは結構アリかとは思っているんですが，やっぱり２年に一度が限度ですね．．．
新天地
で，どこに引っ越したんや？って話ですが，自分はこれが引っ越し４回めでして，下記のように住む場所を転々をしてきました．
1. 市川市（千葉県）
2. 川崎市（神奈川県）
3. 横浜市（神奈川県）
4. ？？
いい感じで住所ヒエラルキーを上がって（市川市，川崎市の皆様ごめんなさい（笑））来ましたが，この流れで行くとついに念願の23区民か？？ひょっとして港区デビュー？？とかって思ってたんですが，
千葉県流山市
に引っ越しました（笑）８月からの仕事に一番都合が良さそうだったということで，千葉に帰ってきました（笑）ということで，２３区民はもうちょっとお預けですね．最寄り駅は南流山で，品川駅とか渋谷駅とは全然違いますが（笑）ゴミゴミしてなくて，ちょっとだけ開発されてる感じがあって，結構気に入りました．あと，つくばチャレンジの会場も断然近くなりまして，車で１時間の距離になりました．今年からはホテルとらなくて良さそうです．
結局会社を退職してからの３ヶ月間で下記のようにほっつきまわって来ましたが，ようやくちょっと落ち着きました．
4/30 ~ 7/5 : 那覇のマンスリーマンション
7/5 ~ 7/30 : 横浜のマンションで滞在
7/30 ~ : 新居！
なんだか，30代中年男性と思えない自由度の高さですが，松尾芭蕉も人生で93回引っ越ししたみたいなので，これからも自由に新天地を求めていければと思います！（笑）
Simple Structure from Motion 〜 実装編０（テストコードとビルド）
前回のエントリからちょっと間が空いてしまいました．．．が，Structure From Motion の簡易版ができたのでエントリを残しておきます．
０．目的＆動機
沖縄で勉強してた Structure from Motion の知識整理をしたく，とっても簡易化した SFM を作りました．あくまでも，SFMの仕組みを理解することを主目的としてます．で，だいぶ丁寧に撮影した写真でないと落ちます（笑）なので，実際にSFMを使う場合は，下で説明する Theia とか，Colmap とかを使ってください．ただ，性能はあまりでないものの，SFMの必須の流れ＆アルゴリズムを理解するのには結構役立つと思います．最新のオープンソースのものはもっと洗練されてますが，基本の流れ＋αでできているものがほとんどだったので．
１．コード
バンドル調整とか，ロバスト性を上げるロジックとかが入ってないですが，，，，これは時間を見つけて更新できれば．
github.com
２．参考にしたソース
Mastering OpenCV という本の 4 章で SFM を扱っているのですが，該当するコードが下記になります．下記のコードをしらみつぶしに読んで自分なりに書き換えたコードが，今回のコードになってます．なので，大きな流れはほぼ下記のコードに沿ってます．
github.com
国際通り三次元復元！のために使っている＆読んでいるコードが下記のソースです．いくらか SFM のオープンソースコードがありますが，おそらく Theia が一番性能が良い（＝大量の写真に耐えうる＆ロバストに三次元復元ができる．）ような気がしてます．が，それでも国際通りで撮影した写真の枚数が多すぎてまだうまくいってないです．．．
github.com
Colmap は SFM だけでなく，Multiview Stereo も実装されてます．
github.com
３．ビルド＆実行の流れ．
３．１．Github からダウンロード & ビルド
git clone git@github.com:koichiHik/three_view_sfm.git three_view_sfm cd three_view_sfm sh build.sh
３．２．テストデータのダウンロード
２で説明しているデータを落とす必要があります．３．１から続きでコマンド打ちます．
cd .. git clone git@github.com:openMVG/SfM_quality_evaluation.git SfM_quality_evaluation
３．３．実行
３．１でビルドしたコードと３．２でダウンロードしたテストデータを使って動かします．
cd three_view_sfm ./build/bin/three_view_sfm --directory=../SfM_quality_evaluation/Benchmarking_Camera_Calibration_2008/fountain-P11/images/
４．実行結果
４．１．実験に使った画像
SFMコミュニティの方が提供してくれている，下記の画像を使ってます．
４．２．特徴点の表示
噴水の写真は１１枚あるんですが，一つ一つの組み合わせに対する特徴点マッチングの結果が出てきます．
Triangulation (三角測量) による空間中の点の計算
引き続き SfM の構成要素となる小さなアルゴリズムをまとめておきます．
０．このエントリで扱う内容
１．Triangulation とは何か？
２．Triangulation して空間中の点の座標を計算する方法．
１．Triangulation とは何か？
１．１．Triangulation (三角測量) の概要
まず，Triangulation とは何か？って話ですが，これは三角測量のことです．
ja.wikipedia.org
上記のWikipediaのページでは下記のように定義されてました．
三角測量（さんかくそくりょう）は、ある基線の両端にある既知の点から測定したい点への角度をそれぞれ測定することによって、その点の位置を決定する三角法および幾何学を用いた測量方法である。その点までの距離を直接測る三辺測量と対比される。既知の1辺と2か所の角度から、三角形の3番目の頂点として測定点を決定することができる。
定義からだとなかなかイメージがわかないので，簡単に図にしてみます．
上図，絵心がないのはいつものことですが，浜に沿って航行する船に対して大砲を射つケースを考えて見ました．大砲を船に当てるためには大砲と船の距離 d を求める必要がありますが，大砲から船まで距離を直接図るわけには行きません．ここでは代わりに２地点間の距離 ”baseline” とそれぞれの地点 (A, B) から船への直線と baseline とのなす角 (α, β) を求めることによって三角関数を使って 大砲と船の距離 d が求まることがわかります． 今でも工事現場周辺に行くと，三脚のついた黄色いカメラを持ったおじさんがいると思うんですが，このおじさんたちは測量士と呼ばれる人で実際に上述の三角測量をしています．「大砲と船の距離」とか「広大な工事現場の二点間の距離」とか，実際に直線的に距離を測定するのが難しいケースでは今でもガンガンつかわれているんですね．
２．Triangulation して空間中の点の座標を計算する方法．
２．０．参考文献
R. I. Hartley and P. Sturm, Triangulation, Comput. Vision Image Understand., 68-2 (1997-11), 146–157.
２．１．満たすべき方程式
まず，Pの位置座標を X，それぞれの正規化画像中の座標を x, x' とすると次の式が成り立ちます．
上記射影変換の式をもう少し展開すると，下記のようになります．
で，Pの中身を書き出してもう少し突っ込んで式展開すると，最終的に２つの画像点対応点から求まる４つの線形方程式ができます．
最終的に求まった４つの方程式ですが，「１．固有ベクトルを使って解く」か，「２．最小二乗法を使って解く」かで２つに別れます．まず，固有ベクトルを使って解く方法ですが，下記のような流れになります．
このやり方ではいつものように Ax = 0 となるように行列式を整理し，この行列に対して最小固有ベクトルを計算してこれを解とします．
もうひとつの「２．最小二乗法を使って解く」では，解の形を X = [X, Y, Z, 1]T とし，変数 < 方程式数とすることによって最小二乗法を使ってときます．
２．２．反復計算による解の高精度化
上記２．１の解法で Triangulation ができるのですが，文献とコードを読んでみるともう少し精度を上げる方法があるみたいだったので，これも書き留めておきます．ちなみに，このエントリで書いている方法はベストな方法ではないです．ベストな方法は上記 Triangulation に乗っている６次式を使う方法とか，最適補正という手法がベストな方法みたいです．が，ここではあんまり深くに入って行くと戻ってこれないので（笑），ある程度精度が出て，理解もできる方法でとどめておきます．
重み係数の適用
射影変換式を変形することによって，１つの画像点に対して下記の通り２つの線形方程式ができることがわかりました．
up3TX = p1TX
vp3TX = p2TX
この式を引き算の形に直すと，以下のようになることがわかります．
up3TX - p1TX = 0
vp3TX - p2TX = 0
で，理論上は上の等式が成り立つわけですが，実際には計測誤差等が含まれるので右辺は完全に 0 にはなりません．
up3TX - p1TX = ε1
vp3TX - p2TX = ε2
ここで，常識の ε1, ε2 が最小二乗法や固有ベクトルの計算を通して最小化しようとしている値ですが，実際ここで最小化したいのは ε ではなく，「求まった３次元点を再投影した値と，画像上で検出した対応点の差（再投影誤差）」が最小化したいものです．もう一度，式展開をしてみます．
上図の最後に「本当に最小化したい誤差」を表現した４つの方程式を書きましたが，分母に求めるべき変数の X が来てしまっているので線形方程式として解くことができません．こうなるとお手上げなので，ここでは前回に求まった X からp3TXを計算し，この値で方程式全体を割ります．
アルゴリズムの流れ
下記にアルゴリズムの流れを書きましたが，「１．重みで方程式全体を割る．」，「２．割った方程式で Triangulation をする．」「３．求まったXで重み更新」という流れを繰り返します．文献によると，多く見積もっても１０回程度で反復計算は収束するとのことでした．
該当部コード
ということで，該当部分のコードです．最小二乗法で計算しています．
cv::Matx41d IterativeLinearLSTriangulation(const cv::Point3d& norm_pnt1, const cv::Matx34d& P1, const cv::Point3d& norm_pnt2, const cv::Matx34d& P2) { // Do once for initial value. double w1(1.0), w2(1.0); cv::Matx43d A; cv::Matx41d B, X; BuildInhomogeneousEqnSystemForTriangulation(norm_pnt1, P1, norm_pnt2, P2, w1, w2, A, B); SolveLinearEqn(A, B, X); // Iteratively refine triangulation. for (int i = 0; i < 10; i++) { // Calculate weight. double p2x1 = (P1.row(2) * X)(0); double p2x2 = (P2.row(2) * X)(0); if (std::abs(w1 - p2x1) < TRI_ITERATIVE_TERM && std::abs(w2 - p2x2) < TRI_ITERATIVE_TERM) { break; } w1 = p2x1; w2 = p2x2; BuildInhomogeneousEqnSystemForTriangulation(norm_pnt1, P1, norm_pnt2, P2, w1, w2, A, B); SolveLinearEqn(A, B, X); } return X; } void BuildInhomogeneousEqnSystemForTriangulation( const cv::Point3d& norm_p1, const cv::Matx34d& P1, const cv::Point3d& norm_p2, const cv::Matx34d& P2, double w1, double w2, cv::Matx43d& A, cv::Matx41d& B) { cv::Matx43d A_((norm_p1.x * P1(2, 0) - P1(0, 0)) / w1, (norm_p1.x * P1(2, 1) - P1(0, 1)) / w1, (norm_p1.x * P1(2, 2) - P1(0, 2)) / w1, (norm_p1.y * P1(2, 0) - P1(1, 0)) / w1, (norm_p1.y * P1(2, 1) - P1(1, 1)) / w1, (norm_p1.y * P1(2, 2) - P1(1, 2)) / w1, (norm_p2.x * P2(2, 0) - P2(0, 0)) / w2, (norm_p2.x * P2(2, 1) - P2(0, 1)) / w2, (norm_p2.x * P2(2, 2) - P2(0, 2)) / w2, (norm_p2.y * P2(2, 0) - P2(1, 0)) / w2, (norm_p2.y * P2(2, 1) - P2(1, 1)) / w2, (norm_p2.y * P2(2, 2) - P2(1, 2)) / w2); cv::Matx41d B_(-(norm_p1.x * P1(2, 3) - P1(0, 3)) / w1, -(norm_p1.y * P1(2, 3) - P1(1, 3)) / w1, -(norm_p2.x * P2(2, 3) - P2(0, 3)) / w2, -(norm_p2.y * P2(2, 3) - P2(1, 3)) / w2); A = A_; B = B_; } void SolveLinearEqn(const cv::Matx43d& A, const cv::Matx41d& B, cv::Matx41d& X) { cv::Matx31d tmp_X; tmp_X(0) = X(0); tmp_X(1) = X(1); tmp_X(2) = X(2); cv::solve(A, B, tmp_X, cv::DECOMP_SVD); X(0) = tmp_X(0); X(1) = tmp_X(1); X(2) = tmp_X(2); X(3) = 1.0; }
