まだソースコードはほとんど読んでおらず、とりあえずexampleの通りに利用する形で、速度と精度を簡単に検証する。
データ
AWSIM西新宿v1.2.0を用いて、シミュレータのGTつきデータを取得した。経路は以下の通り。
点群マッチングには関係ないが、様子を把握するためにカメラ画像を動画化すると以下の通り。
点群マッチングで使われるものは、以下の白く小さい点がマップ点群で、緑色の正方形が各タイミングでのセンサ点群（ダウンサンプリング済み）となる。
保存したrosbagを分解して、各スキャンデータでのPCDファイルと、そのときのタイムスタンプに合わせて線形補間したGT PoseのCSVファイルにした。合計で819フレームのデータが得られた。
データリンク : https://drive.google.com/file/d/1PLaDYRKQ7HqIVlHvc1OBDSd0S7lKTAEh/view?usp=drive_link
PCDマップデータはAWSIM西新宿のものを利用した。
手法
- ndt_omp (tier4のフォーク版）
- fast_gicpのVGICP
- small_gicpのVGICP
の3種類についてそれぞれ実行した。
パラメータ
パラメータとしては
| 項目 | 値 |
|---|---|
| Resolution | 2.0 |
| NumThreads | 4 |
| MaximumIterations | 30 |
| TransformationEpsilon | 0.01 |
を基本として他はデフォルト、ndt_ompについてだけはstep_sizeを0.1とした。実験コード
ノイズ
GT Poseから各種alignを始めても面白くないので、GT Poseに対してxとyの位置について、-3mから3mまでの範囲から一様分布で乱数を取り、それをノイズとして加えた位置を初期位置としてalignを実行した。同一フレームのデータについては、同じノイズが乗った初期位置が3種類の手法に与えられる。
結果
実行時間
実行環境：Intel(R) Core(TM) i7-7700 CPU @ 3.60GHz 8コア
| 手法 | 平均処理時間 [msec] |
|---|---|
| ndt_omp | 14.94 |
| fast_gicp | 1.47 |
| small_gicp | 1.80 |
fast_gcipとsmall_gicpは1回目のalignは非常に時間がかかる（fast_gcipで約5秒、small_gicpで約4秒）ので、1回目は除外して計算している。（上手く利用すれば1回目の構築処理は事前に実行できると思われるため）。
fast_gicp, small_gicpはndt_ompよりも処理時間が1/10程度になっている。信じられないくらい速い。この理由として、今回の実験はノイズを加えてそれなりに離れた位置からalignを始めているのもあり、収束にかかるイテレーション数に差があると思われる。tier4フォーク版ndt_ompではline_searchを切っているためか、離れたところに収束するのは結構イテレーション回数がかかる。実際にndt_ompのイテレーション回数を表示してみると上限の30に達していることが多かった。fast_gicpでもNrIterationを取得する関数を追加しており、そちらを確認すると30回には行かずすぐに終わっていることが多かった。
これを検証するため、3手法全てsetTransformationEpsilon(0.0);として、強制的に上限の30回まで回るようにして実行した。
| 手法 | 平均処理時間 [msec] |
|---|---|
| ndt_omp | 18.08 |
| fast_gicp | 5.14 |
| small_gicp | 1.84 |
fast_gicpはちゃんと遅くなったが、small_gicpはほとんど変わらないままだった。純粋にとても速い可能性が示唆されている。
[2024/04/09追記] 本文中のsetTransformationEpsilon(0.0);とする実験について、設定が上手く反映されていなかった可能性があるとのことだったため、修正後のコードで再実験した結果を追記
| 手法 | 修正前平均処理時間 [msec] | 修正後[msec] |
|---|---|---|
| ndt_omp | 18.08 | 17.95 |
| fast_gicp | 5.14 | 5.18 |
| small_gicp | 1.84 | 2.14 |
そこまで極端には変わっていないと思われる。
収束PoseとGT Poseとの差分：位置
ここでの位置は、GTのPoseから見た相対的な位置関係である。つまりx方向が自車前方向、y方向が左手方向、z方向が上方向。TransformationEpsilonはデフォルトの0.01に戻している。
| 手法 | 平均位置誤差 [m] |
|---|---|
| ndt_omp | 0.45 |
| fast_gicp | 1.29 |
| small_gicp | 0.17 |
fast_gicpは若干精度が悪く出てしまうが、small_gicpはとても良い結果となった。
収束PoseとGT Poseとの差分：向き
GT Poseから見た相対的なroll, pitch, yaw, rot_vecで表現したときのノルムをプロットした。TransformationEpsilonはデフォルトの0.01に戻している。
| 手法 | 平均角度誤差 [deg] |
|---|---|
| ndt_omp | 0.18 |
| fast_gicp | 0.19 |
| small_gicp | 0.12 |
どの手法でも時々スパイクは出るが、見た目のスパイク頻度的にも平均数値的にもsmall_gicpが最も良かった。
まとめ
今回の簡単な検証では、small_gicpはとても高速かつ精度も良く、かなり有望そうな結果であった。ソースコードを丁寧に読んだり、今後複数シーンなどで検証していく価値が大いにあると思われる。
