EM-BASED POINT TO PLANE ICP FOR 3D SIMULTANEOUS LOCALIZATION AND MAPPING

Abstract

D simultaneous localization and mapping (SLAM) is a very impor- tant issue in autonomous robotics. One of the popular algorithms applied as a frontend of SLAM is iterative closest point (ICP). In this paper, the ICP is modelled into a probabilistic framework including both pose estimation and data association steps using expectation maximization (EM). The result derived is that the solu- tion converges to a local minimum if both pose estimation and data association steps employ the same metric. Hence, the measurement model which determines the form of the metric should be the key factor of the algorithm. Then, the point to point, point to plane and plane to plane are analysed in form of their measurement model, which reveals their description of the connection between two scans. Based on analysis, an improvement on point to plane measurement model is presented by estimating the covariance of each plane to relax the model assumption of ICP using eigenvalue decomposition, hence achieving a better solution. The following experiments show a satisfactory performance of the proposed algorithm, in agreement with the theoretic results.