In this paper, a three-dimensional light detection and ranging simultaneous localization and mapping (SLAM) method is proposed that is available for tracking and mapping with 500--1000 Hz processing. The proposed method significantly reduces the number of points used for point cloud registration using a novel ICP metric to speed up the registration process while maintaining accuracy. Point cloud registration with ICP is less accurate when the number of points is reduced because ICP basically minimizes the distance between points. To avoid this problem, symmetric KL-divergence is introduced to the ICP cost that reflects the difference between two probabilistic distributions. The cost includes not only the distance between points but also differences between distribution shapes. The experimental results on the KITTI dataset indicate that the proposed method has high computational efficiency, strongly outperforms other methods, and has similar accuracy to the state-of-the-art SLAM method.
View on arXiv