SGLC: Semantic Graph-Guided Coarse-Fine-Refine Full Loop Closing for LiDAR SLAM

Semantic graph is a fundamental component of our SGLC, which generates distinctive descriptors and guides the subsequent geometric verification and pose estimation. Given a raw LiDAR scan $\mathcal{S}$, semantic label for each point can be obtained using an existing semantic segmentation method [30] with the ability to distinguish between moving and static objects. We then apply clustering [31] to such semantic point clouds to identify object instances. Subsequently, the bounding box of instances can be estimated by enclosing these clusters. As foreground instances such as $pole$ and $trunk$ are naturally standalone, they can be easily represented as individual nodes in the graph. Background point clouds, such as $building$ and $fence$, typically have extensive points and are viewpoint-dependent, making them unstable for representation as instance nodes. Therefore, we construct semantic graphs only for stable foreground instances, such as $pole,\;trunk,\;lamp,\;static\;vehicle$. Each node $\mathbf{v}$ comprises instance center position $\mathbf{c}=[x,y,z]^{\sf T}$, bounding box size $\mathbf{b}=[l,h,w]$, a semantic label $l_{v}$, and a node descriptor $\mathbf{f}\in\mathbb{R}^{D}$ generated based on the semantic graph typologies.

Graph edges are established between pairs of nodes if their spatial Euclidean distance is less than $d_{\text{max}}$. Each edge $\mathbf{e}=(\mathbf{v}_{i},\mathbf{v}_{j})$ is described by a label $l_{e}$ determined by the two nodes it connects and the length $d$. The label $l_{e}$ include categories such as $pole$-$lamp$, $pole$-$trunk$, and so on. Finally, we obtain the semantic graph $\mathcal{G}$ with a set of nodes $\mathcal{V}$ and a set of edges $\mathcal{E}$. The graph adjacency matrix $\mathbf{A}\in\mathbb{R}^{N\times N}$ is created as:

Based on the semantic graph, we design instance node descriptors for the subsequent robust node matching. We first encode the local relationships of nodes by using connected edges. These edges are categorized and quantified to construct a histogram-based descriptor. Specifically, for a node $\mathbf{v}\in\mathcal{V}$, we categorize all connected edges into different intervals based on their labels and lengths, and count the occurrences within each interval with length $d_{i}$ to generate the descriptor $\mathbf{f}_{l}$.

Since $\mathbf{f}_{l}$ only captures the local typologies within the node neighborhood, it does not contain node global properties, such as their global centrality. To enhance the distinctiveness of the descriptor for robust node matching, we employ eigenvalue decomposition on the graph adjacency matrix $\mathbf{A}$, yielding $\mathbf{A}=\mathbf{Q}\mbox{\boldmath$\Lambda$}\mathbf{Q}^{\text{T}}$. The eigenvalues in $\text{diag}(\mbox{\boldmath$\Lambda$})={\lambda_{1},\ldots,\lambda_{n}}$ are arranged in descending order. The $i$-th row of $\mathbf{Q}$ represents the $i$-th instance node embeddings, which capture the node’s global properties in the graph [32]. Therefore, we use the each row of $\mathbf{Q}$ as part of the instance node descriptor. Considering that the dimensions of the matrix $\mathbf{Q}$ generated by each graph are different, and to unify dimensions of the descriptor, we encode each node using the first $k$ columns of $\mathbf{Q}$, i.e., the eigenvectors corresponding to the largest $k$ eigenvalues, thereby generating a row vector $\mathbf{f}_{g}\in\mathbb{R}^{k}$ for each node. Note that the signs of eigenvectors are arbitrary, hence we use their absolute values. Finally, by concatenating $\mathbf{f}_{l}$, $\mathbf{f}_{g}$, we obtain each instance node’s descriptor $\mathbf{f}\in\mathbb{R}^{D}$ in the graph.

Based on our semantic graph, our method effectively detects the loop closure. Theoretically, we could use local node descriptors to determine graph similarity and find loops directly. However, graph matching using nodes for LCD on a scan-scan basis is time-consuming, making it unsuitable for real-time SLAM systems. For fast loop candidate retrieving, we design a novel global descriptor for each LiDAR scan, including the foreground descriptor $\mathbf{F}_{f}$ and background descriptor $\mathbf{F}_{b}$.

