Simultaneous localization and mapping with moving object tracking in 3D range data

A Bayesian framework is designed for simultaneous localization and mapping (SLAM) with detection and tracking of moving objects (DATMO) using only 3D range data. Bayesian formulated occupancy grid maps are used to store and represent the occupancy probability of the environment. Two separate maps (static occupancy grid map and dynamic occupancy grid map) are generated and updated at each instance. The static occupancy grid map functions as the global map and is used to localized the platform using iterative closest point, whereas the dynamic occupancy grid map contains all the information of possible dynamic objects which are used by the Probability Hypothesis Density (PHD) filter for multiple target tracking. The robustness of the PHD filter is leveraged to enable the usage of a more aggressive dynamic voxel detection algorithm when constructing the dynamic occupancy grid map. Data augmentation is introduced to compensate for “infinity return” to further improve the framework’s robustness. The proposed framework was tested on mid-end HDL-32E and high-end HDL-64E LiDAR data obtained from Velodyne LiDAR and KITTI Dataset respectively, and has shown promising results for both cases.