Semi Global Matching (SGM) と 動的計画法
LidarとStereoのフュージョンに向けて SGM を勉強したので，備忘録としてまとめておきます．
今回からGitHubにコードも公開していきたいと思います．ここで使ったサンプルコードは下記にあります．
github.com
Semi Global Matching
計算手順
計算手順は，大きく分けると下記の4ステップになります．
1. 画像の平滑化
左右の画像ペアでマッチングをとった時，ノイズを低減するためにまずガウスフィルタをかけます．この作業は必須ではないですが，視差画像のノイズが少なくなってなめらかになります．
平滑化後画像
該当コードは下記です．ガウスフィルタまで実装するのはしんどいので，OpenCVの力を借りてます．
if (this->gauss_filt) {
cv::GaussianBlur(left, left, cv::Size(3, 3), 3);
cv::GaussianBlur(right, right, cv::Size(3, 3), 3);
}
2. センサス変換
左カメラ画像と右カメラ画像のマッチングを求めることがこのアルゴリズムの出力になります．で，マッチングをするということは「左画像のピクセルと右画像のピクセルがどれだけ似ているか？」という評価式を作ってあげないといけません．この評価式にはいろいろなものがありますが，今回は一番メジャーな？センサス変換&ハミング距離を用いた方法でやってみました．センサス変換では，直近8近傍画素値を見て自分の画素値との大小関係を保存します．
センサス変換では，直近8近傍の大小関係が一つ一つのビットに保存されるだけなので，画素値そのものには直接的な意味はなくなってしまいますが，相対関係が保存されるので，撮影画像の明暗に影響を受けにくいという特徴があります．
センサス変換後画像
該当コードは下記です．入力画像 "img" に対して，センサス変換後画像 "census" を返します．
void Sgbm::census_transform(cv::Mat &img, cv::Mat &census)
{
unsigned char * const img_pnt_st = img.data;
unsigned char * const census_pnt_st = census.data;
for (int row=1; row<rows-1; row++) {
for (int col=1; col<cols-1; col++) {
unsigned char *center_pnt = img_pnt_st + cols*row + col;
unsigned char val = 0;
for (int drow=-1; drow<=1; drow++) {
for (int dcol=-1; dcol<=1; dcol++) {
if (drow == 0 && dcol == 0) {
continue;
}
unsigned char tmp = *(center_pnt + dcol + drow*cols);
val = (val + (tmp < *center_pnt ? 0 : 1)) << 1;
}
}
*(census_pnt_st + cols*row + col) = val;
}
}
return;
}
3. ピクセル毎のコストを計算する
2で作成した左右のセンサス変換画像から，ピクセル毎のコストを計算します．ここで，コストが小さいほどピクセルペアの一致度が高くなるようにコストを計算したいので，ハミング距離をコストとして採用します．ハミング距離の計算コードは下記です．
unsigned char Sgbm::calc_hamming_dist(unsigned char val_l, unsigned char val_r) {
unsigned char dist = 0;
unsigned char d = val_l ^ val_r;
while(d) {
d = d & (d - 1);
dist++;
}
return dist;
}また，扱う画像が既にステレオ平行化されているという前提に立つと，ピクセル毎のコスト計算処理は，下記のようになります．
具体的にコードに書くと，下記のようになります．ピクセル毎のコストはあとの計算で使うので，すべて保存しておく必要があります．
void Sgbm::calc_pixel_cost(cv::Mat &census_l, cv::Mat &census_r, cost_3d_array &pix_cost) {
unsigned char * const census_l_ptr_st = census_l.data;
unsigned char * const census_r_ptr_st = census_r.data;
for (int row = 0; row < this->rows; row++) {
for (int col = 0; col < this->cols; col++) {
unsigned char val_l = static_cast<unsigned char>(*(census_l_ptr_st + row*cols + col));
for (int d = 0; d < this->d_range; d++) {
unsigned char val_r = 0;
if (col - d >= 0) {
val_r = static_cast<unsigned char>(*(census_r_ptr_st + row*cols + col - d));
}
pix_cost[row][col][d] = calc_hamming_dist(val_l, val_r);
}
}
}
}
4. ピクセル毎のコストを集約する
で，ここからがSemi Global Matchingの肝になります．3のステップですでにピクセル毎のマッチングコストを計算済なので，マッチングコストが最小のものを選択することはできるのですが，これだと制約条件が少なすぎてまともな視差画像ができません．実験的にピクセル毎のマッチングコストが最小になるものを使って作成した視差画像が下記になります．視差に連続性が全くない様子がわかると思います．
で，視差の連続性の観点でコストを導入します．隣り合う画素の視差違いによってペナルティを課します．ここで出てくるP1, P2という二つのパラメータがSGMの一番重要なパラメータになります．
ペナルティなし（０）：隣り合う画素の視差が同じ
ペナルティ１（P1） ：隣り合う画素の視差の違いが１
ペナルティ２（P2） ：隣り合う画素の視差の違いが１以上
このようにペナルティを設けるということは，暗黙には下記の仮定があります．
仮定１：隣り合う画素は基本的には視差が同じになるべき．
仮定２：そうはいっても，斜めに写っている壁とかだと隣接する画素で視差が１くらいは違うときもあるんじゃね？
仮定３：視差が１以上のものに関しては大きなペナルティを貸す！ただし，物体の境界とかでは実際に視差が１以上になるのでまったくありえないわけではない．
というわけで，画像を左から右までスキャンしていったときに，上記3つのペナルティを課しながら総コストを計算していきます．ここでようやく，前回必死に勉強した動的計画法が登場します．ペナルティが3種類あるので，動的計画法の分岐は下記のようになります．
隣接する計算済ピクセルの視差値と同じ：（ペナルティ０）
隣接する計算済ピクセルの視差値より１小さい，もしくは大きい：（ペナルティP1）
隣接する計算済ピクセルの視差値より２以上変化する：（ペナルティP2）
これを数式で書くと，，，
L(p, d) = C(p, d) + min(L(p-r, d), L(p-r, d-1) + P1, L(p-r, d+1) + P1, L(p-r, i) + P2)
となります．ここで，，，，
L(p, d) : 現在着目しているピクセルで，視差値が d の時のコスト
C(p, d) : 現在着目しているピクセルで，視差値が d の時のピクセル毎に計算したコスト．
L(p-r, d) : 隣り合う一つ前（計算済）のピクセルで，視差値がdの時のコスト
L(p-r, d-1), L(p-r, d+1) : 隣り合う一つ前（計算済）のピクセルで，視差値がd-1, d+1の時のコスト
L(p-r, i) : 隣り合う一つ前（計算済）のピクセルで，視差値がi (一以上変わる) の時のコスト
ここで，(x, y) と表記せずに p, r とかって書いてあるので混乱してしまったんですが，SGMでは真横にスキャンするだけでなく，上下左右斜めの8方向（もしくは16方向）でスキャンしてコストを計算します．なので，p はそのまま画素位置と解釈して問題ないのですが，r はスキャンライン上の一要素分の移動ベクトルになります．8方向でコスト計算する場合，上記 L(p,d) の計算を8方向分行って，これをすべて足し算し，その結果最小となる視差値を採用します．（うーん．なんだかうまく説明できないので，コードを見てください．）
動的計画法を使って，コストを求めるコードです．
unsigned short Sgbm::aggregate_cost(int row, int col, int depth, int path, cost_3d_array &pix_cost, cost_4d_array &agg_cost) {
// Depth loop for current pix.
unsigned long val0 = 0xFFFF;
unsigned long val1 = 0xFFFF;
unsigned long val2 = 0xFFFF;
unsigned long val3 = 0xFFFF;
unsigned long min_prev_d = 0xFFFF;
int dcol = this->scanlines.path8[path].dcol;
int drow = this->scanlines.path8[path].drow;
// Pixel matching cost for current pix.
unsigned long indiv_cost = pix_cost[row][col][depth];
if (row - drow < 0 || this->rows <= row - drow || col - dcol < 0 || this->cols <= col - dcol) {
agg_cost[path][row][col][depth] = indiv_cost;
return agg_cost[path][row][col][depth];
}
// Depth loop for previous pix.
for (int dd = 0; dd < this->d_range; dd++) {
unsigned long prev = agg_cost[path][row-drow][col-dcol][dd];
if (prev < min_prev_d) {
min_prev_d = prev;
}
if (depth == dd) {
val0 = prev;
} else if (depth == dd + 1) {
val1 = prev + this->p1;
} else if (depth == dd - 1) {
val2 = prev + this->p1;
} else {
unsigned long tmp = prev + this->p2;
if (tmp < val3) {
val3 = tmp;
}
}
}
// Select minimum cost for current pix.
agg_cost[path][row][col][depth] = std::min(std::min(std::min(val0, val1), val2), val3) + indiv_cost - min_prev_d;
//agg_cost[path][row][col][depth] = indiv_cost;
return agg_cost[path][row][col][depth];
}
<||
8方向スキャンして，コスト計算するコードです．
>||
void Sgbm::aggregate_cost_for_each_scanline(cost_3d_array &pix_cost, cost_4d_array &agg_cost, cost_3d_array &sum_cost)
{
// Cost aggregation for positive direction.
for (int row = 0; row < this->rows; row++) {
for (int col = 0; col < this->cols; col++) {
for (int path = 0; path < this->scanlines.path8.size(); path++) {
if (this->scanlines.path8[path].posdir) {
//std::cout << "Pos : " << path << std::endl;
for (int d = 0; d < this->d_range; d++) {
sum_cost[row][col][d] += aggregate_cost(row, col, d, path, pix_cost, agg_cost);
}
}
}
}
}
// Cost aggregation for negative direction.
for (int row = this->rows - 1; 0 <= row; row--) {
for (int col = this->cols - 1; 0 <= col; col--) {
for (int path = 0; path < this->scanlines.path8.size(); path++) {
if (!this->scanlines.path8[path].posdir) {
//std::cout << "Neg : " << path << std::endl;
for (int d = 0; d < this->d_range; d++) {
sum_cost[row][col][d] += aggregate_cost(row, col, d, path, pix_cost, agg_cost);
}
}
}
}
}
return;
}
5. 視差画像を求める．
でここまでくればあとは簡単です．コストが最小の画素値を採用して，視差画像を作成すれば完了です．
最小コストを選択して，視差画像作成
void Sgbm::calc_disparity(cost_3d_array &sum_cost, cv::Mat &disp_img)
{
for (int row = 0; row < this->rows; row++) {
for (int col = 0; col < this->cols; col++) {
unsigned char min_depth = 0;
unsigned long min_cost = sum_cost[row][col][min_depth];
for (int d = 1; d < this->d_range; d++) {
unsigned long tmp_cost = sum_cost[row][col][d];
if (tmp_cost < min_cost) {
min_cost = tmp_cost;
min_depth = d;
}
}
disp_img.at<unsigned char>(row, col) = min_depth;
}
}
return;
}スケーリングして，可視化用画像作成．
cv::Mat tmp;
disp.convertTo(tmp, CV_8U, 256.0/this->d_range);
cv::imshow("Sgbm Result", tmp);
cv::waitKey(0);
