HP2-SLAM: Adaptive Hybrid ICP for Robust and Efficient LiDAR SLAM
Authors: Nam Tran, Thu Tran, Hieu Phan, Thai Luu, Toan Nguyen, William J. Beksi, Tuan Dang
Organizations: Department of Electrical Engineering and Computer Science, University of Arkansas, Fayetteville, AR, USA · VinRobotics · Department of Computer Science, University of Texas at Dallas, Richardson, TX, USA
Achieving robustness, accuracy, and efficiency simultaneously remains a central challenge in light detection and ranging (LiDAR) simultaneous localization and mapping (SLAM). While learning-based approaches deliver strong benchmark performance, they often require extensive training, substantial computational resources, and struggle to generalize to unseen or degenerate environments. Geometry-based methods are efficient and interpretable, yet their performance degrades in planar or repetitive scenes due to limitations of standard iterative closest point (ICP) formulations. We present HP2-SLAM, a minimalist yet robust LiDAR SLAM framework built around a neighborhood-size adaptive hybrid ICP. Our key insight is a planarity-aware adaptive threshold that dynamically classifies correspondences based on local geometric structure and density, thereby enabling a principled balance between point-to-plane and point-to-point residuals. This formulation stabilizes alignment in both structured and degenerate environments without feature engineering, learning modules, or dataset-specific tuning. Integrated into a complete SLAM pipeline with submap management, loop closure detection, and pose graph optimization, HP2-SLAM consistently outperforms strong geometry-based baselines across publicly available datasets while maintaining real-time performance on commodity hardware. Our results demonstrate that carefully designed geometric adaptation can achieve strong generalization and robustness without sacrificing simplicity or efficiency.
Figures & tables
Fig. 1 : HP 2 -SLAM conceptualization. Each LiDAR scan is registered to a local map using a density-aware planarity criterion that separates planar and non-planar correspondences. The hybrid ICP weight α is adaptively updated at each scan based on the observed planar ratio, increasing in structured environments and decreasing in less planar settings.
Fig. 2 : An overview of HP 2 -SLAM. Our system includes LiDAR preprocessing, odometry estimation, local mapping, loop-closure detection using ground-aligned BEV and ORB with RANSAC verification, and pose-graph optimization for global consistency. Odometry is improved by a planarity-aware hybrid residual that applies point-to-plane in planar regions and point-to-point in the non-planar areas with adaptive weighting. This hybrid approach stabilizes odometry and reduces longitudinal drift when mapping degenerate environments, such as long corridors, roads, and bridges.
Fig. 3 : Mapping results on the HeLiPR Bridge (Avia04) sequence. Our method demonstrates stable odometry and consistent map reconstruction across long bridge structures, which are challenging due to repetitive geometric patterns and limited features.
Fig. 4 : Trajectory overlays on Sejong01 (left) and Riverside02 (right). Our approach aligns more closely with the ground truth (GT), particularly in challenging curved and loop regions, as shown in the zoomed-in views.
Fig. 5 : An evaluation on the MathildaAVE sequence. Our methodology consistently yields lower error than the baseline.
Fig. 6 : The APE of raw trajectories on the SanJose-Downtown dataset. HP 2 -SLAM maintains consistently smaller drift in the raw trajectory error, especially at segments with large error spikes.
Method
KAIST
DCC
Riverside
Sejong
ATE (m)
ATE (m)
ATE (m)
ATE (m)
KISS-SLAM [ 8 ]
2.96
3.68
8.34
178.34
PIN-SLAM [ 30 ]
2.42
3.57
7.55
782.96
CT-ICP [ 14 ]
2.76
4.03
8.37
-
MULLS [ 11 ]
34.21
27.30
66.60
1082.55
HP 2 -SLAM (ours)
2.71
3.56
7.50
146.51
TABLE I : A performance comparison of HP 2 -SLAM and the baselines on the Mulran dataset. A lower ATE indicates better localization accuracy. The best and second best performing methods are reported in bold and underline , respectively.
KITTI-corrected
00
01
02
03
04
05
06
07
08
09
10
AVG
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
KISS-SLAM [ 8 ]
0.89
3.11
1.92
0.67
0.38
0.67
0.32
0.33
2.55
1.25
0.71
1.16
GenZ-ICP [ 13 ]
5.19
25.47
11.19
3.27
0.90
1.50
0.86
0.67
4.46
3.07
2.32
5.36
HP 2 -SLAM (ours)
0.85
2.75
1.89
0.51
0.24
0.56
0.31
0.31
2.40
1.15
0.69
1.06
TABLE II : Quantitative evaluations on the KITTI-corrected benchmark using the ATE in meters (m). Compared to KISS-SLAM and GenZ-ICP, HP 2 -SLAM yields lower average error and improved performance across the majority of sequences.
Method
Bridge
Bridge
Bridge
Round
Round
Round
Town
Town
Town
Aeva
Avia
Ouster
Aeva
Avia
Ouster
Aeva
Avia
Ouster
KISS-SLAM [ 8 ]
98.61
148.88
19.47
6.06
3.84
1.18
14.44
12.01
1.99
PIN-SLAM [ 30 ]
-
365.72
19.23
-
7.02
1.47
41.19
11.40
2.55
CT-ICP [ 14 ]
-
41.47
576.62
10.04
3.36
1.81
65.29
63.72
-
MULLS [ 11 ]
365.06
321.87
52.65
19.08
16.39
2.65
39.82
14.93
4.45
HP 2 -SLAM (ours)
57.37
26.77
17.51
5.77
3.17
1.08
11.65
10.45
1.91
TABLE III : Quantitative results on the Bridge, Roundabout and Town sequences of the HeLiPR dataset using the ATE in meters (m). The best and second best performing methods are reported in bold and underline , respectively.
Method
BTS
CP
H237
MAVE
SB
SJD
SJD
2018-10-12
2018-10-11
2018-10-12
2018-10-12
2018-10-03
2018-10-11 (1)
2018-10-11 (2)
KISS-SLAM [ 8 ]
3.07
0.78
25.02
12.7
2.40
1.96
0.73
PIN-SLAM [ 30 ]
6.10
0.64
97.11
7.06
2.23
1.86
1.15
CT-ICP [ 14 ]
10.81
0.97
258.87
61.39
667.28
0.78
1.09
MULLS [ 11 ]
104.14
47.03
354.21
182.59
-
13.38
11.41
HP 2 -SLAM (ours)
2.63
0.64
24.41
4.01
2.07
1.46
0.63
TABLE IV : Quantitative results on the Apollo dataset using the ATE in meters (m). The best and second best performing methods are reported in bold and underline , respectively.
Method
0
1
2
3
4
5
6
7
8
9
10
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
ATE (m)
GenZ-ICP [ 13 ]
4.72
17.21
15.41
3.55
0.64
1.62
1.16
0.73
5.62
3.64
2.16
Point-to-point only
3.50
3.15
6.46
0.52
0.31
1.49
0.42
0.31
2.20
1.31
0.71
Point-to-plane only
3.96
2.49
6.63
0.60
2.41
1.45
0.40
0.39
2.41
0.88
0.79
Adaptive hybrid (ours)
3.46
2.70
6.64
0.51
0.31
1.32
0.41
0.30
2.19
1.14
0.67
TABLE V : A front-end odometry ablation study on KITTI sequences. A lower ATE indicates better localization accuracy. The best and second best performing methods are reported in bold and underline , respectively.
KITTI-corrected
Neighbor Search
PCA
ICP
Backend
Total
Sequence 00
5.92
1.69
0.91
8.72
17.24
Sequence 01
8.68
2.50
1.07
11.36
23.61
Sequence 02
5.70
1.65
0.85
9.48
17.68
Sequence 03
8.37
2.4
1.06
9.91
21.74
Sequence 04
7.01
1.98
0.97
11.24
21.20
Sequence 05
5.95
1.71
0.86
9.56
18.08
TABLE VI : A runtime (ms) breakdown of the front-end and back-end modules on the KITTI sequences.
Fig. 7 : The sensitivity of τref on the KITTI sequences. The ATE remains stable over a wide parameter range, indicating low sensitivity.
LiDAR simultaneous localization and mapping (SLAM) relies on local geometric quantities such as covariances, correspondences, and surface structures. However, most existing pipelines rely on hand-crafted estimates of local geometry and use them as fixed inputs to LiDAR SLAM, which can make the estimated local geometry noisy and unstable in sparse regions of a point cloud or when using low-resolution LiDAR. To address this issue, this paper introduces a self-supervised framework that learns an explicit symbolic representation of local geometry and uses it to improve LiDAR SLAM recursively. Specifically, each point is represented as a Gaussian distribution, allowing local geometry to be described by a covariance. Without dense geometry labels or ground-truth poses, the framework learns by maximizing the likelihood of local geometry, with self-supervision derived from consistency relations over symbolic geometric representations, including predicted covariances, correspondences, and trajectory from SLAM. The learned geometry is then fed back into LiDAR SLAM, forming a reciprocal loop in which improved geometry enhances localization and mapping, and improved localization provides cleaner supervision for subsequent geometry reasoning. This framework is backend-agnostic and can be plugged into existing LiDAR SLAM pipelines without architectural changes. Experiments on KITTI under varying LiDAR resolutions show that the proposed method improves both odometry and global registration.
Jiwoo Kim, Jinwoo Lee, Woojae Shin +2
Korea Advanced Institute of Science and Technology (KAIST), Daejeon, Republic of Korea · Daegu Gyeongbuk Institute of Science and Technology (DGIST), Daegu, Republic of Korea
Geometric light detection and ranging (LiDAR)--inertial simultaneous localization and mapping (SLAM) provides efficient state estimation but lacks the continuous map representation and global refinement capabilities of neural implicit SLAM, while neural implicit methods remain computationally expensive. We propose Lightweight LiDAR--Inertial Neural SLAM (LIN-SLAM), a hybrid method that couples high-rate geometric LiDAR--inertial estimation with neural implicit tracking, continual mapping, and global refinement for accurate real-time onboard SLAM. Experiments on twelve representative sequences from three datasets spanning 32-, 64-, and 128-channel LiDAR configurations show that LIN-SLAM achieves the lowest overall mean absolute trajectory error (ATE) among the evaluated state-of-the-art geometric and neural implicit methods, while substantially reducing runtime compared with existing neural implicit SLAM methods. On a Unitree Go2 equipped with an NVIDIA Jetson Orin NX and a 16-channel LiDAR, LIN-SLAM sustains real-time localization and neural mapping over a 1.47-km indoor trajectory through challenging repetitive corridor environments, with an endpoint closure error of only 0.141~m. To the best of our knowledge, LIN-SLAM is the first LiDAR--inertial SLAM system with a neural implicit backend to achieve real-time online operation on a mobile robotic platform while maintaining state-of-the-art localization accuracy. The code will be released upon acceptance.
Shiqi Che, Yijie Liu, Shuo Wang +1
School of Automation Science and Electrical Engineering, Beihang University, Beijing 100191, China
We present a real-time LiDAR-based framework for Gaussian Splatting SLAM that tightly couples fast G-ICP registration with spherical rasterization-based dense mapping for large-scale sequences. Leveraging LiDAR geometry rather than appearance, we reuse tracking-estimated local covariances to initialize Gaussians with range-aware scales and to derive surface normals for geometry-aware map optimization. We further introduce a covariance-derived geometry score that measures local complexity and drives pruning in planar regions and selective densification in structurally rich areas, while optimized Gaussians and LiDAR-specific confidence cues are fed back to improve tracking robustness. On the Newer College dataset, our method achieves an F-score of 86.78% using purely online trajectories at real-time speed (>20 FPS), and additional experiments on other datasets confirm its stability and scalability.