  Real-time six degree-of-freedom pose estimation with ground vehicles
represents a relevant and well studied topic in robotics, due to its many
applications, such as autonomous driving and 3D mapping. Although some systems
exist already, they are either not accurate or they struggle in real-time
setting. In this paper, we propose a fast, accurate and modular LiDAR SLAM
system for both batch and online estimation. We first apply downsampling and
outlier removal, to filter out noise and reduce the size of the input point
clouds. Filtered clouds are then used for pose tracking and floor detection, to
ground-optimize the estimated trajectory. The availability of a pre-tracker,
working in parallel with the filtering process, allows to obtain pre-computed
odometries, to be used as aids when performing tracking. Efficient loop closure
and pose optimization, achieved through a g2o pose graph, are the last steps of
the proposed SLAM pipeline. We compare the performance of our system with
state-of-the-art point cloud based methods, LOAM, LeGO-LOAM, A-LOAM,
LeGO-LOAM-BOR and HDL, and show that the proposed system achieves equal or
better accuracy and can easily handle even cases without loops. The comparison
is done evaluating the estimated trajectory displacement using the KITTI and
RADIATE datasets.
