Abstract: Sensors producing 3D point clouds such as 3D laser scanners and RGB-D cameras are widely used in robotics, be it for autonomous driving or manipulation. Aligning point clouds produced by these sensors is a vital component in such applications to perform tasks such as model registration, pose estimation, and SLAM. Iterative closest point (ICP) is the most widely used method for this task, due to its simplicity and efficiency. In this paper we propose a novel method which solves the optimisation problem posed by ICP using stochastic gradient descent (SGD). Using SGD allows us to improve the convergence speed of ICP without sacrificing solution quality. Experiments using Kinect as well as Velodyne data show that, our proposed method is faster than existing methods, while obtaining solutions comparable to standard ICP. An additional benefit is robustness to parameters when processing data from different sensors.
Introduction
Aligning 3D point clouds—a process called registration—is a cornerstone of modern robotics, essential for tasks like SLAM (Simultaneous Localization and Mapping), autonomous driving, and robotic manipulation. The "gold standard" for this task has long been the Iterative Closest Point (ICP) algorithm. However, standard ICP is a batch method, requiring the processing of every point in a scan to compute a single transformation update. With modern sensors like Lidar or RGB-D cameras producing hundreds of thousands of points per second, this batch processing becomes a significant computational bottleneck. While sub-sampling can mitigate this, it often discards useful information. This research introduces SGD-ICP, a method that leverages Stochastic Gradient Descent to achieve faster, more data-efficient registration without sacrificing alignment quality.
Methodology
The core innovation of SGD-ICP is the application of Stochastic Gradient Descent (SGD) to the ICP optimization problem. Instead of using the full source point cloud for every iteration, the algorithm selects a small random subset, or mini-batch, of points (e.g., $m=160$). For each point in the mini-batch, the algorithm finds the closest corresponding point in the reference cloud, calculates the gradient of the Euclidean distance loss, and updates the transformation parameters immediately.
To handle the noisy gradients inherent in small batches, the authors compared "Vanilla SGD" (fixed step size) with Adam-SGD, which utilizes adaptive learning rates. Adam-SGD proved superior in stabilizing convergence. Furthermore, to ensure robustness across different physical scales—from indoor rooms to outdoor streets—the point clouds are scaled to a unit cube $[0, 1]$ during optimization.
Experimental Setup
The method was evaluated on two distinct datasets to test versatility:
- ETHZ-ASL Kinect Dataset: An indoor dataset containing high-detail Kinect scans of cluttered rooms, used to benchmark accuracy and runtime.
- KITTI Dataset: A large-scale outdoor autonomous driving dataset using Velodyne Lidar data, testing the algorithm's performance on sparse, long-range, and non-uniformly distributed point clouds.
Results and Discussion
SGD-ICP demonstrates significant improvements in both speed and data efficiency. On the Kinect dataset, the method achieved accuracy comparable to standard ICP while being approximately 15 times faster (0.33 seconds vs. 5.15 seconds).
A key finding is the algorithm's data efficiency: SGD-ICP typically reaches a stable solution in roughly a single "pass" through the data. In contrast, standard batch ICP often requires multiple passes over the entire point cloud to reach the same error level. This efficiency is visualized in the convergence plots, where SGD-ICP (blue) reaches low error levels with orders of magnitude fewer sample points than competing methods.
In KITTI Lidar tests, SGD-ICP produced the lowest translational and rotational errors among all tested methods. This suggests that the stochastic nature of the updates helps the algorithm avoid local minima that often trap traditional batch methods. Experiments also indicated that a mini-batch size between 50 and 300 points provides the optimal balance between convergence speed and computational overhead.
Conclusion and Future Work
SGD-ICP offers a high-performance alternative to traditional ICP by replacing batch optimization with stochastic updates. While the current implementation focuses on point-to-point distance metrics, it remains sensitive to outliers and limited scan overlap. Future work will explore extending this SGD approach to the point-to-plane metric, which is generally more robust to local minima, and utilizing GPU acceleration for even faster mini-batch correspondence searches.
Summary updated