ICPはなぜ失敗するのか

ICPは、近い点どうしの対応と剛体変換の計算を繰り返す局所的な位置合わせ手法である。残差が減ったことは、正しい場所へ移動した証明にならない。ここでは意図的に単純な2D点群を使い、初期姿勢、外れ値、対称性による失敗を分ける。

何を最小化するのか

この実装は点対点の距離の二乗和を小さくする。各反復で変換後のsourceからtargetへの最近傍を探し、距離閾値以内だけを使ってSVDで回転と並進を求める。鏡映にならないよう行列式を補正する。反復上限は100回である。Open3DのICP解説は点対点と点対平面を区別しており、本実験は前者だけを自作する。

入力と実行方法

コードを保存し、python3 engineering_labs.pyを実行する。検証環境はPython 3.12.3、NumPy 1.26.4、Matplotlib 3.6.3。Open3Dのインストールは不要で、その速度や実装との比較でもない。

基準形状は長さ3 mと2 mの線分を組み合わせたL字で、点数は102。角は重複点を含む。sourceはtargetから真の回転20°・並進(0.4, −0.3) mの逆変換で作った。センサ雑音を加えず、対応付けだけで失敗が起きることを観察する。良い初期値は18°・(0.35, −0.25) m、悪い初期値は110°・(1.5, 1) mとした。

三つの指標を同時に見る

残差は採用された最近傍距離のRMSE、対応率は全source点に対する閾値内の割合である。さらに合成データなので、真値との回転誤差と並進誤差を計算できる。

図1 · ボタンで表示を切り替え

良い初期値でも誤差はゼロではない。等間隔の線上の点が隣の点に対応して止まる局所解が残る。対応率1.000だけを合格条件にすると、このずれも見逃す。図の外れ値はsource側のx=4〜7 m、y=3 mに置いた。入力target、外れ値ありの入出力、集計値を公開した。

円と壁で見えなくなる方向

円は半径1 m、120点で、真の変換は恒等変換と定義した。点の集合は90°回しても一致するため、残差がほぼゼロのまま姿勢を90°間違える。形だけから正解を選べないので、反復回数を増やしても情報は増えない。

一本の壁では別の問題がある。点対平面の誤差は壁の法線方向しか測らず、壁に沿う移動を拘束しにくい。有限な点対点の壁では端点や点の配置が情報を与えるが、端点が視野にない長い廊下では弱くなる。円の実験結果と、壁の拘束方向についての説明は区別して読む。

閾値を下げれば解決するわけではない

外れ値を除けた0.5 mの閾値も、初期位置が離れれば正しい対応を消してしまう。広い閾値と狭い閾値では採用点が違うので、残差の数値だけを直接比較できない。点数が3未満ならこのコードは更新を中止するが、3点あれば必ず良い配置という意味でもない。

実際の点群では、初期値をオドメトリ等から与える、粗い解像度から細かくする、外れ値除去を行う、異なる向きの面を観測する、といった方法を検討する。その効果は同じ区間と同じ基準で測る必要がある。

SLAMでの評価へつなぐ

この計算は単一フレームの2D剛体位置合わせであり、3Dの走行精度や動く物体への頑健性を測っていない。SLAM評価では、局所的な残差の外にある軌跡誤差、時刻の整合、失敗区間を評価できる。まず本実験の初期角度だけを変え、収束範囲が一つの数値では表せないことを確かめるとよい。

同じ計算を手元で実行する

実行用コード一式（ZIP）を展開します。実験入口、生成された計算部分、PID原本を同じフォルダへ個別に保存しても実行できます。PIDはPython標準ライブラリのみ、Kalman・ICPはNumPyを使います。従来の図生成コマンドも引き続き利用できます。

python3 -m pip install numpy==1.26.4
python3 experiment.py icp --parameters '{"case":"good_init","threshold":0.5}' > result.json

caseをbad_init・outliers・symmetric_ringへ変える。外れ値はthreshold=10と0.5を比較する。円で残差がほぼゼロでも回転誤差90°が残ることを確認する。