ICPスキャンマッチで、点群マップ

前回のエントリで、ほぼICPスキャンマッチがミスすることがなくなったので、期待大で新フュージョンの実装に取りかかった。
まず、統合ダイアグラムを書いた。

上図、新統合ダイアグラム
ICPスキャンマッチの精度が上がったので、オドメトリによる方向変化成分を使う変わりに、ICPオドメトリによる方向変化成分を使うように変更。
色々あって思ったより実装に時間がかかった（実装2時間デバッグ1時間）。下記が、点群マップ。

上図、コンパスオドメトリ２（改良前）

上図、コンパスオドメトリ３（改良後）
をを〜。綺麗になった。
が、しかし。

上図、コンパスオドメトリ２全体（改良前）

上図、コンパスオドメトリ３全体（改良後）
がーん。改悪されている。。
ICPスキャンマッチ、手ごわすぎる。。。