Probabilistic Scan Matching: Bayesian Pose Estimation from Point Clouds

Estimating position and orientation change of a mobile platform from two consecutive point clouds provided by a high-resolution sensor is a key problem in autonomous navigation. In particular, scan matching algorithms aim to find the translation and rotation of the platform such that the two point clouds coincide. The association of measurements in point cloud one with measurements in point cloud two is a problem inherent to scan matching. Existing methods perform non-probabilistic data association, i.e., they assume a single association hypothesis. This leads to overconfident pose estimates and reduced estimation accuracy in ambiguous environments. Our probabilistic scan matching approach addresses this issue by considering all association hypotheses with their respective likelihoods. We formulate a holistic Bayesian estimation problem for both data association and pose inference and present the corresponding joint factor graph. Near-optimum maximum a posteriori (MAP) estimates of the sensor pose are computed by performing iterative message passing on the factor graph. Our numerical study shows performance improvements compared to non-probabilistic scan matching methods that are based on the normal distributions transform (NDT) and implicit moving least squares (IMLS).