Benchmarking Particle Filter Algorithms for Efficient Velodyne-Based Vehicle Localization

TL;DR

The paper addresses robust real-time localization of a vehicle on a prebuilt 3D map using raw Velodyne scans without feature extraction. It benchmarks two particle-filter frameworks—the standard SIR and a rejection-based optimal proposal—across decimation factors and global-relocation scenarios. Key findings show that decimating LiDAR points by a factor of 100–200 preserves localization quality while enabling updates in 10–20 ms, and that an initial density of about 2 particles per square meter suffices for large outdoor relocalization, with higher densities improving convergence. The work provides practical guidance for deploying PF-based Velodyne localization and releases open-source software to facilitate reproducibility.

Abstract

Keeping a vehicle well-localized within a prebuilt-map is at the core of any autonomous vehicle navigation system. In this work, we show that both standard SIR sampling and rejection-based optimal sampling are suitable for efficient (10 to 20 ms) real-time pose tracking without feature detection that is using raw point clouds from a 3D LiDAR. Motivated by the large amount of information captured by these sensors, we perform a systematic statistical analysis of how many points are actually required to reach an optimal ratio between efficiency and positioning accuracy. Furthermore, initialization from adverse conditions, e.g., poor GPS signal in urban canyons, we also identify the optimal particle filter settings required to ensure convergence. Our findings include that a decimation factor between 100 and 200 on incoming point clouds provides a large savings in computational cost with a negligible loss in localization accuracy for a VLP-16 scanner. Furthermore, an initial density of $\sim$2 particles/m$^2$ is required to achieve 100% convergence success for large-scale ($\sim$100,000 m$^2$), outdoor global localization without any additional hint from GPS or magnetic field sensors. All implementations have been released as open-source software.

Figures (13)

• Figure 1: Overview of the ground-truth 3D map used in the benchmarks, representing $\sim$100,000 $\text{ m}^2$ of the campus of the University of Almeria, points colorized by height.
• Figure 2: The cost function for regular least squares (a) and for truncated least squares (b) with $\theta=1$. The latter significantly reduces the associated cost of outliers, thus removing their contribution to the actual cost function to be optimized.
• Figure 3: Kullback--Leibler divergence (KLD) between the original and decimated version of the proposed likelihood model for point cloud observations. Note that the decimated versions are remarkably similar to the original one up to decimation ratios of roughly $\sim$500.
• Figure 4: Some planar slices of 6D likelihood functions evaluated for different decimation ratios, as both an intensity color plot and a contour diagram. Ground-truth vehicle pose is marked as a large block dot in all the figures. (a) Original p.d.f. $p(z|x)$; (b) Approximate $\hat{p}(z|x)$ for D = 100; (c) Approximate $\hat{p}(z|x)$ for D = 300; (d) Approximate $\hat{p}(z|x)$ for D = 1000.
• Figure 5: The autonomous vehicle prototype employed in this work.