ICPスキャンマッチング：誤差評価関数
昨日のエントリで、ICPオドメトリでバグらしきものを見つけた、と書いたが調査したところ、バグというか、ICPの誤差評価関数を変えた方がよいということが分かった。ICPは、このエントリで書いたように、Error Metricステージで誤差を評価するのだが、今まで、
- 各点ペア（実際には、Matchingステージでprojectionを選択しているので、点と線分の最近点）の距離の2乗の和
であったが、
- 各点ペアベクトル（各誤差ベクトル）の和のベクトル長
に変えた。や、式で表した方が分かりやすいか。
- Error=|E1|^2+|E2|^2+ .....
だったのを
- Error=|E1+E2+....|
にした（E1、E2はベクトル）。
上図、誤認識している図（改善前）
これでほぼ誤認識０となった。微小な誤差の積算でICPオドメトリの軌跡自体は相変わらずあさっての方向へ進んで行ってしまうが、前スキャン、現スキャンを重ねが来すると見た目上はほぼぴったり合っている。つくばチャレンジ2009のトライアル１回目のログでは、10000スキャンぐらいしているが、それを確認したが誤認識なし（ただし、頂点数が少なすぎて全くマッチしないフレームが数件ある、現状その場合旋回成分０とみなしている）。
劇的に改善された。これは、次の一手が楽しみだ。
