お盆休みを終えて....

前回のブログ更新からちょうど一カ月...
さぼっていたわけでなく,割と作業をしてたんですが,,,,,行き詰まりました.

ROSのパッケージをなるべく使いまわすべく,下記を目標にやってました.

1. ステレオの結果をつかって, Local Map を生成.この Local Map に対して,仮想的なレーザースキャンを生成する.
2. 視差画像を鳥瞰図変換して,このLocalMapに対して仮想的なレーザースキャンを生成する.

が,やっぱり既存の amcl, gslam mapping をほとんど変更なしで使うのは無理そうです.
ステレオカメラと2Dレーザースキャナの長所と短所が完全に正反対で,ステレオカメラをレーザースキャンに変更した時点でステレオの長所を全部殺してしまってるみたいです.
ひょっとしたら gslam mapping のパラメータを変更して微調整をすればうまくいくかもしれないのですが,どうも筋が悪そうな気がしてたまらなくなってきたので別の攻め方で行くことにしました.

ステレオカメラ:
長所:画像情報なので,情報リッチ.3次元情報が取得できる.
短所:検知物体がカメラから離れるにしたがって,距離精度が距離の4乗(2乗?)に反比例して下がる.視野角が狭い(レーザスキャナに比べると).

2Dレーザスキャナ:
長所:検知物体からの距離が大きくなっても,ステレオほど急激に精度が劣化しない.視野角が広い.
短所:2Dスキャンなので,情報が限られている.一定の高さ以上の物体しかみつけられない.

下記,ちょっと自分の勉強もかねて整理したので備忘録代わりに残しておきます.

f:id:rkoichi2001:20170826124844p:plain

上記,ステレオの原理を簡単に図示したんですが,「左カメラ」「右カメラ」って書いてある下の長方形がカメラのイメージャー(画像素子)です.
実際には焦点の後ろに撮像素子があるのですが,前に書いても説明は成り立つので簡単のためにそう書いてます.

ステレオカメラで距離を取得するまでのステップを簡単に書くと,,

1. それぞれのカメラ(左カメラ,右カメラ)でほぼ同じ景色を撮影.

左カメラと右カメラで景色を取るわけですが,二つのカメラの距離が微妙に違う(横方向に20cm位)ので,景色の見え方が微妙に違います.

2. 左カメラで撮影した映像が,右カメラで見るとどの位置に来るかマッチングする.

例えば,湘南の海岸に行って映像を撮影したとして,左カメラで写した映像では左上から数えて横800pix,縦400pixのところにきれいなビキニのおねーちゃんが写っていたとして,
右カメラで撮った映像で同じきれいなおねーちゃんを探していくと,横600pix,縦400pixのとことかにあるわけです.
男がやると,このビキニのおねーちゃんの位置探しは絶対間違えないと思いますが,実際には題材はビキニのおねーちゃんではないことと(残念!),
めっちゃ早く(一秒間に10枚の映像ペア)対応するところを見つけないといけないので,この「左画像と右画像の対応点探し」をするアルゴリズムが必要になります.
これが世に言われる「ステレオマッチング」という問題で,この対応点をいかに正確に早く探せるかという研究が結構さかんに行われていました.
僕の場合は,「SGBM」というアルゴリズムを使って対応点を探してます.

3. 2でマッチングを取ることによって,左カメラ・右カメラに移っている同じ被写体の画像位置がわかるので,あとは上記の三角形の相似から距離を求める.

2のステップで対応点を見つけたら,あとは上記のスライドに書いてある三角形の相似の関係から,対応点までの距離がわかります.
ちなみに,スライドで書いてあるそれぞれの数字は,,,

B:左カメラ・右カメラの距離
cx, cy : カメラの主点座標
disp : 左カメラ・右カメラで写っているx座標の位置の差

です.で,ここで disp を使って距離を求めているわけですが,disp はピクセル座標の差なので,基本的に整数になります.
ビキニのおねーちゃんがあまりに遠くにいてしまって,左右の映像で写っている位置が同じになってしまうと disp = 0 になってしまってもはや距離を求めるすべがなくなってしまいます.
また,おねーちゃんの距離がカメラから離れるにしたがって,映像のなかのおねーちゃんのサイズも小さくなってしまうので,disp もどんどん小さくなってしまいます.
つまり(ちょっと強引ですが),,,,被写体が離れるにしたがって,Zの値があんまり正確ではなくなってしまいます.

で,カメラから離れるにしたがって,1mの正方形がどのくらい小さくなってしまうのかを考えてみたんですが,

f:id:rkoichi2001:20170826135823p:plain

20m離れた地点で 1m x 1m の正方形の面積が 34pix x 34 pix になりました.で,これがカメラからの距離によって被写体が小さく映ってしまう影響で,ステレオの場合はもう一つ
カメラから離れるにしたがって,距離の精度が下がってしまうという影響があります.下記,視差と距離の関係ですが,視差が大きい時(カメラの近くに被写体がいる)場合,1pixマッチングが
ずれても誤差は数センチなんですが,視差が小さいとき(カメラの遠くに被写体がいる)場合,1pixのマッチング誤差が数mの誤差を生んでしまいます.
(下の図の例でいうと,視差 54pix のところを間違えて 53pix と判断しても誤差は 2cm ですが,視差 4pix のところを 3pix と判断すると誤差は 7m 以上になります.)

f:id:rkoichi2001:20170826140631p:plain

ということで,ステレオにあった進め方をする必要があり...いま考え中です...ちょっと書くのが疲れたので,また新しいポストを上げます.