Semantic-Gravity ICP (SG-ICP)

• SG-ICP is a semantic generalized iterative closest point method that aligns road-marking point clouds with HD maps using anisotropic covariance modeling.
• It leverages semantic consistency in correspondence matching to reduce spurious matches and improve pose estimation in challenging, GPS-denied scenarios.
• Integrated within a full LiDAR localization pipeline, SG-ICP enhances 2D pose accuracy by adaptively weighting measurements from under-constrained linear features.

Searching arXiv for the specified paper and closely related localization/ICP work to ground the article. Semantic-Gravity ICP (SG-ICP) is a semantic generalized iterative closest point algorithm introduced for LiDAR-based HD map localization with road marking detection in GPS-denied autonomous driving scenarios. In the formulation reported in “LiDAR-based HD Map Localization using Semantic Generalized ICP with Road Marking Detection” (Gong et al., 2024), SG-ICP estimates a rigid-body pose $T=(R,t)\in\mathrm{SE}(2)$ that aligns a detected semantic road-marking point cloud to corresponding semantic elements in an HD map. Its defining characteristic is the joint use of semantic consistency and covariance modeling for under-constrained road-marking geometry, especially linear markings treated as 1-manifolds in $\mathbb{R}^2$. The method is presented as part of an online localization system that includes adaptive segmentation of high-reflectance LiDAR points, spatio-temporal local-map aggregation, LiDAR bird’s-eye-view (LiBEV) rendering, and instance segmentation for road-marking labeling (Gong et al., 2024).

1. Problem formulation and role within HD map localization

SG-ICP is designed for registration between detected road markings and a semantic HD map in a LiDAR-only localization pipeline. The optimization target is the pose $T=(R,t)\in\mathrm{SE}(2)$ that minimizes a sum of Mahalanobis distances over correspondences between detected points $\{q_{Li}\}$ and map points $\{q_{mi}\}$ (Gong et al., 2024). The objective is

$$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$

In this expression, each residual $q_{mi}-R\,q_{Li}-t$ measures geometric misalignment, while $C_{Li}$ and $C_{mi}$ encode how strongly each correspondence constrains the two degrees of freedom in the planar registration. The formulation contains no explicit regularizer beyond the requirement that $C_{mi}+R\,C_{Li}R^\mathsf{T}$ remain positive definite (Gong et al., 2024).

Within the broader system, SG-ICP operates after road-marking detection. The reported detection pipeline first uses an adaptive segmentation technique to isolate high-reflectance points correlated with road markings, then forms a spatio-temporal probabilistic local map by aggregating historical LiDAR scans, and finally generates a LiBEV image for instance segmentation of road markings (Gong et al., 2024). SG-ICP is thus not a standalone detector; it is the registration component that consumes semantic point sets and map semantics.

A plausible implication is that the method’s robustness depends not only on the optimizer itself but also on the semantic quality of the upstream road-marking extraction and instance labeling.

2. Geometric model: linear road markings as 1-manifolds

A central element of SG-ICP is its treatment of linear road markings as 1-dimensional manifolds embedded in 2D space (Gong et al., 2024). If a marking has principal direction $\mathbb{R}^2$0, then any point $\mathbb{R}^2$1 on that marking approximately satisfies

$\mathbb{R}^2$2

where $\mathbb{R}^2$3 is an arbitrary reference point on the line. This model is used to represent the fact that a long linear marking provides weak localization information along its own direction.

To reflect that weak constraint, the covariance assigned to the instance is made large in the direction $\mathbb{R}^2$4. The paper describes starting from the sample covariance $\mathbb{R}^2$5 of the detected points and performing an SVD:

$\mathbb{R}^2$6

The covariance is then re-parameterized using

$\mathbb{R}^2$7

so that

$\mathbb{R}^2$8

The reported interpretation is that a very small $\mathbb{R}^2$9 along the manifold direction means “virtually no force” is exerted by that marking along its length (Gong et al., 2024). This covariance design mitigates the under-constrained problem associated with line-like structures in registration.

This suggests that SG-ICP departs from uniform isotropic weighting by encoding directional observability directly into the registration objective. In practical terms, long lane markings and similar structures contribute strongly across the marking but weakly along it.

3. Semantic constraints and correspondence weighting

SG-ICP incorporates semantics at the correspondence stage and in the weighting of residuals. The reported semantic-consistency rule is that each detected point $T=(R,t)\in\mathrm{SE}(2)$0 is matched only to a map point $T=(R,t)\in\mathrm{SE}(2)$1 sharing the same semantic class, with examples including “dashed lane,” “stop line,” and “arrow” (Gong et al., 2024). This restricts nearest-neighbor search to semantically compatible subsets of the HD map.

