１つ前のスキャン

平日は体力を奪われないように夜中に研究するのは控えているのだが、今日は20分ほどプログラム。

上図、赤が現スキャン、暗い赤が前スキャン（ICPオドメトリ）
さらなる点群マップの精度アップのため、ICPオドメトリを強化したいなと思い、以前はあった前スキャンと現スキャンの重ね描きが、リファクタリング中になくなってしまっていたので、復活させた。あ、そうその前に、今までLRFオドメトリと呼んでいたものは、ICPオドメトリと呼ぶことにした。なぜならLRFはICPスキャンマッチング以外でもいろいろ使えるので。
この、前スキャンと現スキャンで誤差があるフレーム（マッチングに失敗したフレーム）をすばやく発見でき、デバッグが容易になるので、この機能は必須。
さっそくICPオドメトリのバグっぽいものを発見。が、しかし時間切れ。