💻 computer science

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

This paper presents MAD-BA, an open-source framework that simultaneously optimizes sensor poses and 3D surfel-based maps using a generalized LiDAR uncertainty model, achieving superior performance over state-of-the-art methods on public datasets.

Original authors: Krzysztof Ćwian, Luca Di Giammarino, Simone Ferrari, Thomas Ciarfuglia, Giorgio Grisetti, Piotr Skrzypczyński

Published 2026-05-07

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Krzysztof Ćwian, Luca Di Giammarino, Simone Ferrari, Thomas Ciarfuglia, Giorgio Grisetti, Piotr Skrzypczyński

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to build a perfect 3D model of a city using a laser scanner mounted on a robot. You take thousands of snapshots as the robot moves. However, two things go wrong:

The robot gets lost: It thinks it's in one spot, but it's actually a few steps away.
The map is messy: The laser scanner isn't perfect; sometimes it sees a wall clearly, and sometimes it gets confused by dust, rain, or a shiny car, creating "ghost" points or gaps.

Traditionally, robots try to fix these two problems separately. They fix the robot's path first, then try to clean up the map. Or, they fix the path but leave the messy map alone.

MAD-BA is a new method that says, "Let's fix the robot's path and the map at the same time, while also being smart about which laser measurements we trust."

Here is how it works, broken down into simple concepts:

1. The "Surfel" (The Building Block)

Instead of trying to manage millions of individual laser dots (which is like trying to build a house by counting every single grain of sand), MAD-BA groups these dots into Surfels.

The Analogy: Think of a surfel as a floating, flat coin or a sticker. It has a center, a direction it's facing (like a compass), and a size.
Instead of a jagged, pointy cloud of dots, the robot builds a map made of these smooth, flat coins. This makes it much easier to see the shape of walls and floors and to update the map when the robot moves.

2. The "Trust Meter" (Uncertainty Modeling)

Laser scanners are great, but they get "fuzzy" the further away they look or if the beam hits a weird angle.

The Old Way: Treat every laser dot as equally important. If the robot sees a ghost point far away, it might try to force the map to fit that ghost, ruining the whole model.
The MAD-BA Way: It uses a "Trust Meter." It calculates how likely a specific laser reading is to be wrong based on physics (like how wide the laser beam spreads out).
The Analogy: Imagine you are trying to solve a puzzle. Some pieces are clear and sharp; others are blurry and torn. MAD-BA puts less weight on the blurry pieces and more weight on the sharp ones. It tells the computer, "Ignore that fuzzy dot; it's probably a mistake. Trust this clear dot more."

3. The "Group Hug" (Joint Optimization)

This is the core magic. Most systems fix the path, then fix the map. MAD-BA does a simultaneous group hug.

The Analogy: Imagine a group of people trying to stand in a perfect circle while holding hands. If one person steps forward, everyone else has to adjust slightly to keep the circle round.
In MAD-BA, if the robot realizes it took a wrong turn, it doesn't just move the robot's position; it also gently nudges the "coins" (surfels) in the map to fit the new reality. If the map looks weird, it adjusts the robot's path to make sense of the map. They help each other find the truth.

4. The "Smart Connector" (Data Association)

To fix the map, the robot needs to know which laser dot from now matches which dot from before.

The Analogy: Imagine trying to match thousands of socks from a laundry pile. If you just look at them one by one, it takes forever. MAD-BA uses a kd-tree, which is like a super-organized filing cabinet. It instantly sorts the laser dots so the robot can find the matching "sock" in a split second, even in a huge, messy room.

What Did They Prove?

The authors tested this system on real-world data (driving cars, walking through buildings, going up stairs) and compared it to other top methods.

The Result: MAD-BA created maps that were sharper and more accurate than the competition.
The Bonus: Because it trusts the "good" measurements more, it naturally filters out "bad" measurements. For example, if a car drives through the scene, the system realizes that moving object doesn't belong in the permanent map and effectively ignores it, leaving a clean picture of the static building.

In short: MAD-BA is a smarter way to build 3D maps. It fixes the robot's location and the map's shape together, uses a "trust meter" to ignore bad data, and builds the world out of smooth, easy-to-manage flat coins instead of messy clouds of dots.

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

Problem Statement
Accurate and globally consistent 3D modeling is critical for autonomous navigation, robotics, and infrastructure inspection. While modern LiDAR sensors capture millions of points per second, achieving global consistency remains challenging due to sparse, irregular data distributions and the lack of visual cues. Traditional LiDAR Simultaneous Localization and Mapping (SLAM) often relies on Pose-Graph Optimization (PGO), which refines trajectories while keeping the map structure fixed. This decoupling leads to uncorrected noise and inconsistencies in point clouds. Furthermore, existing global refinement methods for LiDAR either treat pose and structure optimization separately (leading to inconsistencies) or rely on local registration techniques (like ICP) that do not jointly optimize global structure. Additionally, LiDAR measurements are inherently noisy due to environmental conditions and sensor physics, yet many systems lack a generalized uncertainty model to weight measurements effectively during optimization.