We generate $\mathbf{F}_{f}$ based on our proposed foreground semantic graph, using the graph edges and nodes. The first part of $\mathbf{F}_{f}$ is similar to the first part of node descriptor $\mathbf{f}_{l}$ as an edge-based histogram descriptor. Instead of considering only the edges connected to a particular node, for $\mathbf{F}_{f}$, we account for all edges within the entire graph. These edges are categorized into subintervals in the histogram based on their types and lengths, forming the global edge descriptor. For the second part, We tally the number of nodes with different labels in the semantic graph to effectively describe the node distribution.

Besides $\mathbf{F}_{f}$, which captures the topological relationships between foreground instances, we further design a descriptor to exploit the extensive geometric information in the background point clouds. Inspired by Scan Context [7] encoding the maximum height of the raw point cloud into different bins of a polar BEV image, we leverage semantic information to replace the height data, constructing similar descriptors based on the background point cloud to generate rotation-invariant ring key descriptors. This descriptor efficiently encodes the appearance characteristics of different background point clouds, offering a concise portrayal at a minimal computational cost.

Finally, we perform L2-normalization on $\mathbf{F}_{f}$ and $\mathbf{F}_{b}$ to enhance the robustness, and concatenate them to form the LiDAR scan descriptor $\mathbf{F}\in\mathbb{R}^{D^{\prime}}$. This comprehensive descriptor incorporates features from both the foreground semantic graph and the background, thus possessing stronger retrieval capabilities. The LiDAR scan descriptor is utilized to retrieve the multiple similar candidate scans in the database. The similarity between descriptors is computed using the Euclidean distance within the feature space. In real applications, we use the FAISS library [33] to establish the descriptors database and perform fast parallel retrieving.

Once loop candidates are obtained, geometric verification is performed on each candidate to determine whether a true loop closure has occurred. The core of geometric verification is to establish node correspondences and estimate an initial relative pose between the query and candidate scans. The similarity between them is then measured by assessing the degree of alignment, including the following four steps:

Instance Nodes Matching. For the query scan with semantic graph $\mathcal{G}_{q}$ and the target candidate scan with semantic graph $\mathcal{G}_{t}$, we create an affiliation matrix $\mathbf{I}\in\mathbb{R}^{N\times M}$ based on node descriptor similarity, where $N$ and $M$ are the number of nodes in the query and target graphs, respectively. Each element $\mathbf{I}_{ij}$ is the cost of assigning the $i$-th node in $\mathcal{G}_{q}$ to the $j$-th node in $\mathcal{G}_{t}$ as:

A high cost is assigned to reject a match if theirs node labels are inconsistent or bounding box sizes differ significantly, i.e., the difference in each dimension of the box beyond $d_{b}$.

The matching results are then determined using the Hungarian algorithm [34], yielding the instance node matching correspondence set between $\mathcal{G}_{q}$ and $\mathcal{G}_{t}$ as:

where $\mathbf{c}=[x,y,z]^{\sf T}$ is the position of node mentioned earlier, $o$ is the number of node matching pairs.

Node Correspondences Pruning. Due to changes in viewpoint during revisit or errors in semantic segmentation, there are incorrect node correspondences. Although RANdom SAmple Consensus (RANSAC) [35] can exclude some outliers, using it with all node matches substantially increases the number of iterations, making it unsuitable for online applications.

To reduce the computational complexity, we propose an efficient outlier pruning scheme based on the local structure of nodes before applying RANSAC. We posit that two nodes of a true positive correspondence should exhibit consistent local geometric structures. For a node $\mathbf{v}$, its neighboring nodes within a certain range $d_{l}$ can be represented as $\{\mathbf{v}_{j}\}_{j=1}^{K}$, where $K$ is the total number of neighbors. $\mathbf{v}$ and any two of its neighbors can form a triangle, denoted as $\Delta_{j}$, and all local triangles of $\mathbf{v}$ is $\{\Delta_{j}\}_{j=1}^{K(K-1)/2}$. In an ideal scenario, corresponding local geometric triangles of matched nodes should be perfectly aligned. However, practical applications are subject to errors from semantic segmentation and clustering. Therefore, we relax this condition to prevent the elimination of correct correspondences. For the matching of local triangles, we consider two triangles to be consistent when their corresponding sides are of equal length. Once the number of successfully matched local triangles between two nodes exceeds $t_{m}$, they are considered as true node correspondences.

