Efficient Sparse ICP

The registration of two geometric surfaces is typically addressed using variants of the Iterative Closest Point (ICP) algorithm. The Sparse ICP method formulates the problem using sparsity-inducing norms, significantly improving the resilience of the registration process to large amounts of noise and outliers, but introduces a significant performance degradation. In this paper we first identify the reasons for this performance degradation and propose a hybrid optimization system that combines a Simulated Annealing search along with the standard Sparse ICP, in order to solve the underlying optimization problem more efficiently. We also provide several insights on how to further improve the overall efficiency by using a combination of approximate distance queries, parallel execution and uniform subsampling. The resulting method provides cumulative performance gain of more than one order of magnitude, as demonstrated through the registration of partially overlapping scans with various degrees of noise and outliers.