グローバル最適なIterative Closest Point(ICP)アルゴリズム”Go-ICP”についての解説です． ICPは有名なところではKinect Fusionなど，3次元点群のレジストレーションに広く用いられています．基本的には，最近傍点の対応付けと姿勢(並進と回転)の推定を順次繰り返していくことで，点群間の2乗誤差を最小にするような姿勢を見つけようとアルゴリズムです． ICPは原理上，点群間の姿勢がある程度離れると局所解に陥ってしまい正しくレジストレーションできないという課題があります． そこで，局所性を改善したアルゴリズム(SoftAssign など)や，ローカルな特徴(Spin Imageや4PCSなど)を用いて大域的にレジストレーションする手法など多様なアルゴリズムが提案されていますが，グローバル”最適”な点群のレジストレーションアルゴリズムはあまり提案されていませんでした． 2013 ICCV,2015 PAMIの論文でグローバル最適なICPアルゴリズムGo-ICP(http://iitlab.bit.edu.cn/mcislab/~yangjiaolong/go-icp/)が発表されました．詳しくはスライドで解説していますが，Branch and Bound(分枝限定)とVoxel ICPの組み合わせで効率的にSE(3)の6自由度パラメータ空間から最適な解を探します． 著者のページ(http://iitlab.bit.edu.cn/mcislab/~yangjiaolong/go-icp/)にソースコードが公開されていますので簡単に試すことができます．公開されているソースはテキストファイルから点群データを読み込んで姿勢をテキストファイルで書き出す仕様になっていますが，外部ライブラリの依存がなく簡単にMatlabのmex関数を作成できますので，いろいろ点群を変えて試したい場合はmex化がオススメです．

1. Go-ICP
Globally optimal ICPの解説
(株)デンソーアイティーラボラトリ
関川雄介
本資料は一部，以下の論文中の図表を使用しています．
http://iitlab.bit.edu.cn/mcislab/~yangjiaolong/go-icp/

2. 問題:3次元データのレジストレーション
組み合わせ
回転
並進
入力：テンプレートX,クエリY 出力：姿勢＆組み合わせ
①

3. 本資料で説明する内容
• 3次元データのレジストレーションの難さ
• 普通のICPの原理と課題
• Go−ICPアルゴリズム
– 準備：Branch and Bound Theory
• ナップサック問題
– Go−ICPのポイント
• 高速化と最適値保証のポイント
• BnBによる最適解の探索(上限・下限計算)
• まとめ

4. ICP(Iterative Closest Point)
• ICP:2つのPoint Cloud間の距離を最小にす
るアルゴリズム
– 組み合わせが決まれば姿勢が決まる⇔姿勢が決
まれば組わせが決まる(Chicken and Egg)
– 両方不明なので，組み合わせを暫定決定(最近傍
ペア)し姿勢を決定→繰り返し
– EMアルゴリズムの一種
https://en.wikipedia.org/wiki/Iterative_closest_point
ALGORITHM ICP
1.並進（重心）→回転（SVD）の推定
2.クエリ点群をワープ
3.ペアの決定（最近傍点）
4.繰り返し

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

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

7. 再掲:3次元データのレジストレーション
あらゆる組合せPにいて最適解をどう探す？
入力：テンプレートX,クエリY 出力：姿勢＆組み合わせ
①

8. ナイーブな方針
• 方針1：組合せを攻める
– すべての組み合わせPを試す
– 😂組み合わせが爆発
• 100個づつの点群でも10!パターン
• 方針2：姿勢パラメータを攻める
– すべてのパラメータ(SE(3))を試す
– 😂膨大なパラメータ
• 各パラメータを512分割した場合5126パラメータ
いずれのアプローチも，
計算量が膨大すぎて実現不可能

9. 様々の手法が提案
• Robustified Local Methods
– ICPの局所性を改善(重み付きで対応を行う
SoftAssing等)
• Global Methods
– ローカル特徴・点群を利用(Spin Image/Point
Pair Feature/4PCS)
– グローバル特徴(Extended Gaussian Image)
Global Optimal な問題は非常に困難
アルゴリズムが近年まで存在しなかった
依然として初期値に依存
必ずしも①の最適解と一
致しない

10. 近年この問題を解くアルゴが提案
• 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との
組み合わせにより最適かつ高速化

11. Go-ICP(2015 PAMI)
Bunch And Bound(BnB)とVoxel ICPの組み
合わせによりSE(3)の解空間を効率的に探索
BnBで解空間を分割しなが
ら探索
ICPの組合せにより，最適
化を効率的に更新

12. 準備: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から一部抜粋&加筆]

13. 準備: 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のみを計算

14. 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()は上限値
*

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

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

17. BnBアルゴリズム
回転 並進
回転/並進それぞれを23個のセルに順次分割
(Nested BnB:回転を固定し並進の最適値を探索)
ICPによる最適値の更新
下限(LB)が低い=最適解の可能
性が高いセルを分割し探索
LBと現状の最適値からグローバル最適の
見込みがないセルの探索を終了

18. 上限(UB)・下限(LB)の計算
LB:エリア内で振動したときに，良くてもでもコレ以上のコストになる
UB:このエリアでは，悪くてもコレ以下のコストは保証する
Pertervation
回転の不確定半径 並進の不確定半径
重要ポイント BnBの方針
LBが小さいセル(最適値の可能性あり)を優先的に攻め，LBが現
在の最適値より大きくなればそのセルの探索を終了
セルが大きいとLBは0に近い値になる
セルが小さくなるとPertervationが小さくなるので，LBがタイト（大
きな値）になり棄却しやすくなる
※LBの証明は論文の4章を参照

19. 処理の進捗
UB，LBがだんだ
んタイトになる
セル数は最初は増大するけ
ど，処理が進むとLBが大きい
セルを棄却するので減少する

20. まとめ
• Go−ICP:対応点不明の3次元データのグ
ローバル最適なレジストレーション
– BnBによる最適性を保証した効率的な探索
– ICP(Voxel ICP)を使った高速な解の更新
• 特徴・課題
– 😃グローバル最適
– 😂処理時間が一定でない(初期値依存)
– 😂アウトライアーがある場合は計算量大＆追加
パラメータ(詳細は論文5.3章)

21. 参考資料
• 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)