MAD-ICP: It Is All About Matching Data -- Robust and Informed LiDAR Odometry

TL;DR

MAD-ICP presents a robust LiDAR odometry system that blends a PCA-based kd-tree for plane segmentation with an ICP-style data association and a pose-uncertainty–driven local map. By maintaining a forest of kd-trees as a local map and adaptively selecting updates using Fisher information, it achieves reliable, real-time odometry across diverse environments and LiDARs with minimal parameter tuning. The method delivers competitive accuracy while dramatically improving robustness, as demonstrated on KITTI, Mulran, Newer College, and Hilti datasets, and is released as an open-source anytime C++ implementation. This approach reduces drift and failure cases in challenging scenes and offers a practical, adaptable solution for real-world robotic systems.

Abstract

LiDAR odometry is the task of estimating the ego-motion of the sensor from sequential laser scans. This problem has been addressed by the community for more than two decades, and many effective solutions are available nowadays. Most of these systems implicitly rely on assumptions about the operating environment, the sensor used, and motion pattern. When these assumptions are violated, several well-known systems tend to perform poorly. This paper presents a LiDAR odometry system that can overcome these limitations and operate well under different operating conditions while achieving performance comparable with domain-specific methods. Our algorithm follows the well-known ICP paradigm that leverages a PCA-based kd-tree implementation that is used to extract structural information about the clouds being registered and to compute the minimization metric for the alignment. The drift is bound by managing the local map based on the estimated uncertainty of the tracked pose. To benefit the community, we release an open-source C++ anytime real-time implementation.

Figures (10)

• Figure 1: Above is the point cloud generated by our LiDAR odometry processing a sequence of KITTI geiger2012we dataset. The small yellow spheres represent the registered poses corresponding to each sensor measurement. The larger ones are poses related to the point clouds which form the model. Below, the normalized information $6\times6$ matrices are used to select the keyframe poses. The point cloud relative to the pose with the highest information is selected to be part of the model.
• Figure 2: Plane segmentation of a point cloud with our kd-tree explained in Sec. \ref{['sec:segmentation']}. Every leaf is depicted with a different color.
• Figure 3: The plot shows the cumulative error curve for each of the compared approaches, computed using all the sequences RPE [%] of Tab. \ref{['tab:results']}. For a given error value on the horizontal axis, the vertical axis shows how many sequences a method achieves a lower error.
• Figure 4: The plots illustrate the qualitative absolute consistency of the compared approaches within some sequences of the employed datasets.
• Figure 5: A 2D illustration of the kd-tree building and transformation processes. Our splitting criteria recursively divides the cloud with a plane whose normal is the direction $\mathbf{d}$ of maximum spread (a, b), identified by . The kd-tree is transformed in the world frame $w$ through an isometry $^w\mathbf{X}$ (c). Our transformed kd-tree maintains the search properties.