Point-to-hyperplane ICP: fusing different metric measurements for pose estimation

The objective of this article is to provide a generalized framework of a novel method that investigates the problem of combining and fusing different types of measurements for pose estimation. The proposed method allows to jointly minimize the different metric errors as a single measurement vector in n-dimensions without requiring a scaling factor to tune their importance. This paper is an extended version of previous works that introduced the Point-to-hyperplane Iterative Closest Point (ICP) approach. In this approach, an increased convergence domain and a faster alignment were demonstrated by considering a four-dimensional measurement vector (3D Euclidean points + Intensity). The method has the advantages of the classic point-to-plane ICP method, but extends this to higher dimensions. For demonstration purposes, this paper will focus on a RGB-D sensor that provides colour and depth measurements simultaneously and an optimal error in higher dimensions will be minimized from this. Results on both, simulated and real environments will be provided and the performance of the proposed method will be carried on real-time visual SLAM.