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 です．
ふー．．．．いつものことながら時間がかかるなあ．．．次は実装編です！
