Range-based 6-DoF Monte Carlo SLAM with Gradient-guided Particle Filter on GPU

TL;DR

This work tackles robust 3D SLAM under high ambiguity where Gaussian parametric methods fail and memory becomes prohibitive for dense maps. It introduces a gradient-guided particle update within a 6-DoF Monte Carlo SLAM framework and a compact keyframe map that shares data across 100k particles, enabling real-time operation on GPUs. Key contributions include (i) gradient-driven per-particle updates to move toward the likelihood mode, (ii) a shared keyframe-based map to reduce memory, and (iii) a loop-closure-based correction of past keyframes to maintain trajectory consistency. Experimental results in forest-like and multi-floor elevator scenarios demonstrate extreme ambiguity handling and real-time performance.

Abstract

This paper presents range-based 6-DoF Monte Carlo SLAM with a gradient-guided particle update strategy. While non-parametric state estimation methods, such as particle filters, are robust in situations with high ambiguity, they are known to be unsuitable for high-dimensional problems due to the curse of dimensionality. To address this issue, we propose a particle update strategy that improves the sampling efficiency by using the gradient information of the likelihood function to guide particles toward its mode. Additionally, we introduce a keyframe-based map representation that represents the global map as a set of past frames (i.e., keyframes) to mitigate memory consumption. The keyframe poses for each particle are corrected using a simple loop closure method to maintain trajectory consistency. The combination of gradient information and keyframe-based map representation significantly enhances sampling efficiency and reduces memory usage compared to traditional RBPF approaches. To process a large number of particles (e.g., 100,000 particles) in real-time, the proposed framework is designed to fully exploit GPU parallel processing. Experimental results demonstrate that the proposed method exhibits extreme robustness to state ambiguity and can even deal with kidnapping situations, such as when the sensor moves to different floors via an elevator, with minimal heuristics.

Figures (8)

• Figure 1: Estimated point cloud map and a set of 100,000 particles processed in real-time on a GPU (a). The very large number of particles enables flexible representation of multi-modal state distributions in highly ambiguous situations (e.g., in a forest-like environment) (b) (c).
• Figure 2: Data structure for particles and keyframes in the proposed system. Each of $N$ particles represents a hypothesis of the current pose and the keyframe poses. Separately from the particles, the point clouds of the keyframes are maintained in a global memory. The map estimate of each particle is given as a union of keyframe point clouds transformed with the estimate of keyframe poses.
• Figure 3: Relationship between the current frame and the neighbor keyframes, with three neighbor keyframes shown as an example. The likelihood is computed based on the registration error between the current frame and each neighbor keyframe. The current pose is then corrected using the gradient of the likelihood with respect to non-recent keyframes, specifically keyframes $i+1$ and $i+2$. The keyframes between the oldest neighbor keyframe and the newest keyframe are updated by propagating the current pose correction into them.
• Figure 4: Forest-like environment for the outdoor experiment. The loop was detected while the LiDAR was facing the rows of trees. The repeated and complex geometries cause multiple uncertain loop candidates and make it difficult to robustly identify correct loops. We cropped the points in the backward direction in the LiDAR frame to imitate a challenging mapping situation that limits the field of view of the LiDAR.
• Figure 5: Estimated trajectories for the forest-like experiment.