MAD-BA: 3D LiDAR Bundle Adjustment -- from Uncertainty Modelling to Structure Optimization

TL;DR

This work tackles the challenge of globally consistent 3D mapping from LiDAR by jointly optimizing sensor poses and the scene representation. It introduces MAD-BA, a surfel-based framework that performs uncertainty-weighted bundle adjustment, enabling simultaneous refinement of both poses and surfels through a robust factor-graph optimization. A generalized LiDAR uncertainty model weights measurements based on beam-divergence-driven simulations, improving robustness to noise and varying sensing conditions. Across multiple public datasets, MAD-BA with uncertainty demonstrates improved pose accuracy and map quality compared to state-of-the-art global refinements, and the authors provide open-source software to foster reproducibility and further research.

Abstract

The joint optimization of sensor poses and 3D structure is fundamental for state estimation in robotics and related fields. Current LiDAR systems often prioritize pose optimization, with structure refinement either omitted or treated separately using representations like signed distance functions or neural networks. This paper introduces a framework for simultaneous optimization of sensor poses and 3D map, represented as surfels. A generalized LiDAR uncertainty model is proposed to address degraded or less reliable measurements in varying scenarios. Experimental results on public datasets demonstrate improved performance over most comparable state-of-the-art methods. The system is provided as open-source software to support further research.

Figures (6)

• Figure 1: Pipeline. Given a trajectory estimated by a LiDAR SLAM system our method jointly optimizes the poses and map. It leverages on our uncertainty model to weight the optimization. The two bottom-right insets qualitatively demonstrate the map refinement achieved by employing our system.
• Figure 2: LiDAR uncertainty model. In the top image, a single LiDAR beam is simulated by casting a set of sub-beams towards a leaf $l$. At the bottom, the uncertainty of this measurement is modeled as a Gaussian distribution computed from the sampled ranges.
• Figure 3: ATE for each iteration of the bundle adjustment algorithm. The plot compares three versions of our system for math-easy sequence and shows that both versions of our BA reduce the trajectory error and accelerate the converge of the algorithm related to the pose-only optimization. For BA with uncertainty, the error didn't increase in the first iteration because measurements with higher uncertainty are weighted less during optimization.
• Figure 4: Chamfer-L1 distance of the cloister sequence for different distance thresholds. The initial map is a surfel map that was created using the initial trajectory. Both variants of our BA notably enhance the quality of the map, however, integrating uncertainty information results in additional improvements.
• Figure 5: Qualitative results of the maps generated from NC math-easy (a) and NC quad-easy (b) sequences, where the color of each surfel's point corresponds to its Euclidean distance to the nearest point in the ground truth map. The left-side images are created before the optimization (using the initial trajectory), while the right ones after our BA with uncertainty method.