For each matched pair, the detector-side covariance $T=(R,t)\in\mathrm{SE}(2)$2 and the map-side covariance $T=(R,t)\in\mathrm{SE}(2)$3 are constructed using the class- and instance-dependent procedure described for 1-manifold-aware covariance estimation. The Mahalanobis weight is then

$T=(R,t)\in\mathrm{SE}(2)$4

The stated effect is that correspondences that are poorly constrained along certain directions, such as those lying on a long linear marking, are naturally down-weighted in those directions (Gong et al., 2024). SG-ICP therefore combines semantic filtering with anisotropic uncertainty propagation.

This semantic weighting distinguishes the method from standard point-to-point ICP in two ways. First, it reduces spurious matches across road-marking categories. Second, it adjusts the information content of each correspondence according to geometric structure rather than treating all matches as equally informative. A plausible implication is that this is especially beneficial in environments where road markings are sparse, elongated, or partially observed.

4. Iterative solver and implementation procedure

The reported input to SG-ICP is a detected semantic point set $T=(R,t)\in\mathrm{SE}(2)$5, a semantic HD map $T=(R,t)\in\mathrm{SE}(2)$6 organized by class $T=(R,t)\in\mathrm{SE}(2)$7, and an initial pose guess $T=(R,t)\in\mathrm{SE}(2)$8; the output is an optimized pose $T=(R,t)\in\mathrm{SE}(2)$9 (Gong et al., 2024). The algorithm proceeds through the following steps:

1. Build a spatial index (kd-tree) for each map class $\{q_{Li}\}$0.
2. Initialize $\{q_{Li}\}$1, $\{q_{Li}\}$2.
3. For each iteration until convergence or the maximum number of iterations:
   • transform detected points as $\{q_{Li}\}$3;
   • for each transformed point $\{q_{Li}\}$4 with class $\{q_{Li}\}$5, find the nearest neighbor $\{q_{Li}\}$6 in map class $\{q_{Li}\}$7, accumulate matched pairs, and track them by instance;
   • for each semantic instance, compute the sample covariance $\{q_{Li}\}$8 of its points and form $\{q_{Li}\}$9 by SVD+$\{q_{mi}\}$0, while retrieving the map instance direction $\{q_{mi}\}$1 and setting $\{q_{mi}\}$2 similarly;
   • linearize the objective about the current $\{q_{mi}\}$3 and solve the $\{q_{mi}\}$4 normal equations using Gauss–Newton or Levenberg–Marquardt for the increment $\{q_{mi}\}$5;
   • update $\{q_{mi}\}$6, $\{q_{mi}\}$7;
   • terminate if $\{q_{mi}\}$8 and the rotation angle is $\{q_{mi}\}$9.
4. Return $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$0 (Gong et al., 2024).

The solver is described as typically using Gauss–Newton, optionally damped by Levenberg–Marquardt, on the 3-DOF pose manifold. The stopping criteria are reported as $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$1 and $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$2, or relative change in objective $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$3, or a maximum of 20 iterations (Gong et al., 2024).

The implementation emphasizes class-specific kd-trees and per-instance covariance estimation. This suggests that the method’s computational profile is determined less by the linear solve, which is negligible, than by semantic nearest-neighbor lookup and instance-wise covariance construction.

5. Integration with road-marking detection and system pipeline

SG-ICP is embedded in a larger online localization system for road-marking-based HD map localization (Gong et al., 2024). The reported pipeline has three upstream stages before registration.

First, an adaptive segmentation technique isolates high-reflectance points correlated with road markings. The stated purpose is to enhance real-time efficiency by filtering the LiDAR data to road-marking-relevant subsets (Gong et al., 2024).

Second, a spatio-temporal probabilistic local map is formed by aggregating historical LiDAR scans, producing a dense point cloud. This local aggregation stage provides the spatial support needed for more reliable subsequent road-marking analysis (Gong et al., 2024).

Third, a LiDAR bird’s-eye-view image is generated, and an instance segmentation network is applied to accurately label road markings. The details block specifically names CenterMask in the runtime breakdown, where “LiBEV image generation + CenterMask inference” is reported as a GPU stage (Gong et al., 2024).

The registration stage then takes these semantic instances and aligns them to a semantic HD map using SG-ICP. By combining semantic instance segmentation on LiBEV renders with a class-dependent covariance model that inflates uncertainty along under-constrained directions, the system is reported to achieve more robust and more accurate 2D pose alignment than classical ICP when matching sparse, linear LiDAR road markings to an HD map (Gong et al., 2024).

