ICP(Iterative Closest Point)スキャンマッチングのデバッグ

やー、1ヶ月ぶりにロボット開発の時間が取れた。本業が忙しい。
今日は、まずは、以前作ったオドメトリ＋LRFの合成を表示する「LRFの点群マップその2」の改良からはじめた。前回時点では、合成した絵は直ちに保存ダイアログが表示されてファイル名を決めて保存して、その後OS付属の画像ビューアで内容をみていた。しかし、地図が見たいのが目的なのにファイル名を決めたり保存したり画像ビューアを起動したり、ちらかったらその画像ファイルを削除したりと、デバッグ目的で使うには不便であった。そこで、今日は、ImageViewFormというフォームを作って閲覧し、必要ならば「Save」ボタンで保存するUIを作った。

上図、新規作成したImageViewForm

次。
一つ前のエントリでは、ICPスキャンマッチによるオドメトリ（LRFオドメトリ）を実装し、ロータリーエンコーダによるオドメトリの精度を超えられないので辞めるみたいなエントリであったが、もうちょっとデバッグしようと思い直し、今日はデバッグツールを作っていた。

上図、現在のLRFオドメトリ（ピンクの軌跡）
現状、バグがあるようでLRFオドメトリ（ICPスキャンマッチングによるオドメトリ）は、あさっての方向へ行ってしまっている（黄緑の軌跡はロータリーエンコーダのみのオドメトリ、こっちが正しい）。

いままで、デバッグ用に画像を作ってファイルに出力しながら、デバッガコンソールにトレースを出力しながら、ステップ実行をしていたが、とてつもなく面倒な作業だった。そしてデバッグ用画像をOSの画像ビューアでみたり、ちらかったデバッグ用画像を削除したり。と言うわけで、これもImageViewFormで表示するようにした。これで狙ったスキャンフレームのみをステップ実行できて、ゴミファイルも散らからずに済む。

上図、新規作成したデバッグツール画面

これでデバッグするぞ！というところで時間切れ。バグが取れたら精度はどのぐらいかな〜。