Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration

The Iterative Closest Point (ICP) algorithm is one of the most widely used methods for point-set registration. However, being based on local iterative optimization, ICP is known to be susceptible to local minima. Its performance critically relies on the quality of the initialization and only local optimality is guaranteed. This paper presents the first globally optimal algorithm, named Go-ICP, for Euclidean (rigid) registration of two 3D point-sets under the <inline-formula><tex-math notation="LaTeX">$L_2$</tex-math><alternatives> <inline-graphic xlink:type="simple" xlink:href="yang-ieq1-2513405.gif"/></alternatives></inline-formula> error metric defined in ICP. The Go-ICP method is based on a branch-and-bound scheme that searches the entire 3D motion space <inline-formula> <tex-math notation="LaTeX">$SE(3)$</tex-math><alternatives> <inline-graphic xlink:type="simple" xlink:href="yang-ieq2-2513405.gif"/></alternatives></inline-formula>. By exploiting the special structure of <inline-formula><tex-math notation="LaTeX">$SE(3)$</tex-math><alternatives> <inline-graphic xlink:type="simple" xlink:href="yang-ieq3-2513405.gif"/></alternatives></inline-formula> geometry, we derive novel upper and lower bounds for the registration error function. Local ICP is integrated into the BnB scheme, which speeds up the new method while guaranteeing global optimality. We also discuss extensions, addressing the issue of outlier robustness. The evaluation demonstrates that the proposed method is able to produce reliable registration results regardless of the initialization. Go-ICP can be applied in scenarios where an optimal solution is desirable or where a good initialization is not always available.