A plausible implication is that SG-ICP is best understood not merely as a modified ICP cost, but as the registration mechanism of a semantics-aware perception stack in which segmentation quality, instance formation, and map semantics are tightly coupled.

6. Quantitative behavior, runtime characteristics, and reported limitations

The paper reports evaluation on eight real-world sequences comprising four scenes and seven LiDARs, with comparison against standard point-to-point ICP (Gong et al., 2024). A subset of the reported mean absolute errors is summarized below.

Sequence	Longitudinal (ICP → SG-ICP)	Lateral / Yaw (ICP → SG-ICP)
Fangshan1 (Hesai-64)	0.158 m → 0.137 m	0.050 m → 0.043 m; 0.233° → 0.208°
Jiashan (VLP-32C+VLP-16)	0.082 m → 0.077 m	0.055 m → 0.050 m; 0.547° → 0.401°
Airport (XT16)	0.129 m → 0.128 m	0.116 m → 0.050 m; 0.447° → 0.230°

Across all eight runs, SG-ICP is reported to reduce lateral error by approximately 15–20% and yaw error by approximately 20–50% relative to vanilla ICP. Visual inspection of trajectory plots is also reported to show far fewer outliers, defined as errors $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$4 or $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$5 (Gong et al., 2024).

The runtime profile is also explicitly reported. All modules run onboard an NVIDIA Jetson AGX Xavier at a 10 Hz LiDAR rate. The average per-frame runtimes are approximately 20 ms for road-point segmentation and local-map update on CPU, approximately 16 ms for LiBEV image generation plus CenterMask inference on GPU, and approximately 4 ms for SG-ICP registration on CPU, with total runtime less than 50 ms and worst-case runtime less than 200 ms (Gong et al., 2024).

The paper also reports computational complexity by component: nearest-neighbor search is $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$6 with kd-trees; covariance and SVD per instance are $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$7 with $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$8; and the linear solve is $$T^* \;=\;\arg\min_{R,t} \sum_{i=1}^N (q_{mi}-R\,q_{Li}-t)^\mathsf{T} \bigl(C_{mi}+R\,C_{Li}\,R^\mathsf{T}\bigr)^{-1} (q_{mi}-R\,q_{Li}-t).$$9, described as negligible. Reported parallelization includes road-marking segmentation and BEV generation on GPU, and kd-tree queries plus Gauss–Newton linear algebra on multi-core CPU (Gong et al., 2024).

The reported results support the claim that SG-ICP improves robustness and localization accuracy relative to standard ICP in the tested settings. At the same time, the formulation is explicitly tied to 2D pose alignment in $q_{mi}-R\,q_{Li}-t$0 and to semantic road-marking instances. This suggests that the method is specialized for planar HD-map localization rather than presented as a generic full-3D registration framework.

7. Interpretation, relation to classical ICP, and common misconceptions

SG-ICP is described as a generalized ICP variant rather than a semantic filter layered onto unchanged ICP. The objective function replaces Euclidean residual weighting with a Mahalanobis form built from $q_{mi}-R\,q_{Li}-t$1, and the covariance construction is inseparable from the semantic and geometric interpretation of road-marking instances (Gong et al., 2024). Its difference from standard point-to-point ICP therefore lies both in correspondence selection and in anisotropic information modeling.

A common misconception would be to interpret the method as assigning stronger weights to linear features because they are visually salient. The formulation reported in the paper does the opposite along the line direction: covariance is inflated, or equivalently information is reduced, where the geometry is under-constrained (Gong et al., 2024). The purpose is not to force stronger alignment along elongated structures, but to avoid overconfident constraints in directions where the structure provides little localization information.

Another possible misconception is that “semantic” refers only to class labels. In the reported algorithm, semantics affects nearest-neighbor eligibility, instance grouping, and covariance assignment. Class-consistent matching and class-dependent covariance therefore operate jointly rather than as independent modules (Gong et al., 2024).

More broadly, SG-ICP can be situated as a registration strategy for sparse, structured map elements rather than dense unconstrained point clouds. The paper’s emphasis on dashed lanes, stop lines, arrows, instance-level covariance, and 1-manifold modeling indicates that the algorithm targets the geometry of road infrastructure markings as represented in semantic HD maps (Gong et al., 2024). A plausible implication is that its principal advantage emerges when feature geometry is anisotropic and semantically typed, conditions under which classical isotropic ICP is especially prone to under-constrained alignment.