Methodology
The paper introduces MAD-BA, a unified global Bundle Adjustment (BA) framework designed to simultaneously optimize sensor poses and 3D map structure. The methodology is built upon four core components:

Surfel-Based Representation: The 3D map is represented using surfels (oriented discs), defined by a center point, surface normal, and radius. This representation is chosen for its efficiency in fusion and updates, its ability to capture thin surfaces better than voxel-based approaches, and its avoidance of computationally expensive topological updates required by mesh-based methods.
Efficient Data Association: To match LiDAR measurements with surfels, the system employs kd-trees built via Principal Component Analysis (PCA). This structure encodes plane segmentation and enables efficient nearest-neighbor queries. Data association is performed by matching tree leaves against existing surfels based on Euclidean distance, normal distance, and angular thresholds.
Generalized LiDAR Uncertainty Model: To address measurement noise, the authors propose a sample-based uncertainty model. Instead of relying on proprietary waveform data (often unavailable), the system simulates the LiDAR beam as a cone based on beam divergence. By casting multiple sub-beams within this cone towards a leaf's mean, the system samples a set of ranges. The standard deviation of these sampled ranges is computed to estimate measurement uncertainty ( $\sigma$ ). This $\sigma$ is used to weight the residuals in the cost function, prioritizing reliable measurements.
Joint Optimization: The system minimizes a cost function using the Levenberg-Marquardt (LM) algorithm. The cost function aggregates point-to-plane distances weighted by the Huber robust loss function and the estimated uncertainty. Crucially, the optimization jointly updates both the sensor poses (parameterized in the Lie algebra $se(3)$) and the surfel positions. Surfels are constrained to move only along their normal direction to preserve local geometry and reduce degrees of freedom, while poses are updated in all six degrees of freedom.

Key Contributions
The paper outlines three primary contributions:

A scalable and robust Bundle Adjustment framework tailored specifically for LiDAR data that integrates pose and structure optimization.
A generalized LiDAR uncertainty model that estimates measurement reliability based on beam divergence and range, allowing for confidence-weighted optimization without requiring raw waveform data.
An open-source implementation of the system to support reproducibility and further research.

Experimental Results
The system was evaluated on three public datasets: Newer College Dataset (NC), A Vision Benchmark in Rome (VBR), and KITTI. The evaluation compared MAD-BA against state-of-the-art methods, specifically BALM2 (a global refinement method that optimizes poses only) and HBA (a hierarchical BA method).

Trajectory Accuracy: Quantitative results using Absolute Trajectory Error (ATE) show that MAD-BA consistently outperforms BALM2 and HBA across most sequences. For instance, on the NC "math-easy" sequence, MAD-BA with uncertainty achieved an RMS error of 0.043 m, compared to 0.087 m for the initial trajectory and 0.135 m for HBA.
Map Quality: Evaluation on the NC dataset (which provides high-resolution ground truth maps) demonstrated that MAD-BA significantly improves map accuracy, completion, and F-score compared to initial maps and other methods. The surfel-based maps showed reduced geometric discrepancies and better alignment with ground truth.
Robustness and Ablation:
- Uncertainty Modeling: Incorporating the uncertainty model reduced the number of iterations required for convergence and improved final accuracy compared to a version where all measurements were weighted equally.
- Initialization: The system demonstrated strong tolerance to initial trajectory errors, converging reliably with Gaussian noise up to 1 m in translation and 0.2 rad in rotation.
- Sensor Density: When downsampled to simulate a VLP-16 sensor, the system maintained effectiveness in most scenarios, though performance degraded in vertical motion cases (e.g., stairs) due to insufficient vertical field of view.
- Dynamic Filtering: Qualitative results indicated that the surfel representation implicitly filters out dynamic objects (e.g., moving cars, pedestrians), resulting in cleaner static maps.

Significance and Claims
The paper claims that MAD-BA addresses the gap between pose-only optimization and separate structure refinement by providing a unified framework that achieves globally consistent and robust mapping. By jointly optimizing poses and surfels and incorporating a physics-motivated uncertainty model, the system mitigates noise and sparsity inherent in LiDAR data. The authors state that their approach surpasses existing LiDAR-based methods in robustness and accuracy, producing clean, high-fidelity 3D maps while compensating for dynamic artifacts and LiDAR skewing. The work is presented as a step toward more reliable state estimation in robotics, with the open-source nature of the code intended to facilitate community adoption and further development.

1. The "Surfel" (The Building Block)

2. The "Trust Meter" (Uncertainty Modeling)

3. The "Group Hug" (Joint Optimization)

4. The "Smart Connector" (Data Association)

What Did They Prove?

More like this