Using augmented measurements to improve the convergence of icp

Point cloud registration is an essential part for many robotics applications and this problem is usually addressed using some of the existing variants of the Iterative Closest Point (ICP) algorithm. In this paper we propose a novel variant of the ICP objective function which is minimized while searching for the registration. We show how this new function, which relies not only on the point distance, but also on the difference between surface normals or surface tangents, improves the registration process. Experiments are performed on synthetic data and real standard benchmark datasets, showing that our approach outperforms other state of the art techniques in terms of convergence speed and robustness.