Go-ICPGlobally optimal ICP の解説

( 株 ) デンソーアイティーラボラトリ関川雄介

本資料は一部，以下の論文中の図表を使用しています．http://iitlab.bit.edu.cn/mcislab/~yangjiaolong/go-icp/

問題 :3 次元データのレジストレーション

組み合わせ 回転

並進

入力：テンプレート X, クエリ Y 出力：姿勢＆組み合わせ

①

本資料で説明する内容• 3 次元データのレジストレーションの難さ• 普通の ICP の原理と課題• Go−ICP アルゴリズム–準備： Branch and Bound Theory• ナップサック問題

– Go−ICP のポイント• 高速化と最適値保証のポイント• BnB による最適解の探索 ( 上限・下限計算 )

• まとめ

ICP(Iterative Closest Point)• ICP:2 つの Point Cloud 間の距離を最小に

するアルゴリズム–組み合わせが決まれば姿勢が決まる⇔姿勢が決

まれば組わせが決まる (Chicken and Egg)–両方不明なので，組み合わせを暫定決定 ( 最近

傍ペア ) し姿勢を決定→繰り返し– EM アルゴリズムの一種

https://en.wikipedia.org/wiki/Iterative_closest_point

ALGORITHM 　 ICP1. 並進（重心）→回転（ SVD ）の推定2. クエリ点群をワープ3. ペアの決定（最近傍点）4. 繰り返し

ICP の課題 ( 概要 )コスト関数がパラメータ空間で多峰性→ 局所解にトラップされやすい

局所解に収束 大域解に収束

ICP の課題 (1D の具体例 )• 最小二乗誤差は一般に非凸

Non- Convex

再掲 :3 次元データのレジストレーションあらゆる組合せ P にいて最適解をどう探す？

入力：テンプレート X, クエリ Y 出力：姿勢＆組み合わせ

①

ナイーブな方針• 方針 1 ：組合せを攻める–すべての組み合わせ P を試す–😂組み合わせが爆発　• 100 個づつの点群でも 10! パターン

• 方針 2 ：姿勢パラメータを攻める–すべてのパラメータ (SE(3)) を試す–😂膨大なパラメータ• 各パラメータを 512 分割した場合 5126 パラメータ

いずれのアプローチも，計算量が膨大すぎて実現不可能

様々の手法が提案• Robustified Local Methods– ICP の局所性を改善 ( 重み付きで対応を行う

SoftAssing 等 )• Global Methods–ローカル特徴・点群を利用 (Spin Image/Point

Pair Feature/4PCS)–グローバル特徴 (Extended Gaussian Image)

Global Optimal な問題は非常に困難アルゴリズムが近年まで存在しなかった

依然として初期値に依存

必ずしも①の最適解と一致しない

近年この問題を解くアルゴが提案• Branch and Bound Theory に基づいた非

凸最適化

タイトル 概要The 3D-3D Registration Problem Revisited(2007 ICCV) BnB で最適化による SO(3) の最適

化Lipschitz optimization を応用

Go-ICP Globally optimal ICP (2013 ICCV 2015 PAMI) SE(3) への拡張　 BnB と Voxel ICP

との組み合わせにより最適かつ高速化

Go-ICP(2015 PAMI)Bunch And Bound(BnB) と Voxel ICP の組み合わせにより SE(3) の解空間を効率的に探索

BnB で解空間を分割しながら探索 ICP の組合せにより，最適化を効率的に更新

準備 :Branch and Bound Theory って？

• Branch and bound is an algorithm design paradigm for discrete and combinatorial optimization problems, …

• The algorithm explores branches of this tree, which represent subsets of the solution set.

• Before enumerating the candidate solutions of a branch, the branch is checked against upper and (lower) bounds on the optimal solution, and is discarded if it cannot produce a better solution than the best one found so far.

• The algorithm depends on the efficient estimation of the (lower) and upper bounds of a region/branch of the search space [Wikipediaから一部抜粋 & 加筆 ]

