Continuous vehicle localisation using sparse 3D sensing, kernelised rényi distance and fast Gauss transforms

Abstract

This paper is about estimating a smooth, continuous-time trajectory of a vehicle relative to a prior 3D laser map. We pose the estimation problem as that of finding a sequence of Catmull-Rom splines which optimise the Kernelised Renyi Distance (KRD) between the prior map and live measurements from a 3D laser sensor. Our approach treats the laser measurements as a continual stream of data from a smoothly moving vehicle. We side-step entirely the segmentation and feature matching problems incumbent in traditional point cloud matching algorithms, relying instead on a smooth and well behaved objective function. Importantly our approach admits the exploitation of sensors with modest sampling rates - sensors that take seconds to densely sample the workspace. We show how by appropriate use of the Improved Fast Gauss Transform we can reduce the order of the estimation problem from quadratic (straight forward application of the KRD) to linear. Although in this paper we use 3D laser, our approach is also applicable to vehicles using 2D laser sensing or dense stereo. We demonstrate and evaluate the performance of our approach when estimating the full 6DOF continuous time pose of a road vehicle undertaking over 2.7km of outdoor travel.