ICP(Iterative Closest Point)のアルゴリズム - かみやんの技術者ブログ

つくばチャレンジ２００７の出場者の技術資料にICPを使っているという人が何人かいたので、僕もICPを採用することにしました。という訳でまずはICPについて調べました。

ICPはパターンマッチングの１種なので、ちょっと人工知能的。手書き認識とか、OCRとか、音声認識とか、顔認識とかジェスチャー認識も多くはパターンマッチングが使われる。やっとソフトウェアらしい自分の領分なので楽しい。

『Efficient Variants of the ICP Algorithm』この論文が、ICPの様々なアルゴリズムについて検証していてよい感じでした。3Dのレーザーレンジファインダから得られる２つのメッシュの合成について書かれています。
この論文は、主にいろいろなアルゴリズムを試験して、何が高速であるかを検証しています。

概要

概要としては、ICPは以下の６つのステージからなります。

Selection : １つまたは２つのメッシュからいくつかの点を選択する
Matching : 1つの点が他方のメッシュのどこに対応するか求める
Weighting　: 重み付け
Rejecting : 不要なペア（点対）を除外する
Error Metric : 誤差量を求める
Minimizing : 誤差量を最小化する

簡単に言えば、２つの図形のうちの１つの図形を平行移動したり回転させて対応点間の距離が最小になればOKという話。収束演算系ですね。誰が考えてもそうだろうという当たり前の計算ですね。どう平行移動するか回転させるかについては論点がずれるのでこの論文ではアルゴリズムの比較検討はしていないです。

テストシーン

テストシーンとして、ウェーブとフラクタルランドスケープとプラスプレーンを用意しました。ウエーブは、どのアルゴリズムでも簡単に収束します。フラクタルランドスケープは、実データぐらいの複雑度です。プラスプレーンは、特徴的な箇所が非常に小さく、ほかは平らなのでマッチさせるのが難しいテストシーンです。

Selection

セレクションでは、以下のアルゴリズムあります。

すべての点を使う
ユニホームサブサンプリング（一定間隔でサンプリング）
ランダムサンプリング（イテレーションごとに違う点を選ぶ）
色の輝度の傾きを使う（訳注：3D LRF＋カメラのイメージの場合か？）
ノーマルスペースサンプリング（法線の変化が大きいところに多くサンプリングし、変化が少ないところを間引く。本論文が提案）
上記とは別に、１つのメッシュから選ぶか、両方のメッシュから選ぶか

Fig2が、ウェーブシーンでの比較。大差なし。多少、ノーマルスペースサンプリングがよい。
Fig3が、プラスプレーンシーンでの比較。ノーマルスペースサンプリングのみ収束している。
Fig5が、１つのメッシュからのみと両方のメッシュから選ぶ場合の比較。大差なし。多少、両方の方がよい。

上図が、ノーマルスペースサンプリングの例です。溝に多くの対応点を取っているため、プラスプレーンシーンでも収束できています。

Matching

マッチングでは、以下のアルゴリズムあります。マッチングとは、一方のメッシュで選んだ点に対応する点を他方のメッシュから決めることです。

最近点。最近点を選ぶ。高速化には、事前にk-dツリーを構築するとよい。
ノーマルシューティング。ソース側の点の法線を延長してディスティネーション側のメッシュの面との交点を求める。
プロジェクト（投影）。ディスティネーション側のメッシュの原点（LRFのある位置やカメラの位置）とソース側の点を結んだ直線と、ディスティネーション側のメッシュの面との交点。
コンパチブル。上記に加えて、色や法線の傾きを使ってよりよい点を探す。

上図が、フラクタルランドスケープシーンでの試験結果。ノーマルシューティングがよい。次が、プロジェクト。最近点が最も悪い。最近点が悪いのは、ノイズ等でちょっとした出っ張りがあると最近点だとそこに対応点が集中するためだと思われます。

上図が、プラスプレーンシーンでの試験結果。最近点＋コンパチブルが完全に収束。次が、最近点（遅いけど収束しそう）。次が、ノーマルシューティング＋コンパチブル（局所解にはまっている）。プロジェクトとノーマルシューティングは、全く求まらず。

上図が、フラクタルランドスケープシーンでの、計算速度。プロジェクトが高速（0.1秒以下）。次がノーマルシューティング（0.8秒ぐらい）。最後が最近点（1.1秒ぐらい）。ちなみにノーマルシューティングや最近点の場合は、k-dツリーを事前に作成して高速化してあり、その時間は含まれていません。k-dツリー作成時間は0.64秒ぐらい。計測は、550MHzのPentiumIII XeonでC++で実装です。

Weighting

