Dynamic Vehicle Pose Estimation with Heuristic L-Shape Fitting and Grid-Based Particle Filter

Abstract

Vehicle pose estimation with LIDAR plays a crucial role in autonomous driving systems. It serves as the fundamental basis for functions such as tracking, path planning, and decision-making. However, the majority of current vehicle pose estimation techniques struggle to produce satisfactory results when faced with incomplete observation measurements, such as L-shaped point cloud clusters without side contours or those including side-view mirrors. In addition, the requirement for real-time results further increases the difficulty of the pose estimation task. In this paper, we present a vehicle Pose Estimation method with Heuristic L-shape fitting and grid-based Particle Filter (PE-HL-PF). We design a geometric shape classifier module to divide clusters into symmetrical and asymmetrical ones according to their shape features. Furthermore, a contour-based heuristic L-shape fitting module is introduced for asymmetrical clusters, and a structure-aware grid-based particle filter is used to estimate the pose of symmetrical clusters. PE-HL-PF first utilizes a heuristic asymmetrical module that selects dominant contours fitting orientation in a heuristic manner, thereby avoiding the need for a complex traversal search. Additionally, a symmetrical module based on particle filtering is incorporated to enhance the stability of orientation estimation. This method achieves significant improvements in both the runtime efficiency and pose estimation accuracy of incomplete point clouds. Compared with state-of-the-art pose estimation methods, our PE-HL-PF demonstrates a notable performance improvement. Our method can estimate the pose of thousands of objects in less than 1 millisecond, a significant improvement over previous methods. The results of experiments performed on the KITTI dataset validate the effectiveness of our approach.