Efficient Velodyne SLAM with point and plane features

This paper develops and tests a plane based simultaneous localization and mapping algorithm capable of processing the uneven sampling density of Velodyne-style scanning LiDAR sensors in real-time. The algorithm uses an efficient plane detector to rapidly provide stable features, both for localization and as landmarks in a graph-based SLAM. When planes cannot be detected or when they provide insufficient support for localization, a novel constraint tracking algorithm selects a minimal set of supplemental point features to be provided to the localization solver. Several difficult indoor and outdoor datasets, totaling 6981 scans, each with approximate to 70,000 points, are used to analyze the performance of the algorithm without the aid of any additional sensors. The results are compared to two competing state-of-the-art algorithms, GICP and LOAM, showing up to an order of magnitude faster runtime and superior accuracy on all datasets, with loop closure errors of 0.14-0.95m, compared to 0.44-66.11m.