MegaParticles: Range-based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter
Kenji Koide Shuji Oishi Masashi Yokozuka Atsuhiko Banno



[
    Zuyi Wang
    April 29, 2024
==================

empty
empty







This paper presents a 6-DoF range-based Monte Carlo localization method with a GPU-accelerated Stein particle filter. To update a massive amount of particles, we propose a Gauss-Newton-based Stein variational gradient descent (SVGD) with iterative neighbor particle search. This method uses SVGD to collectively update particle states with gradient and neighborhood information, which provides efficient particle sampling. For an efficient neighbor particle search, it uses locality sensitive hashing and iteratively updates the neighbor list of each particle over time. The neighbor list is then used to propagate the posterior probabilities of particles over the neighbor particle graph. The proposed method is capable of evaluating one million particles in real-time on a single GPU and enables robust pose initialization and re-localization without an initial pose estimate. In experiments, the proposed method showed an extreme robustness to complete sensor occlusion (i.e., kidnapping), and enabled pinpoint sensor localization without any prior information.







§ INTRODUCTION


Reliable sensor localization is crucial for autonomous systems such as service robots and autonomous driving vehicles. In particular, point-cloud-based localization has been widely used in many applications as a result of the emergence of precise and affordable range sensors. Although the recent development of scan-matching-based localization techniques (e.g., sliding window optimization <cit.> and the tight coupling of LiDAR and IMU constraints <cit.>) has significantly improved the pose tracking accuracy and reliability, it is still challenging to deal with cases where no prior knowledge of the sensor position is available (e.g., initial position estimation without GNSS and re-localization after kidnapping).

Monte Carlo localization (MCL) is a category of localization methods that estimate the sensor pose using random-sampling-based probabilistic inference <cit.>. In particular, a particle filter is the most common approach used for 2D LiDAR localization and mapping <cit.>. Owing to its non-linear, non-Gaussian nature, a particle filter enables reliable localization even in cases where single hypothesis algorithms suffer from observational ambiguity and repeated environmental structures. However, because the number of particles required to fill the state space grows exponentially as the dimension increases, it has generally been considered to be difficult to apply the Monte Carlo approach to 6-DoF pose estimation in 3D environments.

Several studies have tackled 6-DoF MCL with carefully designed sampling techniques (e.g., RBPF with rotation translation decoupling <cit.>). While these methods enable real-time 6-DoF pose estimation with fewer particles, it is still difficult to deal with situations where strong ambiguity causes state distributions with many modes. The recently proposed Stein particle filter <cit.> combines a particle filter with Stein variational gradient descent (SVGD) <cit.>. SVGD updates the particle states collectively to improve the sampling efficiency with gradient information while retaining the diversity of the particles. Although a Stein particle filter has been shown to enable 6-DoF pose estimation in a 3D environment with only 50 particles, it requires a costly kernel-based computation that prevents increasing the number of particles.




In this work, we propose a 6-DoF Monte Carlo localization method that combines GPU parallel processing and SVGD-based particle state optimization [See the project page for supplementary videos: <https://staff.aist.go.jp/k.koide/projects/icra2024_mp/>]. The proposed method was designed to fully leverage the GPU acceleration, and it is capable of evaluating and updating a million particles (1024^2 particles) in real-time, as shown in Fig. <ref>. Inspired by <cit.>, we use SVGD <cit.> to update the particle states for efficient sampling. To efficiently perform the SVGD-based collective particle state optimization, we propose an approximated Gauss-Newton SVGD with locality sensitive hashing (LSH)-based neighbor particle search in the SE3 pose space. Furthermore, to quickly estimate and propagate the particle posterior probabilities, we developed a Bayesian filter on a dynamic neighbor particle graph.

To demonstrate the robustness and re-localization ability of the proposed method, we conducted experiments in an indoor environment with repeated and symmetric structures and in a large outdoor environment with dynamic objects and vegetation changes. The experimental results showed that the proposed method could robustly estimate the sensor pose starting from a particle set initialized with a uniform 6-DoF pose distribution. It was also robust to the complete blockage of sensor data (i.e., kidnapping), which is difficult, if not impossible, to overcome with existing scan-matching-based localization methods. Furthermore, the neighbor-particle-graph-based posterior probability estimation enabled pinpoint localization once the sensor moved and the pose ambiguity was resolved.


The main contributions of this work is three-fold:

  
  * We developed an efficient particle matching cost evaluation with nearest neighbor field.
  
  * To efficiently perform stein particle filtering on a GPU, we devised a LSH-based nearest neighbor search method on the SE3 pose space.
  
  * Dynamic nearest neighbor graph-based posterior probability propagation.






§ RELATED WORK





 §.§ Iterative Scan Matching-based Localization


The most common approach to 3D map localization is to iteratively apply point cloud scan matching (e.g., ICP <cit.> and NDT <cit.>) between LiDAR scan and map points. Because these scan matching methods require a precise initial guess for convergence and are error-prone in feature-less environments, they are often integrated with other data sources (e.g., wheel odometry <cit.> and IMU measurements <cit.>) using an extended Kalman filter or a factor graph for robust estimation. 
This approach enables efficient and precise sensor localization owing to the high-definition point cloud data provided by recent range sensors. However, these single hypothesis methods heavily rely on the continuity of the point cloud data. They easily fail to continue tracking the sensor pose when the point cloud data are interrupted. Even a momentary data drop (e.g., 1 s) can cause a critical estimation failure, and it is almost impossible to deal with a longer interruption (i.e., kidnapping) using iterative-scan-matching-based methods.





 §.§ Monte Carlo Localization


MCL is one of the most popular approaches to the 2D localization problem. It represents and estimates a state distribution with a finite set of state samples. In particular, a particle filter is the most representative non-parametric Monte Carlo Bayes filter used to estimate the state distribution through particle importance weighting and resampling <cit.>. Because of its non-linear and non-Gaussian nature, it is robust to ambiguity in the observations and environment. With a sufficient number of samples, it can even enable pinpoint sensor localization without any prior knowledge (i.e., global localization). 

Despite its success in 2D localization, the MCL approach has not been commonly used in 3D scenarios because of the curse of dimensionality. Because the number of samples to fill a unit space grows exponentially with the dimensionality, a massive number of particles is needed, with a large computation cost, to retain the robust properties of the 2D MLC approaches in 3D scenarios. Most of the existing 3D MCL methods perform state estimation in 3 or 4 DoF (<cit.>).

Recently, several studies have proposed 6-DoF MCL methods that smartly sample and evaluate particles. Akai et al. used a 3D distance field for accelerated likelihood evaluation and performed 6-DoF sensor pose estimation with a vehicle motion model <cit.>. Maggio et al., used a NeRF(Neural radiance field)-based map representation and VIO(visual inertial odometry)-based precise motion prediction <cit.>. Deng et al. decoupled the rotation and translation components and estimated a 6-DoF object pose using a Rao-Blackwellized particle filter <cit.>. Maken et al. proposed a Stein particle filter that introduced SVGD to improve the sampling efficiency and demonstrated 4-DoF global localization and 6-DoF pose tracking on a 3D map <cit.>. Although these methods enabled 6-DoF state estimation with only 50-1,000 particles using smart sampling schemes, this number of samples made it difficult to represent a multi-modal distribution in a 6-DoF space. Thus, re-localization and ambiguity handling abilities were limited.




 §.§ GPU-Accelerated Localization


Several studies have used GPU acceleration to achieve fast and robust localization. However, most of them used the GPU for only a portion of the system (e.g., nearest neighbor search <cit.> or occupancy gridmap generation <cit.>).

Our work was inspired by the work of Peng and Weikersdorfer, who proposed 2D localization with a histogram filter that used a belief tensor representing all the possible states <cit.>. The entire system was designed to leverage the GPU computation power, and they showed that a non-parametric filter accelerated with the GPU exhibited an excellent pinpoint global localization ability without any initial state assumption. Inspired by their work, we fully utilized GPU acceleration to achieve 6-DoF localization with a global re-localization ability.




§ METHODOLOGY





 §.§ Problem Setting


Our objective is to estimate SE3 sensor pose T_t on 3D point cloud map ℳ = { p_k^M ∈ℝ^3 |_k = 1, …, N^M} from sensor point cloud measurements 𝒫_t = { p_k^S ∈ℝ^3 |_k = 1, …, N^S}.

As with a conventional particle filter, we estimate the state distribution using a set of samples (i.e., particles) 𝒳_t = { x_t^i |_i = 1, …, N^P} and update the particle states through prediction and correction steps. In our case, each particle x_t^i represents a hypothesis of the sensor pose: x_t^i  T_t^i. Inspired by <cit.>, we employ SVGD-based particle optimization in the correction step for efficient and robust state sampling. After the correction step, we explicitly estimate the posterior probability of each particle through a Bayesian filter over a neighbor particle graph.




 §.§ Prediction Step


To update the particle states in the prediction step, we first perform GICP scan matching <cit.> between consecutive scan point clouds 𝒫_t-1 and 𝒫_t to obtain an estimate of the sensor motion Δ T_t ∼ T_t - 1^-1 T_t. We also estimate the covariance matrix Σ^Δ T∈ℝ^6 × 6 based on the Hessian matrix of the scan matching optimization result.

Then, the states of particles are updated as follows:

    T_t^i    =  T_t-1^i Δ T_t exp( δ^i ),

where δ^i ∼𝒩 (0, Σ^Δ T) is a random noise in the tangent space of Δ T_t. 




 §.§ Correction Step


Likelihood evaluation: We employ the GICP distribution-to-distribution distance <cit.> for the log likelihood function log p(𝒫_t |  x_t^i). In GICP, each point p_k^* is modeled as a Gaussian distribution 𝒩(μ_k^*, Σ_k^*) representing the local geometrical shape. For each scan point p_k^S, we find the nearest map point p_k^M and compute the log likelihood as follows:

    log p(𝒫_t |  x_t^i) = - ∑_k  e_k^⊤Ω_k  e_k, 
        e_k = μ_k^M -  T_t^i μ_k^S,   Ω_k = ( Σ^M_k +  T_t^i Σ^S_k ( T_t^i)^⊤)^-1.


For an efficient nearest map point search, we precompute a nearest neighbor field m^nnf that voxelizes the map space at a specific resolution (e.g., 0.1 m for indoors, 0.2 m for outdoors) and stores the index of the nearest map point for each voxel. When computing the log likelihood, we look up m^nnf to find the nearest map point of each scan point.

Particle state update: We propose Gauss-Newton-based approximate SVGD particle optimization to update particle states with the likelihood function.

Because log p(𝒫_t| x_t^i) is in the least squares form, as in Gauss-Newton optimization, we can obtain an optimal particle displacement vector ψ^i =  H^-1 b to maximize the log likelihood through quadratic approximation, where

    H   = ∑_k  J_k^⊤Ω_k  J_k,     b   = ∑_k  J_k^⊤Ω_k  e_k,     J_k   = ∂ e_k/∂ T_t^i.

Then, we update the states of particles based on modified SVGD:

    T_t + 1^i    =  T_t^i  exp( ϕ(  T_t^i, 𝒫_t ) ), 
    ϕ( T_t^i, 𝒫_t)    = ∑_ x_t^j ∈𝒳_t^i ( k( T^i_t,  T^j_t) ψ^j + ∇_ T_t^j k( T_t^i,  T_t^j) ) /∑_ x_t^j ∈𝒳_t^i  k( T_t^i,  T_t^j) ,

where k is a positive definite kernel and 𝒳_t^i is a set of neighbor particles, which includes x_t^i itself. Kernel k measures the distance between particles and controls how particles affect each other. We use the following exponential kernel with diagonal weighting matrix W^K = diag( [σ_r, σ_r, σ_r, σ_t, σ_t, σ_t] ) (e.g., σ_r = 5.0 rad^-1 and σ_t = 2.5 m^-1):

    k (  T_t^i,  T_t^j )    = exp( - d_ij^⊤ W^K  d_ij), 
     d_ij   = log( ( T_t^i )^-1 T_t^j ).


Intuitively, the component ∑ k( T^i_t,  T_t^j) ψ^j in Eq. <ref> is a weighted sum of the Gauss-Newton update vectors of neighboring particles that pushes particle x_t^i toward the modes of the likelihood function. ∇_ T_t^j k( T_t^i,  T_t^j) is the gradient of the kernel at the particle location that causes repulsive forces between particles and helps maintain the diversity of the particles in the state space. 

The original SVGD computes the update direction of a particle using all the other particles, resulting in a computational burden with a large number of particles. Considering that only neighbor particles have large kernel values and affect the update direction in SVGD, we only use up to K neighbor particles 𝒳_t^i (e.g., 20 neighbors) to compute the update vector in Eq. <ref>.


Our formulation has the following differences from <cit.>:

  
  * We use a Gauss-Newton-based update instead of an L-BFGS-based update for faster convergence.
  
  * We only use K neighbor particles to compute the update vector of a particle. To efficiently find neighbor particles, we use an LSH-based iterative neighbor particle search.
  
  * We directly fit the particles to the likelihood function to avoid costly posterior probability propagation. Instead, we propose a Bayesian filter based on a dynamic neighbor particle graph that can be obtained as a by-product of the neighbor particle search.



LSH-based iterative neighbor particle search: Because we handle a massive amount of dynamically changing particles in the non-Euclidean SE3 space, conventional nearest neighbor methods (e.g., linear search and spatial-partitioning) cannot be applied to our problem.

To efficiently find neighbor particles, we use an iterative neighbor particle search using locality sensitive hashing (LSH) based on stable distributions <cit.>. Algorithm <ref> describes the proposed neighbor particle search algorithm. We first distribute particles into a hash table with N^B buckets using the following hash function (Line 3 – 5):

    f^LSH ( T_t^i)    = hash( [ ⌊ζ[i] ⌋ |_i=1 ⋯ 6 ] ), 
    ζ   = α W^K log( ( T^LSH) ^-1 T_t^i ) + δ_i^LSH,

where hash is a function used to compute a hash value from a tuple of integers <cit.>, T^LSH is a random transformation that defines the tangent space to compute the hash value, δ^LSH∼𝒩( 0, Σ^LSH ) is a random Gaussian noise, and α is a constant. 






As shown in Fig. <ref>, T^LSH defines the grid to discretize transformation T_t^i, and noise δ^LSH causes a fluctuation in the position of T_t^i in the grid space. Because particles with a small displacement tend to fall in the same cell of the grid, f^LSH assigns the same hash value to close particles with a high probability. By using f^LSH, we store all the particles in the hash table and then find particles that fall in a same bucket as neighbor particles. In this way, we can efficiently find neighbor particle candidates for all the particles in time linear to the number of particles (Line 6 – 9).

However, the neighbor particles found will contain false positives and negatives because f^LSH is a probabilistic function. We thus assume that the neighborhood relationship of particles does not significantly change in a short time interval, and we iteratively update the neighbor list 𝒳_t^i of each particle over time by keeping only the K closest particles (Line 9 – 12). 





 §.§ Posterior Propagation on Neighbor Particle Graph


Similar to <cit.>, the SVGD-based correction step does not perform resampling. Because the proposed algorithm keeps all the particles alive, it does not suffer from the sample impoverishment problem <cit.>. Meanwhile, a massive number of particles is obtained with an extremely non-linear distribution with many modes, and explicitly estimating the posterior distribution and representative state of the estimation result is not straight forward. We thus propose a method to explicitly estimate the posterior probability of each particle by propagating probabilities over a neighbor particle graph that is a by-product of the iterative neighbor particle search.

Given prior probability p( x_t^i) of particle x_t^i, we first obtain initial posterior probability p( x_t^i | 𝒫_t) ∝ p( x_t^i) p(𝒫_t |  x_t^i) with likelihood p(𝒫_t |  x_t^i) in Eq. <ref>. We then compute the weighted average of the neighbor particles:

    p' ( x_t^i | 𝒫_t)    = ∑_ x_t^j ∈𝒳_t^i k( T_t^i,  T_t^j) p ( x_t^j | 𝒫_t) /∑_ x_t^j ∈𝒳_t^i k( T_t^i,  T_t^j) .

We iteratively apply Eq. <ref> several times (e.g., 10 times). This process can be interpreted as locally distributing and smoothing the posterior probabilities over the neighbor particle graph under the random walk assumption. The proposed Bayesian filtering approach can also be interpreted as a histogram filter with sparse and dynamic state bins updated with SVGD. After the posterior probability update, we obtain the state of the particle with the highest posterior probability as the representative state.






 §.§ Implementation Details


We implemented the proposed algorithm using NVIDIA CUDA. All the processed in the proposed method are designed to be run on a GPU, and thus our algorithm can perform most of the operations without CPU-GPU synchronization, resulting in drawing mostly 100% computation power of the GPU.





§ EXPERIMENT






 §.§ Indoor Experiment


Experimental setting: To demonstrate the robust initialization and re-localization ability of the proposed method, we conducted experiments in an indoor environment with repeated and symmetric structures. We used an Azure Kinect to acquire point clouds at 10 Hz. We recorded two sequences (Easy01 and Easy02) while walking in a corridor without aggressive motion and data interruption, and two other sequences (Kidnap01 and Kidnap02) with thee long data interruptions (10-20 s) in each by completely blocking the sensor view. Furthermore, the sensor was moved through room after room during the data interruptions, and we believe that none of the existing methods could deal with these severe kidnapping situations. The sequences (Easy01, Easy02, Kidnap01, Kidnap02) had durations of 139, 136, 147, and 109 s, respectively [The dataset is available at : <https://staff.aist.go.jp/k.koide/projects/icra2024_mp/>].

To run the proposed algorithm, we initialized 1024^2 particles with a uniform distribution covering the entire map (50 m × 35 m × 4 m and full SO3 rotation). All the particles were evaluated and updated in real-time on a single GPU (NVIDIA A100).

As a baseline, we ran two localization algorithms based on iterative scan matching, FAST_LIO_LOCALIZATION [<https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION>] and hdl_localization <cit.>. FAST_LIO_LOCALIZATION uses FAST_LIO2, which is a tightly coupled LiDAR-IMU odometry estimation method <cit.>, to estimate the sensor ego-motion and periodically performs scan-to-map registration to correct estimation drift. For comparison, we also ran FAST_LIO2 <cit.> without map-based pose correction. hdl_localization <cit.> performs NDT-based scan-to-map registration and unscented Kalman filter-based IMU fusion. Note that we used IMU data and initial poses only for these existing methods, whereas the proposed method performed localization using only point cloud data without initial guess.

To obtain reference sensor trajectories, we manually aligned scan point clouds with the map point cloud and performed batch optimization of the scan-to-map registration errors and IMU motion errors. We evaluated the estimated trajectories using the absolute trajectory error (ATE) metric <cit.> using the evo toolkit [<https://github.com/MichaelGrupp/evo>]. Because we initialized the proposed method with a uniform distribution (no initial guess), we excluded the beginning of each sequence until the posterior probability distribution converged to a single pose hypothesis (10-30 s).






Initialization: Fig. <ref> shows how the posterior probability distribution of the proposed method converged from a uniform distribution in the Easy01 sequence. We applied weighted kernel density estimation to the 2D particle positions for visualization. Although the particles were uniformly initialized without any prior information (Fig. <ref> (a)), the posterior probability quickly converged around the correct position as the sensor moved (Fig. <ref> (b)(c)). Although there was still orientation ambiguity (upright and flipped) due to the symmetric environment structure (Fig. <ref> (d)), when the sensor made a turn and entered a room (Fig. <ref> (e)), the ambiguity was eventually resolved (Fig. <ref> (f)).

Dealing with kidnapping: Fig. <ref> shows how the proposed method recovered from kidnapping in the Kidnap02 sequence. During the data interruption, the particles were spread around, and the posterior distribution diverged (Fig. <ref> (b)). Meanwhile, the sensor was moved to another room while the sensor view was kept completely blocked. Once the point cloud data became available, the particles spread over the map (Fig. <ref> (c)) and then quickly converged to a few possible locations with structures similar to those in the observed scan points (Fig. <ref> (d) (e)). Eventually, it converged around the correct pose (Fig. <ref> (f)) and recovered from the kidnapping. In these experiments, the proposed method successfully recovered the tracking of the sensor pose after every kidnapping.

As expected, both FAST_LIO_LOCALIZATION and hdl_localization failed to continue tracking the sensor pose in the Kidnap01 and Kidnap02 sequences because of the severe data interruptions, and their estimations eventually became corrupted. These results showed the superior re-localization ability of the proposed method. The non-Gaussian state estimation combined with a massive amount of particles made it possible to search for candidate sensor locations over the entire map, and the SVGD-based particle update made the particles quickly converge to the correct location.



Estimation accuracy: Fig. <ref> shows the estimated trajectories for the Easy02 sequence. Once the posterior distribution converged around the correct pose, the proposed method robustly kept tracking the sensor pose until the end of the sequence. Although the estimation result showed momentary pose jitters because we simply chose the particle with the highest posterior probability as the representative state, these jitters could be filtered out using simple trajectory smoothing.


































Table <ref> summarizes the ATE results for the evaluated methods. We can see that although FAST_LIO without map-based correction showed large errors (1.86 m and 6.16 m) for Easy01 and Easy02, the errors were significantly reduced to 0.07 and 0.14 m with map-based correction, respectively. While hdl_localization showed good accuracy for Easy01 (0.14 m), it showed a worse result for Easy02 (16.8 m) as a result of the tracking failure caused by the feature-less environment. Although the proposed method showed a slightly worse estimation result for the Easy01 sequence due to pose jitters, these results were generally comparable to those of the existing methods (0.25 m and 0.13 m).

While the existing methods became corrupted in the Kidnap01 and Kidnap02 sequences, the proposed method successfully kept tracking the sensor pose through these sequences. Fig. <ref> shows a plot of the ATE results for the proposed method during the Kidnap02 sequence. We can see that, at the beginning, the proposed method quickly converged to the correct location, and the ATE decreased. Although the ATE increased during kidnapping, once the point cloud data became available, the estimation converged to the correct sensor locations and showed small ATE values.



Processing time:  Table <ref> summarizes the processing time of each step in the proposed method. The entire process was highly parallelized and performed on a single GPU in real-time, and it took approximately 91 ms for each scan frame on average.
















 §.§ Outdoor Experiment








Experimental setting: We conducted an experiment in the outdoor environment (280 × 200 × 30 m^3) shown in Fig. <ref>. We used a Livox MID360 to acquire point clouds at 10 Hz. Similar to the indoor experiment, we completely occluded the sensor view eight times during the experiment. As in the indoor experiment, the reference trajectory was obtained through the batch optimization of the registration errors and IMU errors. Note that the scan data were acquired one month after the recording of the map data, and thus there were changes in the dynamic objects (vehicles and pedestrians) and vegetation. The proposed method ran in real-time and took approximately 88.5 ms for each input point cloud.

Estimation result: Fig. <ref> shows how the posterior probability distribution changed during the experiment. Starting with an initial uniform distribution (Fig. <ref> (a)), the particles quickly converged to the correct location in a few seconds (Fig. <ref> (b)). Although the posterior distribution diverged during a data interruption (Fig. <ref> (c)), it quickly converged and recovered once point cloud data became available (Fig. <ref> (d)). Fig. <ref> shows the estimated trajectory. Although the sensor data were interrupted many times, the proposed method successfully recovered the estimation every time. It required only a few seconds to converge to the correct pose in most of cases. It took longer (15 and 32 s) at two places where the map point cloud was cropped and there was surrounding vegetation. We consider that these map defects and changes prevented the likelihood function from being minimized at the correct location and affected the global localization. 














§ CONCLUSION


This paper presented a particle filter for 6-DoF sensor localization with GPU-accelerated SVGD optimization. A massive number of particles updated in parallel using SVGD on a GPU enabled robust initialization and re-localization without any prior information. 




IEEEtran


