  Probabilistic point cloud registration methods are becoming more popular
because of their robustness. However, unlike point-to-plane variants of
iterative closest point (ICP) which incorporate local surface geometric
information such as surface normals, most probabilistic methods (e.g., coherent
point drift (CPD)) ignore such information and build Gaussian mixture models
(GMMs) with isotropic Gaussian covariances. This results in sphere-like GMM
components which only penalize the point-to-point distance between the two
point clouds. In this paper, we propose a novel method called CPD with Local
Surface Geometry (LSG-CPD) for rigid point cloud registration. Our method
adaptively adds different levels of point-to-plane penalization on top of the
point-to-point penalization based on the flatness of the local surface. This
results in GMM components with anisotropic covariances. We formulate point
cloud registration as a maximum likelihood estimation (MLE) problem and solve
it with the Expectation-Maximization (EM) algorithm. In the E step, we
demonstrate that the computation can be recast into simple matrix manipulations
and efficiently computed on a GPU. In the M step, we perform an unconstrained
optimization on a matrix Lie group to efficiently update the rigid
transformation of the registration. The proposed method outperforms
state-of-the-art algorithms in terms of accuracy and robustness on various
datasets captured with range scanners, RGBD cameras, and LiDARs. Also, it is
significantly faster than modern implementations of CPD. The source code is
available at https://github.com/ChirikjianLab/LSG-CPD.git.
