Direct 6-DoF Pose Estimation from Point-Plane Correspondences

Localizing a mobile sensor in an indoor environment usually involves obtaining 3D scans of the environment and estimating the sensor pose by matching the successive scans. This can be done effectively by minimizing point-plane distances for which only iterative solutions are available. Iterative solutions are notorious for convergence issues, and are inefficient for long sequences of scans. This paper presents a direct method for estimating 6-dof pose of a sensor by minimizing point-plane distances. Through experimental evaluation it is shown that the direct method gives accurate estimates, and performs robustly in presence of noise. The performance of the direct method is also evaluated with point clouds of different scale, poor plane configurations and large numbers of points and planes. A MATLAB implementation of the direct solution is available at: http://people.eng.unimelb.edu.au/kkhoshelham/research.html#directmotion.