PCE-SLAM: A real-time simultaneous localization and mapping using LiDAR data

This paper describes a real-time design, implementation and validation of a LiDAR-based Simultaneous Localization and Mapping solution for intelligent vehicles. We propose a two-step Sweep-to-Sweep Motion Estimation and Sweep-to-Map Registration framework that compensates for the distortion of the point cloud, estimates the vehicle's motion and generates a 3D map of the world. The rotating motion of LiDAR and the longitudinal motion of the vehicle together create an inherent distortion in relative motions observed in each scan per sweep. Hence applying the same translation and rotation values to the entire sweep does not guarantee the optimum estimation for LiDAR's relative motion from sweep to sweep. Therefore, the key idea in this paper is to obtain accurate localization of the vehicle by processing LiDAR sweep in a batch-wise fashion followed by 3D Map Registration using Iterative Closest Point (ICP). To reduce drift in localization ICP utilizes local map information in a radius of about 100m from the position of vehicle. Our main contribution is to introduce an innovative CPU-only pipeline for simultaneous localization and mapping that runs real-time on Intel architecture. We have tested our algorithm by processing every sweep from Velodyne VLP-16 LiDAR at about 50ms on vehicle moving at speeds up-to 25mph in urban roads and parking lot structures. Our algorithm has been evaluated on KITTI datasets for city and suburban roads with an average relative position error of around 1%.