💻 computer science

HP2-SLAM: Adaptive Hybrid ICP for Robust and Efficient LiDAR SLAM

HP2-SLAM is a minimalist, real-time LiDAR SLAM framework that achieves robust and accurate localization across diverse environments by employing a planarity-aware adaptive hybrid ICP to dynamically balance point-to-plane and point-to-point residuals without relying on learning modules or extensive training.

Original authors: Nam Tran, Thu Tran, Hieu Phan, Thai Luu, Toan Nguyen, William J. Beksi, Tuan Dang

Published 2026-08-18

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Nam Tran, Thu Tran, Hieu Phan, Thai Luu, Toan Nguyen, William J. Beksi, Tuan Dang

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Autonomous robots, from self-driving cars to delivery drones, rely on a fundamental ability to know where they are and what surrounds them. This process, known as simultaneous localization and mapping, or SLAM, allows a machine to build a mental map of an unknown environment while tracking its own movement within it. For years, the most reliable way to achieve this has been through light detection and ranging, or LiDAR. By firing laser pulses and measuring how long they take to bounce back, a robot creates a precise three-dimensional cloud of points representing the world. The challenge lies in connecting these points together as the robot moves. If the robot travels down a long, empty highway or a repetitive corridor, the laser points look nearly identical in every direction. Without distinct features to grab onto, the robot's internal calculation of its position can drift, causing it to believe it is in a different place than it actually is.

For a long time, researchers have tried to solve this by teaching computers to recognize patterns, much like a human learns to recognize a street corner. However, these learning-based systems often require massive amounts of data to train and struggle when faced with environments they have never seen before. On the other hand, traditional geometric methods are fast and efficient but often fail in those very same repetitive or empty spaces because they rely on rigid mathematical rules that cannot adapt to the changing shape of the world. The question has remained: can a robot navigate robustly and efficiently without needing a massive training dataset or complex feature engineering?

A team of researchers has developed a new system called HP2-SLAM that answers this question with a fresh approach to how a robot interprets the geometry of its surroundings. Instead of forcing the robot to choose between two different ways of matching laser points, this new framework allows the system to decide, in real time, which method is best for the specific patch of ground the robot is currently looking at. The core of their innovation is a smart way of measuring how "flat" or "structured" a small area of the environment is. When the robot scans a flat surface, like a long wall or a smooth road, the system knows to use one type of mathematical alignment. When it scans a complex corner or a tree, it switches to a different type. Crucially, this switch happens automatically based on the density of the laser points, meaning the robot does not need a human to pre-program it for specific environments.

The researchers tested this system on a wide variety of real-world datasets, including highways, bridges, and dense city streets, using different types of laser sensors. They found that their method consistently outperformed existing geometric systems, particularly in the difficult scenarios where other robots tend to get lost. In tests involving long, repetitive bridges where the laser points offer very few unique features, older systems began to drift and lose their way. HP2-SLAM, however, maintained a tight and accurate path, successfully recognizing that the flatness of the bridge required a different calculation than the complex geometry of a city intersection. The system achieved this by dynamically balancing two different error-checking methods, weighting them based on what the robot actually saw in that moment. This allowed it to stay stable even when the environment was confusingly uniform.

What makes this work particularly significant is that it achieves these results without the heavy computational cost of learning-based systems. The researchers demonstrated that their algorithm runs in real time on standard, off-the-shelf computer hardware, processing data fast enough to keep up with a moving vehicle. They also showed that the system is remarkably robust to changes in the environment. Whether the robot was moving slowly through a quiet town or speeding down a highway, the system adjusted its internal rules automatically. It did not need to be retrained or have its settings tweaked for each new location. In fact, the researchers found that the system was so adaptable that it could handle different types of laser sensors without any special adjustments, suggesting that the method is a general solution rather than a trick for a specific machine.

The study also looked closely at how sensitive the system is to its own settings. Often, complex robotic software requires precise tuning of numbers to work correctly; change one value slightly, and the whole system might fail. The researchers found that their new approach was surprisingly forgiving. Even when they changed the parameters that controlled how the system judged the flatness of the ground, the robot's performance remained steady. It did not collapse or become erratic. This stability suggests that the method is ready for real-world deployment, where conditions are unpredictable and perfect tuning is impossible. By proving that a carefully designed geometric adaptation can be both simple and powerful, the researchers have shown that robots do not need to be "taught" to see the world in the same way humans do to navigate it effectively. They simply need a way to understand the shape of the world as it changes right in front of them.

In the end, the work demonstrates that the key to robust navigation is not necessarily more data or more complex learning, but a smarter way of using the data that is already there. By letting the robot's own observations dictate how it calculates its position, HP2-SLAM bridges the gap between the speed of traditional methods and the adaptability of modern learning. It offers a path forward for autonomous machines that can operate reliably in the messy, repetitive, and often confusing environments of the real world, from the endless stretch of a highway to the tight turns of a city block, without needing a human to hold their hand.

Technical Summary: HP2-SLAM

Problem Statement

