Uncertainty-Aware Adaptive Semidirect LiDAR Scan Matching for Ground Vehicle Positioning

Abstract

Iterative Closest Point (ICP) algorithms are widely used in the literature for the estimation of relative transformations using 3D LiDAR point clouds. This class of algorithms proves to be efficient when the 3D data share sufficient overlapping parts and a good initial guess is provided. However, large relative motions and mutual occlusion of objects in real-road scenarios hinder traditional optimization-based ICP from achieving optimal estimation. This paper explores both direct and feature-based 3D LiDAR scan matching using the ICP framework in different contexts, such as parking, residential, urban, and highway scenarios. In order to guarantee the scan matching performances in scenarios with scarce geometric information and fast ego-vehicle motion, we propose an adaptive semi-direct scan matching method together with an alignment uncertainty quantification. The proposed semi-direct scan matching is tested on both the public KITTI and self-recorded LS2N datasets, which accomplishes the robust 6 Degrees of Freedom (DoF) pose estimation and consistent scene reconstruction. We demonstrate that the proposed approach outperforms the state-of-the-art and achieves the leading results with 68.3% average relative fitness and 5.71 cm average RMSE, respectively.