Real-time Multi-Adaptive-Resolution-Surfel 6D LiDAR Odometry using Continuous-time Trajectory Optimization

Jan Quenzel, Sven Behnke

Introduction

Simultaneous Localization and Mapping (SLAM) is a fundamental capability for autonomous robots. However, the high data rates of modern 3D LiDAR sensors present a significant computational challenge for real-time onboard processing. Traditional systems often treat LiDAR scans as discrete snapshots, leading to motion distortion (the "rolling shutter" effect) and unrealistic, jerky trajectory estimates. The MARS (Multi-Adaptive-Resolution-Surfel) system addresses these issues by combining continuous-time B-Spline trajectories with multi-resolution surfel maps to achieve high-accuracy, real-time 6D LiDAR odometry.

The Challenge of High-Speed LiDAR Odometry

Most existing SLAM systems struggle with two primary issues when a robot moves quickly:

  1. Motion Distortion: Because LiDAR sensors sweep their environment over time, the robot's movement during a single scan causes the resulting point cloud to appear blurred or warped.
  2. Trajectory Discontinuity: Aligning discrete scans often results in unrealistic motion estimates because the system lacks a physical prior that motion must be continuous.

MARS aims to bridge this gap by creating a computationally efficient system suitable for real-time use on platforms like UAVs and self-driving cars, while maintaining the physical consistency of a continuous-time trajectory.

Methodology: The MARS Architecture

The MARS system introduces a novel architecture designed for efficiency and smoothness.

The MARS system architecture, showing the flow from LiDAR input to continuous-time registration and local map updates.

Continuous-time B-Splines

Instead of estimating a single pose for every scan, MARS optimizes a Lie group B-Spline. This mathematical curve represents the robot's path smoothly over time. This approach naturally compensates for motion distortion and allows for the joint registration of multiple scans within a sliding window, enforcing a physical prior on the robot's motion.

Multi-resolution Surfel Maps and Adaptive Selection

The environment is represented using "surfels" (surface elements), which store the mean and covariance of point clusters. A key innovation in MARS is the Adaptive Resolution Selection scheme.

Comparison of point cloud, voxel grid, and lattice representations for single scans and local maps. The adaptive resolution allows the system to use coarser surfels for simple planes and finer ones for complex structures.

The system analyzes the "flatness" of surfaces using eigenvalues. For simple planar regions like walls or floors, it utilizes coarser, lower-resolution surfels. Fine-grained surfels are reserved for complex, detailed structures. This significantly reduces the number of associations required during registration, speeding up computation without sacrificing accuracy.

Efficient Storage and Registration

To maintain a low memory footprint, MARS utilizes sparse voxel grids and permutohedral lattices, which scale more effectively than dense grids for high-dimensional data. Registration is performed using a Gaussian Mixture Model (GMM) formulation with a unique weighting scheme. By inversely weighting surfels based on measurement count, the system prevents dense "near-field" points from over-influencing the trajectory estimate, leading to better global alignment.

Experimental Results

The authors evaluated MARS across three distinct scenarios: the Newer College Dataset (handheld LiDAR), the Urban Loco Dataset (autonomous driving in dense urban canyons), and the DRZ Living Lab (high-speed UAV maneuvers).

Performance and Robustness

  • Accuracy: MARS consistently achieved state-of-the-art accuracy, ranking at the top of benchmarks against popular methods like A-LOAM and SuMa.
  • Real-time Efficiency: The adaptive resolution scheme reduced registration runtime from approximately 30ms to 23ms per window, allowing the system to run comfortably in real-time on standard hardware.
  • UAV Maneuverability: In UAV tests, MARS handled angular velocities up to 3.9 rad/s (approx. 220 deg/s). While other methods often failed during sharp turns, MARS maintained an Absolute Trajectory Error (ATE) frequently below 0.1 meters.

Conclusion and Future Work

MARS demonstrates that combining continuous-time trajectory optimization with adaptive-resolution mapping provides a robust solution for real-time LiDAR odometry. By enforcing motion smoothness and optimizing computational resources where they are most needed, it enables stable tracking even during high-speed maneuvers. Future work includes integrating IMU data and visual features into the spline optimization and developing global SLAM capabilities for long-term loop closure.

Summary updated

BibTeX
@inproceedings{quenzel2021real,
  author = {Jan Quenzel and Sven Behnke},
  title = {{Real-time Multi-Adaptive-Resolution-Surfel 6D LiDAR Odometry using Continuous-time Trajectory Optimization}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2021},
  url = {https://openreview.net/forum?id=jIX0tD0h7lb},
}

Loading related content...

Figure