Initial Pose Estimation. Based on pruned correspondences, denoted as $\mathcal{M}_{q}^{{}^{\prime}}$ and $\mathcal{M}_{t}^{{}^{\prime}}$, we estimate a relative pose between the query and candidate scans based on RANSAC and SVD decomposition [36]. In each RANSAC iteration, we randomly select three matching pairs of $\mathcal{M}_{q}^{{}^{\prime}}$ and $\mathcal{M}_{t}^{{}^{\prime}}$ to solve the transformation equation based on SVD decomposition as:

by which we obtain a transformation in each iteration and finally select the transformation with the greatest number of inliers from all iterations as a coarse pose estimation for loop candidate verification $\mathbf{T}_{\text{coarse}}=\{\mathbf{R}_{c},\mathbf{t}_{c}\}$.

Loop Candidate Verification. We verify the loop candidates by calculating scan similarity on two levels. First, we evaluate the semantic graph similarity between the query scan and candidate scan as follows:

where $\mathbf{c}_{j}^{t}{{}^{\prime}}$, $\mathbf{c}_{j}^{q}{{}^{\prime}}$ are the inlier correspondences. $u$ is the number of inliers. $S_{\text{graph}}$ measures the alignment of the two graphs.

Furthermore, we use $\mathbf{T}_{\text{coarse}}$ to align the background point clouds of the query and candidate scans, and then calculate the cosine similarity of their background descriptors $\mathbf{F}^{q^{\prime}}_{b}$ and $\mathbf{F}^{t^{\prime}}_{b}$. This provides greater reliability compared to using graph similarity alone. If both $S_{\text{graph}}$ and the background similarity exceed thresholds $t_{g}$ and $t_{b}$, the query scan and the candidate scan are considered a true loop closure. We then proceed to estimate their precise pose transformation as follows.

The aforementioned $\mathbf{T}_{\text{coarse}}$, estimated at the sparse instance node level, may not be accurate and robust enough for closing the loop. To enhance the accuracy of the relative pose estimation, we propose performing a dense points registration on all foreground instance points from the input semantic point cloud, using $\mathbf{T}_{\text{coarse}}$ as the initial transformation.

Benefiting from the already obtained instance node matches, finding the corresponding point matches between matched instances becomes easy and fast due to the significantly reduced search space. In fact, $\mathbf{T}_{\text{coarse}}$ already initially align the query scan and candidate scan. Therefore, directly searching for the nearest neighbor points as point matches is sufficient to initialize and solve the Iterative Closest Point (ICP) as:

where $\mathcal{C}$ is the set of instance points nearest neighbor correspondences between query scan and loop scan.

The dense registration result $\mathbf{T}_{\text{icp}}$ encompasses the registration of all foreground instance points. However, background points, such as those from buildings and roads offering stable plane features, are also valuable for pose estimation. Therefore, to further optimize the pose accuracy from dense points registration, we utilize point-to-plane residuals to refine the final relative pose.

where $\mathcal{C^{\prime}}$ is the set of point correspondences with consistent plane normal vector, denoted as $\mathbf{n}_{j}^{\sf T}\in\mathbb{R}^{3}$.

$\mathbf{T}_{\text{coarse}}$ is estimated via sparse node matching, while $\mathbf{T}_{\text{icp}}$ is determined by dense instance points registration. This sparse-to-dense registration mechanism enhances pose estimation accuracy in a coarse-to-fine manner. Finally, the pose is further refined using point-to-plane constraints for background points. This semantically-guided coarse-fine-refine pose estimation process considers the alignment of both instance points and background points. By separately handling instance points and background points, each alignment stage begins with a favorable initial value, ensuring fast matching and convergence, as well as multi-step checked robust pose estimation.

Dataset. We evaluate our method on KITTI [16] and KITTI-360 [17] datasets. Additionally, Ford Campus [18] and Apollo [19] datasets are used to test its generalization ability.

