C$^3$P-VoxelMap: Compact, Cumulative and Coalescible Probabilistic Voxel Mapping

TL;DR

This paper tackles the memory and computation bottlenecks of probabilistic voxel mapping in LiDAR SLAM by introducing C3P-VoxelMap, a compact point-free voxel representation with a cumulative plane update that eliminates the need to store past points. It combines a probabilistic plane model (normal $n$, center $q$, covariance) with a tractable cumulative update, and augments the map with on-demand voxel merging based on locality-sensitive hashing to capture large planar features. The approach achieves constant space per voxel and linear-time updates, while cross-voxel denoising via merging improves localization accuracy. Experimental results on KITTI, UTBM, and indoor data show about 20% improvements in accuracy and up to 70% reductions in memory, highlighting its potential for real-time, resource-constrained LiDAR SLAM.

Abstract

This work presents a compact, cumulative and coalescible probabilistic voxel mapping method to enhance performance, accuracy and memory efficiency in LiDAR odometry. Probabilistic voxel mapping requires storing past point clouds and re-iterating on them to update the uncertainty every iteration, which consumes large memory space and CPU cycles. To solve this problem, we propose a two-folded strategy. First, we introduce a compact point-free representation for probabilistic voxels and derive a cumulative update of the planar uncertainty without caching original point clouds. Our voxel structure only keeps track of a predetermined set of statistics for points that lie inside it. This method reduces the runtime complexity from $O(MN)$ to $O(N)$ and the space complexity from $O(N)$ to $O(1)$ where $M$ is the number of iterations and $N$ is the number of points. Second, to further minimize memory usage and enhance mapping accuracy, we provide a strategy to dynamically merge voxels associated with the same physical planes by taking advantage of the geometric features in the real world. Rather than scanning for these coalescible voxels constantly at every iteration, our merging strategy accumulates voxels in a locality-sensitive hash and triggers merging lazily. On-demand merging not only reduces memory footprint with minimal computational overhead but also improves localization accuracy thanks to cross-voxel denoising. Experiments exhibit 20% higher accuracy, 20% faster performance and 70% lower memory consumption than the state-of-the-art.

Figures (6)

• Figure 1: Bias-variance tradeoff of voxel maps. (a) Small voxels capture details of the environment but are susceptible to noise, i.e., large variance (note the wiggling planes on the walls and the ground). (b) Large voxels are more resistant to noise but might lead to approximation bias (note the thick wall is over-smoothed into a plane).
• Figure 2: Locality-sensitive hash keys are comprised of five parameters: $\theta$, $\varphi$ are spherical coordinates of normal $\mathbf{n}$, $d$ is distance from origin to the plane, $u$ and $v$ are projection coordinates of a voxel on the plane.
• Figure 3: Illustration of voxel merging. Through LSH, voxel planes proximate to each other in the parameter space are hashed into the same bucket. A merge operation is triggered once a bucket accumulates enough voxels.
• Figure 4: Illustration of accuracy enhancement thanks to voxel merging. The solid angle represents the robustness of plane estimation against noise. Merged voxel demonstrates a smaller angle than the original voxel under the same noise level.
• Figure 5: Illustration of voxel planes: (a) w/o merge and (b) w/ merge. Voxel merge eliminates the wiggling of planes and unifies the poses of planes associated with the same physical plane.

Theorems & Definitions (4)

• Lemma 1
• Lemma 2
• Lemma 3
• Lemma 4