準備 : BnB の例 ナップサック問題• 問題：容量 100Kg のバッグに Value が最

大になるように Item を詰めたい• 条件： Item は分割 NG→ 離散最適問題

A BRANCH AND BOUND ALGORITHM FOR THE KNAPSACK PROBLEM*t PETER J. KOLESAR (Peter J 1967)

容量 100Kg のナップサック

Item の Weight と Value

愚直にやると 7 つの Item を入れる / 入れないで　27 通り試す必要あり

※ ナップサック問題は Upper Bound のみを計算

BnB による解き方の方針：1. 効率が良い Item=Value/Weight が高い Item を優先的にいれる2. 順次 Item を確定し， Item の分割を許した場合の Value の上限 (UB) を計算3. グローバル最適の見込が高い =UB が高い Node を優先的に攻める4.UB と確定値からグローバル最適 ( 最大 ) になる見込みが無い Node は捨てる

① アイテム 1(New Index) を入れないと確定② アイテム 1(New Index) を入れると判断したときの UB

③④ こっちのノードの方が UB が大きいので優先的に探索

途中で UB が確定値より小さいことがわかる→攻める必要なし

図 ノード内の数字は確定した Item ID で * は入れないことを確定した item ID. 　 B() は上限値

*

Go−ICP : 高速化のポイント• BnB と ICP の組み合わせ– ICP ：効率的に局所解まで到達– BnB ：下限値に基づき攻めるべき解空間を決定

BnB によるパラメータ空間の探索SE(3) を 26 個のセルに順次分割　回転 / 並進を順次最適化 : 回転 SO(3) を 23 セルに分割 , 並進 R3 を 23 セルに分割

BnB アルゴリズム

回転 並進

回転 / 並進それぞれを 23 個のセルに順次分割(Nested BnB: 回転を固定し並進の最適値を探索 )

ICP による最適値の更新

下限 (LB)が低い =最適解の可能性が高いセルを分割し探索

LB と現状の最適値からグローバル最適の見込みがないセルの探索を終了

上限 (UB) ・下限 (LB) の計算

LB: エリア内で振動したときに，良くてもでもコレ以上のコストになる

UB: このエリアでは，悪くてもコレ以下のコストは保証する

Pertervation

回転の不確定半径 並進の不確定半径

重要ポイント　 BnBの方針LB が小さいセル ( 最適値の可能性あり ) を優先的に攻め， LB が現在の最適値より大きくなればそのセルの探索を終了セルが大きいと LB は 0 に近い値になるセルが小さくなると Pertervation が小さくなるので， LB がタイト（大きな値）になり棄却しやすくなる

※LB の証明は論文の 4 章を参照

処理の進捗

UB ， LB がだんだんタイトになる

セル数は最初は増大するけど，処理が進むと LB が大きいセルを棄却するので減少する

まとめ• Go−ICP: 対応点不明の 3 次元データのグ

ローバル最適なレジストレーション– BnB による最適性を保証した効率的な探索– ICP(Voxel ICP) を使った高速な解の更新

• 特徴・課題–😃グローバル最適–😂処理時間が一定でない ( 初期値依存 )–😂アウトライアーがある場合は計算量大＆追

加パラメータ ( 詳細は論文 5.3 章 )

参考資料• Steven Gold+ "New

algorithms for 2D and 3D point matching: pose estimation and correspondence," Pattern Recognition, Vol. 31, No. 8, pp. 1019-1031, 1998

• PETER J +, ”A BRANCH AND BOUND ALGORITHM FOR THE KNAPSACK PROBLEM”

• Yang+,”Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration”

• Makadia, A+ “Fully Automatic Registration of 3D Point Clouds”• Nicolas+ “Super4PCS: Fast Global

Pointcloud Registration via Smart Indexing”• Drost+ “

Model globally, match locally:Efficient and robust 3D object recognition”

• ICP(Wikipedia)