Implementation Details. The parameters used in our experiments are listed in Tab. I. Due to background similarity being easily affected by semantic segmentation quality, we set $t_{b}$ for different datasets ($t_{b}=0.5$ for KITTI, KITTI-360 and Apollo, $t_{b}=0.45$ for Ford Campus). Other parameters remain unchanged. We use the semantic information from a semantic segmentation network [30] on all datasets and we train different models on the KITTI dataset to ensure that each test sequence remains unseen during the training phase. We mainly build semantic graphs from $static\;vehicle$, $pole$ and $trunk$ instances, and utilize background points from $building$, $fence$, $road$ and $vegetation$. This is convenient for replacing or adding other semantic information. To fairly compare all semantic-assisted loop closing methods, we evaluate them using the same labels from SegNet4D [30]. All the experiments are conducted on a machine with an AMD 3960X @3.8GHz CPU and a NVIDIA RTX 3090 GPU. More implementation details can be found in our online supplementary ¹¹1https://github.com/nubot-nudt/SGLC/Supplementary.pdf.

We follow the experimental setups of Li et al. [10] to evaluate LCD performance and regard LiDAR scan pairs as positive samples of loop closure when their Euclidean distance is less than 3 m, as negative samples if the distance exceeds 20 m. For the KITTI dataset, we perform performance comparisons on all sequences with loop closure. For the KITTI-360 dataset, in line with [12], we focus on sequences 0002 and 0009 with the highest number of loop closures. Besides, we also report the results on the Ford Campus dataset (sequence 00) and a subset of the Apollo Columbia Park MapData proposed by [37] for generalization evaluation.

Metrics. Following [10, 13], we use the maximum $F_{1}$ score and Extended Precision ($EP$) as evaluation metric.

Baselines. We compare the results with SOTA baselines, including DNN-based methods: PointNetVLAD (PV) [2], SGPR [3], LCDNet [12], OverlapTransformer (OT) [5], BEVPlace [6], PADLoC [29], as well as handcrafted-based methods: Scan Context (SC) [7], GOSMatch [15], SSC [10], BoW3D [13], Contour Context (CC) [11].

Results. The results are shown in Tab. II. Our method outperforms SOTA on multiple sequences and achieves the best average F1max score and $EP$ for the KITTI dataset. Specifically, for sequence 08 with many reverse loop closures, our approach still exhibits superior performance, proving its good rotational invariance. And on the KITTI360 dataset, our method remains competitive. Additionally, we report the average runtime for all methods, including descriptor generation and loop closure determination. Our method performs as fast as handcrafted methods while achieving superior performance in most cases, demonstrating both efficiency and effectiveness.

Furthermore, to investigate the loop closure detection performance at further distances, we followed [9, 5] regarding two LiDAR scans as a loop closure when their overlap ratio is beyond 0.3, which indicates that the maximum possible distance between them is around 15 m. And we adopt the same experimental setup as theirs to evaluate our method on the KITTI 00 sequence using AUC, F1max, Recall@1, and Recall@1% as metrics. The results are shown in Tab. III. Due to BoW3D leaning more towards geometric verification, we are unable to generate its AUC and recall@1% results from its open-source implementation. From the results, our method significantly outperforms SOTA baselines in terms of overall metric, indicating its robustness.

To evaluate the generalization capability of SGLC, we conducted additional experiments on the Ford Campus and Apollo datasets, focusing on loop pairs with an overlap ratio greater than 0.3. We primarily compared semantic-assisted methods to assess their adaptability to scene semantic changes or label quality declines. Due to the page limitation, we show the semantic segmentation details in our online supplementary. As shown in the Tab. IV, our method demonstrates the best performance on both the Ford Campus and Apollo datasets, highlighting its strong generalization capability. When the quality of semantic segmentation declines, some instances may be partially segmented, but after clustering, they still form a single object, which does not affect the construction of our semantic graph. Once the semantic graph is built, the LiDAR scan descriptors retrieve multiple candidate scans, including the correct loop scan. Through our geometric verification via graph matching, the true loop closure is identified. Besides, for short-term loop closing in SLAM, static vehicles can serve as valuable feature nodes to enhance loop detection.

