ステレオカメラを使ったOGMの作成

金曜朝のエントリで少し書きましたが，ステレオカメラを使ったマップを作成中です．
「GWの取り組み」エントリで書いたとおり，スキャナの性能が思いのほか高くなく，，，，結局カメラでなんとかしようとしています．
地図作成ですが，いろいろと研究されて洗礼されたテクニックが存在すると思うのですが，自分の浅い理解ではつまるところ下記の手順でやればよいのかと思います．

１．地図作成する領域（例えば大清水公園）の中で，ロボットの相対位置を正確に把握すること．
２．定期的にロボットの周辺環境を検知して，検知結果を１の情報を用いで領域の中に落としこむこと．

ここで，普通は１にも２にも誤差が含まれてしまいます．１の誤差がオドメトリ誤差で，計測値を積算していくなかでどうしても膨らんでいきます．２の誤差がセンサーの検知誤差で，こちらは積算されることは無いですが，精度が完璧なセンサーは存在し得ないのでそのセンサー・環境に特有の誤差が乗ってきます．誤差補正を全くせずに，大清水公園で取得したデータをもとにマップを作った結果が下記になります．

上記マップは，ステレオカメラで取得したデータを３次元座標に置き換えて，地表から30cm以上，150cm未満のエリアに存在する物体をプロットしたものです．見てあきらかな用に，ヨー角誤差が膨らみ，全体として地図がゆがんでいます．大清水公園を一周した地図なので，スタートとゴール地点でループが閉じないと行けないのですが，完全一致するところまで来ていません．

ただ，地図に写っている木の本数等はある程度一致していて，なんとなく環境の特徴を拾っているようにも見えます．

で，このままではちょっとローカリゼーションするのが難しそうなので，トライアンドエラーで地図を改善します．

ロボットの相対位置を正確に把握するためのアプローチとして，，，
１．VOを作ってみる．
FindEssentialMatを使って，GW取得データでVOを作る．
２．バンドル調整で最適化摩る．
VOで関連付けたカメラ座標をバンドル調整で最適化する．

センサーの誤差を抑えるためのアプローチとして，，，
１．Disparity画像に対してStixelを計算し，フリースペース内外境界のみでマップを作成する．

ちょうど金曜の朝にStixel作成のアルゴリズムを公開しているgithubのリポジトリを見つけたので，次のエントリではStixelを使ってみます．