重み付けには以下のアルゴリズムがあります。

コンスタント。重み付けしない。
リニア。距離に比例。
コンパチビリティノーマル。２つの点の法線の内積。
アンサートンリィ。Appendixに計算式がある（訳注：よく読んでない）

Fig11がウェーブシーンでの試験結果。大差なし。
Fig12がプラスプレーンシーンでの試験結果。大差なし。アンサートンリィとコンパチビリティノーマルが多少よい。

Rejecting

除外には以下のアルゴリズムがあります。

コンスタント。一定距離以上離れたものを除外（閾（しきい）値指定）
ワースト。n%を除外。距離の離れたものからn%を除外。10%とか。
シグマ。標準偏差からの外れたものを除外。2.5σとか。
インコンパチブル。隣り合った２つの点対の距離（２つ）に一定以上の差があるとき。
上記とは別にメッシュの境界（淵）を除く。

上図は、ウェーブシーンでの試験結果。ワーストがやや悪いが大差なし。

Error MetricとMinimization

誤差量には、以下のアルゴリズムがあります。

point-to-point。点と点の距離の2乗の和。
point-to-plane。点と平面の距離の2乗の和（点と平面の最近点。点と最近点を結ぶ直線と平面の法線は同じ方向になる。この誤差量は、非線形になる）

最小化には、以下のアルゴリズムがあります。

select-match-minimize。現在の変換行列（平行移動と回転）で誤差量を評価して、誤差量が小さくなる新たな変換行列を探して、それを繰り返す。
上のアルゴリズムに加えて、収束を加速するためにエクストラポレーションを追加する。エクストラポレーションについては、「Besl 92」参照（訳注：まだ読んでない）。
上記とは別だが、初期位置を適当に変えて何度か収束演算を行い、その中でベストなものを最終結果とする。これは局所解に陥らないようにするためである。主にpoint-to-pointの誤差量を使った場合に使う。

Fig15が、フラクタルランドスケープシーンでの試験結果。Point-to-plane＋エクストラポレーションが1番よい。次が、point-to-plane。次が、point-to-point＋エクストラポレーション。point-to-pointのみが最もよくない。
Fig16が、プラスプレーンシーンでの試験結果。Point-to-plane＋エクストラポレーションが1番よい。次がpoint-to-plane。次がpoint-to-point＋エクストラポレーションとpoint-to-pointのみは、解が求まっていない。

高速なICPアルゴリズム

計算量が多いのは、Matchingで、検索しなくてよいのでプロジェクションを選択しました。あと収束に大きく影響があるのでError Metricは、point-to-plane。それ以外は、シンプルなものを選びました。

Selectionでは、ランダムサンプリング。
Matchingでは、プロジェクション。
Weightingは、コンスタント。
Rejectingは、一定値の閾値での除外
Error Metricsは、point-to-plane。
Minimizationは、標準的なselect-match-minimizeとして、オーバーシュート（行き過ぎ）の可能性があるため、エクストラポレーションはつけず。

まとめ

論文の概説は、こんなところ。
結局、select-match-minimizeが具体的にどう平行移動と回転を決めるのかは書いていなかった。でもプロジェクションが高速！というところとか、rejectが必要とか、point-to-planeの方がよいとか、論文読まなければ違う実装していただろうな。というか、とっても役に立つ論文でした。
実装のほうは、おおむねできてきました。もうちょっと精度を上げたらまた報告します。
実装してわかったことは、Top-URGのスキャンログは、100msごとにあるのだが、この間隔で、前のスキャンと今のスキャンのポリラインをICPでマッチングさせると、回転成分はよい結果が得られるが、平行移動成分は誤差が多くて（というかあまり変化がなくて）いまいち使えませんでした。まだ、詳細な原因を追究しきれてないけど、恐らくTop-URG(UTM-30LX)自体が持つ誤差が数mmで、100msごとのスキャンデータの場合は、ロボットがそれほど動いていない（動いた量も数mm）で、誤差に埋もれて平行移動量が求まらない感じです。１スキャン前との比較ではなく、数10スキャン前との比較等だったら平行移動成分も求まると思います（未試験）。
ま、でも、当初の計画通り、併進成分はロータリーエンコーダによる位置推定、回転成分はTop-URG(UTM-30LX)による補正という形である程度誤差を抑えられそうな感じが見えてきました。
ちなみに今ICPマッチングを実装中ですが、MatrixクラスとかVectorクラスとかLineクラスから作っていて、そんな基本クラス側にもバグが潜んでいて、思ったよりも時間がかかりました。

あー、時間がない。
つくばチャレンジ2008まであと13日！

2010/03/06追記：LRFの点群マップ