To validate the loop pose estimation performance, we follow Cattaneo et al. [12] and evaluate our approach on the KITTI sequences 00 and 08 and KITTI-360 sequences 0002 and 0009. To keep consistent with them, we choose the loop closure samples when the distance is within 4 m. Additionally, we also select the more challenging loop pairs with overlap rate beyond 0.3 for testing.

Metric. (i) Registration Recall (RR), represents the percentage of successful alignment. (ii) Relative Translation Error (RTE), the Euclidean distance between the ground truth and estimated poses. (iii) Relative Rotation Error (RRE), the rotation angle difference between the ground truth and estimated poses. Two scans are regarded as a successful registration when the RTE and RRE are below 2 m and 5^∘, respectively.

Baselines. The baseline methods for comparison mainly include 6-DoF pose estimation methods BoW3D [13] and LCDNet [12]. LCDNet is available in two version: LCDNet (fast) utilizes an unbalanced optimal transport (UOT)-based head to estimate relative pose while LCDNet replaces the head by a RANSAC estimator.

Results. As shown on the Tab. V, Our method achieves the best loop pose estimation performance on both distance-based loop pairs and more challenging overlap-based loop pairs. Initially, both LCDNet and our method achieve a 100% RR for distance-based loop pairs. However, for overlap-based loop pairs, LCDNet’s performance declined, while our method still maintains superior registration capabilities, demonstrating its robustness for viewpoint changes. In terms of alignment accuracy, our method also significantly outperforms other baselines. We also present the qualitative results in Fig. 3. As can be seen, our method achieves better alignment even in low overlap loop closure, attributing to the semantically-guided coarse-fine-refine point cloud alignment process. Tab. VI shows the results on the KITTI-360 dataset with distance-based close loop. It can be observed that LCDNet exhibits the best generalization ability, but our method remains competitive and has the best alignment accuracy, considering both RTE and RRE. We also report the average running time of pose estimation on our machine. From the comparison, our method boasts the fastest running speed. The total runtime of our system, incorporating semantic segmentation, loop closure detection and poses estimation, is 98.1 ms, less than the data acquisition time of 100 ms for a typical rotational LiDAR sensor, thereby rendering it suitable for real-time SLAM systems.

We integrate SGLC into A-LOAM odometry to eliminate cumulative errors for evaluating the accuracy of SLAM trajectories, and compare with baseline integrated with BoW3D. We utilize a Incremental Smoothing and Mapping (iSAM2) [38] based pose-graph optimization (PGO)²²2https://github.com/gisbi-kim/SC-A-LOAM framework to build a factor graph for managing the the keyframe poses. If a loop closure is detected, we will add this loop closure constraint into the factor graph to eliminate errors and ensure global trajectories consistency. As shown in Fig. 4, we present the trajectory error analysis between keyframe and ground truth poses. From the results, our method significantly enhances the SLAM system by reducing the cumulative odometry error, and shows superior performance compared to the baseline that incorporates BoW3D. This improvement is attributed to the excellent loop closing capability of SGLC.

In this section, we conduct a series of ablation studies on KITTI sequence 00 and 08 and report the mean metrics to evaluate the effectiveness of designed components, and analyze their runtime. The results are presented in Tab. VII. $[$A$]$ is our baseline by removing those components listed in Tab. VII. Comparing $[$A$]$ and $[$B$]$, it is demonstrated that incorporating the global properties of instance nodes can enhance the performance of loop closing. As anticipated, it increases each node’s distinctiveness, which is beneficial for finding the correct node correspondences. For $[$C$]$, we can see the effectiveness of outlier pruning in improving registration accuracy with the same number of RANSAC iterations as $[$B$]$. In experiment $[$D$]$, $[$E$]$ and $[$F$]$, dense points registration and point-to-plane alignment are applied mainly for loop scan determined after geometric verification, so they do not affect the results of loop closure detection. The results clearly show that both dense points registration and point-to-plane alignment can improve registration performance, and we can get optimal results only when they operate in conjunction. The comparison of $[$F$]$, $[$G$]$, $[$H$]$ also indicates that even direct point-to-point ICP or point-to-plane alignment on the raw scan cannot get optimal results and significantly increase computational costs on direct point-to-point ICP, which demonstrates that our registration process effectively utilizes foreground and background. Besides, each component in our framework exhibits high execution efficiency.