Achieving simultaneous robustness, accuracy, and efficiency in LiDAR Simultaneous Localization and Mapping (SLAM) remains a central challenge. While learning-based approaches offer strong benchmark performance, they often demand extensive training data, significant computational resources, and struggle to generalize to unseen or degenerate environments. Conversely, geometry-based methods are efficient and interpretable but suffer from performance degradation in planar or repetitive scenes (e.g., highways, long corridors) due to the limitations of standard Iterative Closest Point (ICP) formulations. Specifically, point-to-point ICP becomes ill-conditioned in planar environments causing drift, while point-to-plane ICP is sensitive to noise and unreliable when geometric support is weak. Existing hybrid strategies often rely on complex feature extraction, sensitive parameter tuning, or global degeneracy indicators that fail to leverage explicit correspondence-level geometric classification.

Methodology

HP2-SLAM is a minimalist LiDAR SLAM framework built upon the KISS-SLAM pipeline, replacing its original point-to-point ICP module with a novel neighborhood-size adaptive hybrid ICP. The core innovation lies in a density-aware adaptive planarity criterion that dynamically classifies correspondences based on local geometric structure and density, rather than using fixed thresholds or heuristic weighting.

Key Technical Components:

Adaptive Planarity Criterion:
- For each candidate correspondence, a local neighborhood $N$ is formed around the target point, and a covariance matrix is computed.
- A local planarity score $s$ is derived from the eigenvalues of the covariance matrix ( $s = \lambda_3 / (\lambda_1 + \lambda_2 + \lambda_3)$ ).
- Instead of a fixed cutoff, the decision boundary $\tau(n)$ scales inversely with the local neighborhood size $n$ : $\tau(n) = \tau_{ref} (N_{ref} / n)$ . This ensures stricter flatness conditions in dense regions while relaxing tolerance in sparse areas.
- Correspondences are classified into planar ( $P$ ) and non-planar ( $Q$ ) sets based on whether $s < \tau(n)$ .
Adaptive Hybrid ICP Formulation:
- The alignment objective is a convex combination of point-to-plane residuals (for $P$ ) and point-to-point residuals (for $Q$ ).
- The mixing weight $\alpha$ is adaptively updated in real-time based on the observed ratio of planar to non-planar correspondences: $\alpha = |P| / (|P| + |Q|)$ .
- This formulation balances constraints automatically without manual tuning for specific environments.
Robust Loss and Scale Estimation:
- The system employs the Geman-McClure estimator to suppress outliers and dynamic structures.
- The scale parameter $c$ for the robust loss is estimated online from recent motion prediction errors, allowing the system to adapt its robustness to varying motion regimes (aggressive vs. slow motion).
Full SLAM Integration:
- The front-end is integrated into a complete pipeline including submap management, loop closure detection (using ground-aligned Bird's-Eye-View density images and ORB descriptors), and pose graph optimization to ensure global consistency.

Key Contributions

The paper outlines three primary contributions:

A Neighborhood-Size–Adaptive Planarity Criterion: A method for density-aware correspondence classification that scales with local point density, reducing sensitivity to fixed thresholds.
An Adaptive Hybrid ICP Formulation: A mechanism that dynamically balances point-to-point and point-to-plane residuals based on real-time geometric observations, stabilizing alignment in both structured and degenerate environments.
A Minimalist Full-SLAM Integration: A complete system that achieves improved global consistency while preserving real-time efficiency on commodity hardware, avoiding the need for feature engineering or learning modules.

Experimental Results

Extensive evaluations were conducted on public datasets including KITTI, Apollo, MulRan, and HeLiPR, covering diverse sensors (handheld, UAV, vehicle-mounted) and challenging environments (bridges, tunnels, repetitive structures).

Accuracy: HP2-SLAM consistently outperformed strong geometry-based baselines, including KISS-SLAM, MULLS, CT-ICP, and GenZ-ICP. On the MulRan dataset, it achieved competitive Average Trajectory Error (ATE) with CT-ICP and PIN-SLAM while significantly outperforming MULLS and KISS-SLAM, particularly in high-speed and feature-scarce sequences.
Robustness in Degenerate Environments: Qualitative results on the HeLiPR Bridge sequence demonstrated that HP2-SLAM successfully mitigated the severe drift observed in KISS-SLAM (which relies solely on point-to-point ICP) by exploiting limited structural cues through adaptive planarity.
Generalization: The system maintained high accuracy across different LiDAR types (e.g., Ouster, Avia, Aeva) and motion conditions without requiring dataset-specific tuning.
Efficiency: The system operates at approximately 50 Hz (average 19 ms per scan) on commodity hardware (Intel i7, NVIDIA RTX 3060), maintaining real-time performance despite the added complexity of adaptive planarity estimation.
Sensitivity Analysis: Ablation studies showed that the system is robust to variations in the reference threshold $\tau_{ref}$ , with ATE remaining stable over a wide parameter range, unlike fixed-threshold schemes.

Significance and Claims

The authors claim that HP2-SLAM demonstrates that carefully designed geometric adaptation can achieve strong generalization and robustness without sacrificing simplicity or efficiency. By deriving residual balancing directly from local geometric structure in a principled, density-aware manner, the framework addresses the fundamental limitation of ICP degeneracy in planar environments.

The paper emphasizes that this approach:

Eliminates the need for extensive training data or black-box learning modules.
Avoids complex feature extraction and sensitive parameter tuning.
Provides a "small and simple" design philosophy that is deployable on resource-constrained robotic platforms.
Achieves global consistency through a complete SLAM pipeline, distinguishing it from front-end-only odometry systems like GenZ-ICP.

The authors conclude that HP2-SLAM offers a practical solution for real-world urban deployments, capable of handling degenerate scenarios and cross-sensor generalization while maintaining real-time efficiency.