Using extended measurements and scene merging for efficient and robust point cloud registration

Point cloud registration is a fundamental building block of many robotic applications. In this paper we describe a system to solve the registration problem, that builds on top of our previous work (Serafin and Grisetti (2015)), and that represents an extension to the well known Iterative Closest Point (ICP) algorithm. Our approach combines recent achievements on optimization by using an extended point representation (Serafin and Grisetti (2014)) that captures the surface characteristics around the points. Thanks to an effective strategy to search for correspondences, our method can operate on-line and cope with measurements gathered with an heterogeneous set of range and depth sensors. By using an efficient map-merging procedure our approach can quickly update the tracked scene and handle dynamic aspects. We also introduce an approximated variant of our method that runs at twice the speed of our full implementation. Experiments performed on a large publicly available benchmarking dataset show that our approach performs better with respect to other state-of-the art methods. In most of the tests considered, our algorithm has been able to obtain a translational and rotational relative error of respectively cm and 1 de