Free Space Computation

以前の記事で紹介したStixelのアルゴリズムに登場する
Free Space Computationを実装しようと頑張ってます

Free Space Computationとは

視差画像をもとにシーンのFree Space(移動可能な領域、路面)を推定する手法です
推定結果のイメージです(赤く塗った領域がFree Space)

f:id:mizunashi:20160820225810j:plain

より具体的には、Free Space Computationは
画像の列ごとに「路面と物体の下端との境界」となる行を見つける問題となります

参考文献

[1]Efficient Representation of Traffic Scenes by Means of Dynamic Stixels
[2]Free Space Computation Using Stochastic Occupancy Grids and Dynamic Programming

考え方

下図に、画像のある列におけるステレオ観測値(=奥行き)の分布を示します

f:id:mizunashi:20160821230953p:plain

緑線が路面、赤線が物体、青線が背景の領域です
この図から、

「路面と物体の下端との境界」から手前側(カメラ側)は路面の領域
物体領域の奥行きはほぼ一定(物体は大抵の場合直立しているため)

ということがわかると思います

これをより数学的に書くと次のようになります

$v_b$ を「路面と物体の下端との境界」となる行、 $V$ を画像下端の行とすると、 $[v | v_b <= v <= V$ ]を満たす $v$ は路面領域内にあり、その視差は $d_R(v)$ で表される
$h_v$ を物体の高さ(画素単位)とすると、 $[v | v_b - h_v <= v <= v_b$ ]を満たす $v$ は物体領域内にあり、その視差は $d_R(v_b)$ で一定である

ここで $d_R(v)$ はある行 $v$ に対応する路面上の視差を返す関数であり
論文では観測した路面の視差をB-Splineでフィッティングすることで $d_R(v)$ を求めています

この考えをもとに、境界の位置で最小となるようなコスト関数を定義します

コスト関数

コスト関数は以下のように、2つのサブコスト $\Gamma_v^R$ と $\Gamma_v^O$ の重み付き和となります
$\Gamma_v = \alpha_1 \Gamma_v^R + \alpha_2 \Gamma_v^O$

$\Gamma_v^R$ は路面(Road)のコストで、以下のように表されます
$\Gamma_v^R = \sum_{v=v_b}^V | d_R(v) - d_v |$

$\Gamma_v^O$ は物体(Object)のコストで、以下のように表されます
$\Gamma_v^O = \sum_{v={v_b - h_v}}^{v_b} | d_R(v_b) - d_v |$

これを各画素について計算します
コードで書くとこんな感じです

for (int u = 0; u < disp.cols; ++u)
{
	for (int vb = 0; vb < disp.rows; ++vb)
	{
		float objectCost = 0, roadCost = 0;

		for (int v = vb; v < disp.rows; ++v)
			roadCost += fabsf(disp(v, u) - roadDisp[v]);

		for (int v = max(vb - hv, 0); v < vb; ++v)
			objectCost += fabsf(disp(v, u) - roadDisp[vb]);

		score(vb, u) = alpha1 * roadCost + alpha2 * objectCost;
	}
}

DPによる境界の計算

最終的な出力である「路面と物体の下端との境界」を計算します
これは先ほど求めたスコアの値だけではなく、境界の空間的な連続性も考慮する必要があります
そこで画像の各画素をノードとして、左端と右端を結ぶ最適な経路を求める問題を考えます

これは動的計画法的に以下のように書けます

for (int v = 0; v < score.rows; v++)
{
	dpcost(v, 0) = score(v, 0)
}

for (int u = 1; u < score.cols; u++)
{
	for (int v = 0; v < score.rows; v++)
	{
		float mincost = FLT_MAX;
		int minpath = 0;

		for (int vv = 0; vv < score.rows; vv++)
		{
			int jump = abs(disp(v, u) - disp(vv, u - 1));
			int penalty = std::min(P1 * jump, P1 * P2);
			float cost = score(v, u) + dpcost(vv, u - 1) + penalty;

			if (cost < mincost)
			{
				mincost = cost;
				minpath = vv;
			}
		}

		dpcost(v, u) = mincost;
		dppath(v, u) = minpath;
	}
}

// back track
float mincost = FLT_MAX;
int minv = 0;
for (int v = 0; v < dpcost.rows; v++)
{
	if (dpcost(v, dpcost.cols - 1) < mincost)
	{
		mincost = dpcost(v, dpcost.cols - 1);
		minv = v;
	}
}

for (int u = dppath.cols - 1; u >= 0; u--)
{
	path[u] = minv;
	minv = dppath(minv, u);
}

上記コードで境界の空間的な連続性を与えるのが、以下の部分です

int jump = abs(disp(v, u) - disp(vv, u - 1));
int penalty = std::min(P1 * jump, P1 * P2);

隣接ノードとの視差の絶対差分に比例したペナルティを与えます
(ただし、ある程度の不連続性を許容するため差分がP2以上であればペナルティは一定とする)

ただこの計算、オーダーが $O(w*h*h)$ となって結構重いです
これであってるんかな…

実装結果

前回紹介したデータセットを使って実験した結果が最初に見せた図です
入力の視差画像はデータセットから与えられます
路面上の視差 $d_R(v)$ はB-Splineではなく、直線で近似してます
まあ何となくあってそうに見える…

おわりに

Free Space Computationについて理解したこと、実装したことを紹介しました
今後はB-splineによる路面モデルの推定を実装したいです

おぺんcv

画像処理エンジニアのブログ