ICP(Iterative Closest Point)マッチングによるオドメトリ
前回のエントリでは、つくばチャレンジ2009、トライアル１回目のLRFの点群マップが汚い、ICPスキャンマッチで補正したいと書いた。
今日は、以前に書いたICPスキャンマッチの実装を久しぶりに動かした。がしかし、1年半ぐらい前のものなので座標系が今と違っていたりと、いろいろデバッグしていた。というかずっとデバッグ。というかデバッグがしんどい。頂点が多いものをステップ実行したり、デバッグ用に画像に一時的に出力したり。
1年半前のときは、オフィス内で走ったときのLRF点群データだったが、今日はトライアル１回目のデータのため、さらにマッチが厳しい。
あと、1年半前のときは、回転成分と併進成分両方をICPスキャンマッチで求めてみたのだが、LRFの誤差に比べてロボットの移動速度が遅いため、ICPスキャンマッチでは併進成分の補正ができなかった。なので、今回は併進成分はオドメトリからの値をそのまま使い、回転成分のみをICPスキャンマッチで求めることにした。あと、1年半前の時点では、ちょっとリアルタイムに間に合わないと書いたが、具体的にはメインの制御周期が100msであり、LRFの取得も100msにしているのだが、ICPスキャンマッチが100ms〜200msぐらいであった。今回、併進成分の補正をはずしたところ30ms程度に高速化された。リアルタイムでのICPマッチングはできるかもしれない。
上図、ICPマッチングによる補正前の点群
上図、ICPマッチングによる補正後の点群
補正前の点群をみると直線とかがないので人間でも分かりにくい。最初、このデータもICPマッチングで補正できなかったが、デバッグとパラメータ調整でマッチングできた。
早速、LRFオドメトリ（併進成分はオドメトリ、回転成分はLRFのICPスキャンマッチ）の軌跡を描画してみた（ピンク）。
上図、LRFオドメトリの描画を追加（ピンクの軌跡）
