1 Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration Jiaolong Yang, Hongdong Li, Dylan Campbell, and Yunde Jia Abstract—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 L2 error metric defined in ICP. The Go-ICP method is based on a branch-and-bound (BnB) scheme that searches the entire 3D motion space SE(3). By exploiting the special structure of SE(3) 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. Index Terms—3D point-set registration, global optimization, branch-and-bound, SE(3) space search, iterative closest point F 1 INTRODUCTION given an initial transformation (rotation and transla- Point-set registration is a fundamental problem in tion), it alternates between building closest-point cor- computer and robot vision. Given two sets of points respondences under the current transformation and in different coordinate systems, or equivalently in the estimating the transformation with these correspon- same coordinate system with different poses, the goal dences, until convergence. Appealingly, point-to-point is to find the transformation that best aligns one of ICP is able to work directly on the raw point-sets, the point-sets to the other. Point-set registration plays regardless of their intrinsic properties (such as distri- an important role in many vision applications. Given bution, density and noise level). Due to its conceptual multiple partial scans of an object, it can be applied simplicity, high usability and good performance in to merge them into a complete 3D model [1], [2]. In practice, ICP and its variants are very popular and object recognition, fitness scores of a query object with have been successfully applied in numerous real- respect to existing model objects can be measured world tasks ( [7], [14], [15], for example). with registration results [3], [4]. In robot navigation, However, ICP is also known for its susceptibility to localization can be achieved by registering the current the problem of local minima, due to the non-convexity view into the global environment [5], [6]. Given cross- of the problem as well as the local iterative procedure arXiv:1605.03344v1 [cs.CV] 11 May 2016 modality data acquired from different sensors with it adopts. Being an iterative method, it requires a good complementary information, registration can be used initialization, without which the algorithm may easily to fuse the data [7], [8] or determine the relative poses become trapped in a local minimum. If this occurs, between these sensors [9], [10]. the solution may be far from the true (optimal) solu- Among the numerous registration methods pro- tion, resulting in erroneous estimation. More critically, posed in literature, the Iterative Closest Point (ICP) there is no reliable way to tell whether or not it is algorithm [11], [12], [13], introduced in the early 1990s, trapped in a local minimum. is the most well-known algorithm for efficiently regis- To deal with the issue of local minima, previous tering two 2D or 3D point-sets under Euclidean (rigid) efforts have been devoted to widening the basin of transformation. Its concept is simple and intuitive: convergence [16], [17], performing heuristic and non- deterministic global search [18], [19] and utilizing • J. Yang is with the Beijing Lab of Intelligent Information Technology, other methods for coarse initial alignment [20], [21], Beijing Institute of Technology (BIT), China, and the Australian Na- etc. However, global optimality cannot be guaranteed tional University (ANU), Australia. Email: [email protected]. with these approaches. Furthermore, some methods, • H. Li and D. Campbell are with the Australian National University (ANU), and National ICT Australia (NICTA). such as those based on feature matching, are not Email: [email protected]; [email protected]. always reliable or even applicable when the point-sets • Y. Jia is with the Beijing Lab of Intelligent Information Technology, Bei- are not sampled densely from smooth surfaces. jing Institute of Technology (BIT), China. Email: [email protected]. This work is, to the best of our knowledge, the 2 first to propose a globally optimal solution to the It was shown by Jian and Vemuri [29] that if Euclidean registration problem defined by ICP in 3D. the point-sets are represented with Gaussian Mix- The proposed method always produces the exact and ture Models (GMMs), ICP is related to minimizing globally optimal solution, up to the desired accuracy. the Kullback-Leibler divergence of two GMMs. Al- Our method is named the Globally Optimal ICP, abbre- though improved robustness to outliers and poor viated to Go-ICP. initializations could be achieved by GMM-based tech- We base the Go-ICP method on the well-established niques [17], [29], [30], [31], the optimization was still Branch-and-Bound (BnB) theory for global opti- based on local search. Earlier than these works, Ran- mization. Nevertheless, choosing a suitable domain garajan et al. [32] presented a SoftAssign algorithm parametrization for building a tree structure in BnB which assigned Gaussian weights to the points and and, more importantly, deriving efficient error bounds applied deterministic annealing on the Gaussian vari- based on the parametrization are both non-trivial. ance. Granger and Pennec [33] proposed an algo- Our solution is inspired by the SO(3) space search rithm named Multi-scale EM-ICP where an anneal- technique proposed in Hartley and Kahl [22] as well ing scheme on GMM variance was also used. Biber as Li and Hartley [23]. We extend it to SE(3) space and Straßer [34] developed the Normal Distributions search and derive novel bounds of the 3D registration Transform (NDT) method, where Gaussian models error. Another feature of the Go-ICP method is that were defined for uniform cells in a spatial grid. Mag- we employ, as a subroutine, the conventional (local) nusson et al. [35] experimentally showed that NDT ICP algorithm within the BnB search procedure. The was more robust to poor initial alignments than ICP. algorithmic structure of the proposed method can be Some methods extend ICP by robustifying the dis- summarized as follows. tance between points. For example, Sharp et al. [36] proposed the additional use of invariant feature de- Use BnB to search the space of SE(3) scriptor distance; Johnson and Kang [37] exploited Whenever a better solution is found, call ICP color distances to boost the performance. initialized at this solution to refine (reduce) the Global Methods. To address the local minima prob- objective function value. Use ICP’s result as an lem, global registration methods have also been inves- updated upper bound to continue the BnB. tigated. A typical family adopts stochastic optimiza- Until convergence. tion such as Genetic Algorithms [19], [38], Particle Swam Optimization [39], Particle Filtering [18] and Our error metric strictly follows that of the original Simulated Annealing schemes [1], [40]. While the local minima issue is effectively alleviated, global optimal- ICP algorithm, that is, minimizing the L2 norm of the closest-point residual vector. We also show how a ity cannot be guaranteed and initializations still need trimming strategy can be utilized to handle outliers. to be reasonably good as otherwise the parameter With small effort, one can also extend the method with space is too large for the heuristic search. robust kernels or robust norms. A preliminary version Another class of global registration methods intro- of this work was presented as a conference paper [24]. duces shape descriptors for coarse alignment. Local descriptors, such as Spin Images [3], Shape Con- texts [4], Integral Volume [41] and Point Feature His- 1.1 Previous Work tograms [20] are invariant under specific transfor- mations. They can be used to build sparse feature There is a large volume of work published on ICP correspondences, based on which the best transfor- and other registration techniques, precluding us from mation can be found with random sampling [20], giving a comprehensive list. Therefore, we will focus greedy algorithms [3], Hough Transforms [42] or BnB below on some relevant Euclidean registration works algorithms [41], [43]. Global shape descriptors, such addressing the local minimum issue in 2D or 3D. For as Extended Gaussian Images (EGI) [21], can be used other papers, the reader is referred to two surveys to find the best transformation maximizing descriptor on ICP variants [6], [25], a recent survey on 3D point correlation. These methods are often robust and can cloud and mesh registration [26], an overview of 3D efficiently register surfaces where the descriptor can registration [27] and the references therein. be readily computed. Robustified Local Methods. To improve the robust- Random sampling schemes such as RANSAC [44] ness of ICP to poor initializations, previous work has can also be used to register raw point clouds di- attempted to enlarge the basin of convergence by rectly. Irani and Raghavan [45] randomly sampled smoothing out the objective function. Fitzgibbon [16] 2-point bases to align 2D point-sets using similarity proposed the LM-ICP method where the ICP error transformations.