SAGE-ICP: Semantic Information-Assisted ICP

Jiaming Cui, Jiming Chen, Liang Li

TL;DR

This paper introduces SAGE-ICP, a novel approach that enhances LiDAR-based odometry by integrating semantic information directly into the Iterative Closest Point (ICP) algorithm. By leveraging semantic labels for tasks like downsampling, data association, and dynamic object removal, SAGE-ICP achieves more accurate and robust pose estimation, especially in challenging environments with moving objects and sparse features, while maintaining real-time performance.

What Problem Does This Paper Address?

Accurate and robust pose estimation is crucial for autonomous robots navigating unknown environments. While LiDAR odometry (LO) offers advantages over visual odometry due to its robustness to lighting conditions, it can struggle in large-scale, dynamic scenes with sparse geometric features. Traditional ICP methods, while effective, can be computationally expensive and sensitive to initial guesses. Existing semantic-assisted approaches often rely heavily on perfect semantic segmentation and can be slow due to complex optimization. This paper aims to bridge this gap by developing a real-time, semantic-information-assisted ICP that is robust to imperfect semantic labels and improves accuracy in challenging scenarios.

Methods

SAGE-ICP builds upon the KISS-ICP framework, emphasizing simplicity and efficiency. The core contributions lie in how semantic information is integrated:

  • Semantic Segmentation: A 3D convolutional network (Cylinder3D, with a faster variant PVD) is used for real-time point-wise semantic labeling.
  • Dynamic Object Removal: Instead of filtering all dynamic objects (which can degrade accuracy), SAGE-ICP uses prior knowledge to distinguish and remove potentially dynamic vehicles before registration. This is achieved by analyzing the spatial distribution of points around detected vehicle instances.
  • Adaptive Voxel Downsampling: To preserve crucial small objects and critical semantic information, SAGE-ICP employs adaptive voxel sizes for downsampling based on semantic categories. This ensures that important features are not lost, unlike traditional uniform downsampling.
  • Semantic-Assisted Association: Data association in ICP is enhanced by incorporating semantic labels. Points are associated based on both their semantic consistency and Euclidean distance, with a weighting factor to handle imperfect segmentation.
  • Adaptive Voxel Map: The local map used for registration is an adaptive voxel map that prioritizes storing critical semantic points and dynamically updates its density, while also removing voxels too far from the current pose to save memory.

Datasets

The paper utilizes the following datasets for training and evaluation:

  • SemanticKITTI [15, 16]: Used for training the semantic segmentation model.
  • KITTI Odometry Benchmark [17]: A standard dataset for evaluating odometry systems.
  • KITTI-360 [18]: A dataset with longer sequences, used for evaluating generalization.
  • KITTI Road Sequences (renamed KITTI-PART): Specific sequences from the KITTI dataset, used for focused evaluation.

Key Insights and Ideas

  • Semantic Information as a "Smart Filter": By intelligently using semantic labels, SAGE-ICP can filter out noise and dynamic objects more effectively than purely geometric methods, leading to better pose estimation.
  • Robustness to Imperfect Semantics: The method is designed to be robust even when semantic segmentation is not perfect, thanks to semantic-assisted association and adaptive downsampling strategies. This is a significant advantage over approaches that critically depend on flawless semantic labels.
  • Adaptive Downsampling Preserves Critical Features: Using different voxel sizes for different semantic categories is crucial for retaining small but important objects (like road signs) that might be lost with uniform downsampling.
  • Targeted Dynamic Object Removal: Removing only potentially dynamic vehicles, rather than all dynamic objects, prevents the loss of valuable static features (like parked cars) that can aid localization.

Key Results

  • Improved Accuracy: SAGE-ICP consistently outperforms baseline methods like KISS-ICP, LOAM, SuMa++, and SA-LOAM on KITTI Odometry Benchmark and KITTI-360, achieving lower relative translation and rotation errors. For instance, on KITTI Odometry, the proposed method shows significant improvements on sequences like 01 and 10, which are challenging due to dynamic objects and fewer geometric features.
  • Real-time Performance: The system achieves real-time performance, with the registration section running faster than the LiDAR frame rate (around 11.60 Hz). The semantic segmentation part is also optimized to run in real-time through parallel processing.
  • Robustness: Experiments demonstrate that SAGE-ICP is robust to erroneous semantic segmentation results, showing that even with imperfect labels, the semantic information still provides a benefit.

Limitations and Future Work

  • Dependence on Semantic Segmentation Quality: While robust to some errors, the performance is still influenced by the quality of the semantic segmentation model. The paper notes that in sequences with many vans and trucks incorrectly inferred as buildings, performance degrades.
  • Future Work: The authors plan to explore fusing semantic information for loop closure detection and incorporating other sensors to further improve both semantic segmentation and pose estimation. They also aim to make their code publicly available.

Summary updated

Abstract

Robust and accurate pose estimation in unknown environments is an essential part of robotic applications. We focus on LiDAR-based point-to-point ICP combined with effective semantic information. This paper proposes a novel semantic information-assisted ICP method named SAGE-ICP, which leverages semantics in odometry. The semantic information for the whole scan is timely and efficiently extracted by a 3D convolution network, and these point-wise labels are deeply involved in every part of the registration, including semantic voxel downsampling, data association, adaptive local map, and dynamic vehicle removal. Unlike previous semantic-aided approaches, the proposed method can improve localization accuracy in large-scale scenes even if the semantic information has certain errors. Experimental evaluations on KITTI and KITTI-360 show that our method outperforms the baseline methods, and improves accuracy while maintaining real-time performance, i.e., runs faster than the sensor frame rate.
BibTeX
@inproceedings{cui2023sage,
  author = {Jiaming Cui and Jiming Chen and Liang Li},
  title = {{SAGE-ICP: Semantic Information-Assisted ICP}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2023},
}
Figure