点群マップ少し改善

リファクタリングも落ち着いてきたところで、本題のポイント・クラウド・マップの改善をしたいところ。LRFオドメトリ（ICPスキャンマッチング)は、失敗だったので、今度は地磁気センサ（TDS-01V）のデータの時間遅れを対処してみようと。

上図、統合ダイアグラム

まず、現状は、ロータリーエンコーダの併進成分と地磁気センサの方位を掛け合わせてコンパスオドメトリを得て、GPSの位置へ少しずつ寄せるという形で統合オドメトリとしている。ところがデバイスの特性か自分のPCへの取り込み方が悪い(スレッド経由でTDS01Vからの受信）のか、時間遅れがある。そこでカレント方位はロータリーエンコーダの方向変化成分の積算＋磁気センサへ少しずつ寄せる形とした（上図の統合コンパス）。

旧コンパスオドメトリと旧統合オドメトリもそのまま実装を残して比較できるようにした。

上図、AibiUIのログ再生モード（つくばチャレンジ2009のトライアル１）

• オドメトリがオレンジ
• コンパスオドメトリ１(旧)が深緑
• コンパスオドメトリ２(新)がこげ茶
• 統合オドメトリ１(旧)が黄緑
• 統合オドメトリ２(新)が黄土色
• 上図に移っていないがLRFオドメトリがピンク

となっている。時間切れのため、統合するときのパラメータ調整をしていないが、点群マップを描画してみた。

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

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

ほんの少し改善されたが、まだまだだ。もう少し詳細にデータを追って改善したい。