Lidar-Vision Depth Fusion for Robust Loop Closure Detection in SLAM Systems

1. Introduction

As mobile robots and autonomous driving systems continue to expand into complex environments, the requirements for accuracy and robustness in long-term autonomous navigation using Simultaneous Localization and Mapping (SLAM) systems have become increasingly stringent [1]. Odometry, as the fundamental pose estimation module of SLAM, is susceptible to cumulative drift caused by sensor noise, environmental dynamics, and data association errors [2,3].Without timely correction, this drift can lead to map distortion and trajectory deviation, ultimately resulting in navigation failure. Loop Closure Detection (LCD) serves as a global constraint mechanism by identifying when a robot revisits a previously mapped location, making it a key technique for correcting accumulated errors and maintaining global consistency between the map and trajectory [4].

Existing loop closure detection methods can be categorized into three types according to sensor modality: LiDAR-based, vision-based, and multi-sensor fusion approaches. LiDAR-based methods rely on high-precision 3D geometric information, exhibiting strong stability under varying illumination and in textureless scenes. Representative works such as Scan Context [5], Scan Context++ [6], and Contour Context [7] construct global descriptors to enable fast matching. However, due to the inherent sparsity of LiDAR point clouds, these methods struggle to represent spatial features adequately in long-range or unstructured environments, which may result in missed loop detections [8]. Vision-based methods [9] leverage the rich texture and color information of images to distinguish between visually similar scenes, yet they are highly sensitive to illumination changes and seasonal variations, often suffering from a significant drop in matching accuracy under strong lighting, rain, or other adverse weather conditions [10].

Each single-sensor modality has its limitations in specific environments.Nevertheless, LiDAR and vision sensors are inherently complementary: LiDAR provides stable, illumination-invariant 3D geometric structures, while cameras capture fine-grained texture details to distinguish similar structures [11,12]. Most existing fusion methods operate at the feature level, where image and point cloud features are independently extracted and then aligned or associated via homomorphic transformation [13], projection [14], or attention mechanisms [15] to achieve cross-modal integration. In contrast, decision-level fusion methods combine independent detection results from different sensors to avoid false matches caused by single-sensor noise, effectively leveraging their respective advantages to enhance overall robustness [16,17].

To address the limitations of single-modality systems and the shortcomings of existing fusion strategies, this paper proposes a LiDAR–vision deep fusion loop closure detection algorithm. The primary objective is to establish a tight correlation between geometric and textural information, thereby improving the accuracy and robustness of loop detection in complex environments.

The main contributions of this work are summarized as follows:

• Spatiotemporal Alignment and Depth Completion Framework: A unified process is designed to achieve spatial alignment of LiDAR point clouds and camera images via extrinsic calibration, handle sampling rate discrepancies through timestamp interpolation, and complete depth estimation of visual keypoints using neighborhood search. This enables the construction of 3D visual features as a foundation for cross-modal fusion.
• Hybrid Feature Descriptor Construction: A fusion descriptor combining LiDAR geometric triangle descriptors (representing spatial topology) and visual BRIEF descriptors (representing local texture) is developed. Efficient loop candidate retrieval is achieved via hash indexing, balancing matching efficiency and discriminability. An improved RANSAC-based geometric verification method is further introduced to suppress noise and reduce false matches.
• Comprehensive Evaluation on Public Datasets: Extensive experiments are conducted on the KITTI and NCLT datasets to validate the proposed algorithm’s effectiveness across urban, mixed indoor–outdoor, and seasonally varying environments, demonstrating its potential as a robust solution for long-term autonomous navigation in SLAM systems.

2. Related Work

2.1. Loop Closure Detection Based on Single Modality

LiDAR-based loop closure detection methods can generally be divided into two categories: direct point cloud matching and feature descriptor matching. Direct matching methods such as ICP [18] and NDT [19] estimate relative poses by iteratively optimizing point correspondences , but their high computational cost makes them unsuitable for real-time applications. Descriptor-based methods extract global or local features to reduce data dimensionality and have become the mainstream approach. Among global descriptor methods, Scan Context [5] projects 3D point clouds into an azimuth–radial grid and encodes the maximum height in each cell to achieve rotation-invariant matching. Scan Context++ [6] further improves robustness by introducing sub-descriptors to handle translation and rotation sensitivity, but its adaptability to non-spherical LiDARs remains limited. Local descriptor methods such as LinK3D [20] and BoW3D [8] employ a Bag-of-Words (BoW) framework that utilizes local geometric features for fast retrieval. However, in sparse regions, feature redundancy can lead to false detections. Map Closure [21] constructs LiDAR density maps for loop detection, showing strong viewpoint robustness, but compressing 3D structures into 2D maps leads to loss of spatial information and incomplete scene representation [22].

Vision-based loop closure detection, typically formulated as Visual Place Recognition (VPR), identifies revisited locations through image feature matching. Among local feature methods, SURF [23] achieves scale and rotation invariance but is computationally expensive. ORBcombines FAST corner detection and BRIEF descriptors, balancing efficiency and robustness, and is widely used in systems such as ORB-SLAM [1]. Global feature methods such as HOG [24] and PHOG [25] capture image gradient distributions to describe structural information but are sensitive to local detail changes. To improve efficiency, researchers have proposed Bag-of-Words (BoW) models such as DBoW2 [9], which use K-means clustering to build a visual vocabulary and TF–IDF weighting to enhance discriminability for fast retrieval. However, BoW models suffer from a fixed vocabulary problem, limiting generalization to unseen scenes. FAB-MAP 2.0 [26] introduces probabilistic modeling of word co-occurrence to improve large-scale adaptability, but its reliance on SURF features leads to slow extraction, restricting real-time performance.

2.2. LiDAR–Vision Fusion for Loop Closure Detection

Multimodal fusion methods combine LiDAR geometric information with visual texture to overcome the limitations of single modalities. According to the fusion level, these methods can be categorized as feature-level fusion and decision-level fusion. Decision-level fusion methods, such as MSF-SLAM [16], integrate LiDAR and visual loop closure results through logical operations (e.g., OR fusion). Although this improves robustness, it fails to fully exploit cross-modal feature correlations and remains vulnerable to false detections caused by a single modality. iBTC [27] introduces binary visual descriptors to assist LiDAR matching and achieves real-time performance but lacks deep feature-level interaction.

Feature-level fusion methods have received greater attention. CoRAL [12] generates elevation maps from LiDAR data and fuses them with projected RGB image features, aggregating multimodal representations via a NetVLAD layer. However, this approach depends heavily on deep learning and large amounts of labeled data. MinkLoc++ [28] designs a specialized feature extraction module that jointly learns point cloud and image features to generate multimodal descriptors, but it is sensitive to calibration errors. BEV Fusion [11] projects image features onto a bird-eye-view (BEV) plane for fusion with LiDAR BEV features, though projection bias can occur in non-flat terrains. Additionally, some approaches incorporate attention mechanisms [29] to model cross-modal dependencies, but their high computational complexity limits their use on embedded platforms.

In summary, existing fusion approaches still face challenges such as low cross-modal alignment accuracy, insufficient feature fusion depth, and limited robustness. To address these problems, this paper proposes a LiDAR–vision deep fusion loop closure detection algorithm that integrates spatiotemporal alignment, depth completion, hybrid descriptor construction, and improved RANSAC-based geometric verification, providing a novel solution for enhancing loop closure detection performance.

3. Method

Figure 1 illustrates the algorithm workflow. First, Harris corners are detected from input images with BRIEF descriptors extracted. Depth information of keypoints is obtained via LiDAR point cloud projection to generate a sparse depth map, where invalid depths are completed to form a fused depth map, which is then back-projected into 3D space. Triangular descriptors with binary descriptors are constructed based on LiDAR and visual keypoints. For matching, LiDAR geometric descriptors are prioritized, with visual descriptors used as supplements when unavailable. An improved RANSAC algorithm is applied for geometric verification, incorporating a matching error weighting mechanism to enhance matching accuracy and robustness of transformation estimation.

3.1. Visual Feature Extraction and Depth Completion

Harris corner detection identifies corners by analysing grey-level changes within local windows [30]. For each detected Harris keypoint, N random pairs of pixels $(p_a,p_b)$ are sampled within its neighborhood, and a binary test is defined as:

$$\tau (p_a,p_b)=\left\{\begin{array}{cc}1,\hfill & I\left(p_a\right)<I\left(p_b\right)\hfill \\ 0,\hfill & I\left(p_a\right)\ge I\left(p_b\right)\hfill \end{array}\right.$$

(1)

where $I\left(p\right)$ denotes the grey-scale intensity of pixel p . The concatenation of all N binary tests forms the BRIEF descriptor, which is compact in storage and enables efficient matching.

After projecting LiDAR points onto the image plane, the generated depth map contains many blank areas, requiring depth completion for visual keypoints. This process begins with spatiotemporal alignment of the LiDAR and camera data. Because LiDAR and camera operate at different sampling frequencies, for each image frame, two LiDAR scans with timestamps closest to the image are found, and linear interpolation is applied to obtain the interpolated LiDAR point cloud at the image timestamp, ensuring temporal consistency. Given the extrinsic calibration parameters, each LiDAR point $P_l={[x_l,y_l,z_l]}^T$ is transformed into the camera coordinate system $P_c={[x_c,y_c,z_c]}^T$ as:

$$P_c=R_{lc}·P_l+t_{lc}$$

(2)

where $R_{lc}$ and $t_{lc}$ are the rotation matrix and translation vector from LiDAR to camera, respectively.

For depth completion, a neighborhood search is applied: a sparse depth map records the LiDAR-derived depth for each pixel. For keypoints without valid depth, the nearest valid depth pixel within a $5\times 5$ window is found and assigned to the keypoint; if no valid value exists, the keypoint is discarded to prevent invalid matches.

According to the pinhole camera model, each keypoint with completed depth $d_k$ is back-projected into 3D space as a visual keypoint $P_v={[x_v,y_v,z_v]}^T$:

$$x_v={\displaystyle \frac{(u_k-c_x)·d_k}{f_x}},y_v={\displaystyle \frac{(v_k-c_y)·d_k}{f_y}},z_v=d_k$$

(3)

where $(u_k,v_k)$ denote the pixel coordinates, and $(f_x,f_y,c_x,c_y)$ are the intrinsic parameters of the camera.

3.2. Multimodal Feature Fusion and Matching

To fuse LiDAR geometric and visual texture features, triangular descriptors are constructed based on LiDAR keypoints and 3D visual keypoints, respectively. For LiDAR triangular descriptors: after preprocessing LiDAR point clouds, FAST corner detection is used to extract keypoints. For each keypoint, two neighboring keypoints are selected to form a triangle, with three side lengths $(l_1,l_2,l_3)$ calculated and vertex height encodings recorded to form geometric descriptors. For visual triangular descriptors: triangles are constructed from 3D visual keypoints using the same method, with side lengths calculated and vertex BRIEF descriptors recorded to form visual texture descriptors.

For LiDAR keypoint extraction: first, LiDAR scans are accumulated into submaps, which are voxelized. Voxels belonging to planes are identified based on the proportion of planar points, then merged into larger planes. Non-planar voxels are projected onto adjacent planes along their normals to form projection images. Keypoints are extracted by finding pixels with the highest density in local regions of these images, denoted as $L_k$.

The hash key for the constructed LiDAR and visual triangle descriptors is computed as

$$\begin{array}{cc}\hfill \mathrm{Hash}\left(L\right)& =\mathrm{Hash}(l_1,l_2,l_3)=\mathrm{Int}.\mathrm{Hash}({\overline{l}}_1,{\overline{l}}_2,{\overline{l}}_3)\hfill \\ \hfill & =\mathrm{Mod}\left([\mathrm{Mod}({\overline{l}}_3·p+{\overline{l}}_2)·p,B]+{\overline{l}}_1·p\right)\hfill \end{array}$$

(4)

where $\overline{l}=l/0.1$, p is a large prime number and B denotes the hash table size. For each triangular descriptor in the current frame, the hash key is calculated to retrieve similar descriptors from the hash table. The top 50 candidate loop frames are selected based on descriptor similarity (using height-encoded Hamming distance for LiDAR and BRIEF Hamming distance for vision).

3.3. Improved RANSAC-Based Geometric Verification

Traditional RANSAC estimates the transformation matrix by randomly sampling point pairs and using a fixed threshold to determine inliers, which makes it sensitive to noise and mismatches [31]. To improve robustness, a weighted error mechanism is introduced. For each candidate correspondence pair ${\{p_i,q_i\}}_{i=1}^N$, the transformation $T=\left[R\right|t]\in SE(3)$ maximizing the weighted inlier count is found as

$$T^{*}=arg\underset{T}{max}\sum _{i=1}^N{w}_i·1\left(\parallel T\left(p_i\right)-q_i\parallel <\sigma \right)$$

(5)

where $w_i$ is a weight function defined as

$$w_i=exp\left(-{\displaystyle \frac{\parallel T\left(p_i\right)-q_i{\parallel }^2}{2{\sigma }^2}}\right)$$

(6)

and $\sigma$ is a decay parameter controlling the influence of error magnitude on the weight. This soft weighting allows each correspondence to contribute proportionally to its residual, improving stability in noisy data.

Finally, validated loop constraints are incorporated into the SLAM pose graph for global optimization. In the graph, nodes represent robot keyframe poses $T_i\in SE\left(3\right)$, and edges represent odometry and loop constraints. The optimization objective is formulated as

$$\underset{T_i}{min}\sum _{(i,j)\in L_{\mathrm{odom}}}\parallel e_{\mathrm{odom}}(T_i,T_j){\parallel }^2+\sum _{(i,j)\in L_{\mathrm{loop}}}{\parallel e_{\mathrm{loop}}(T_i,T_j,T_{ij})\parallel }^2$$

(7)

where $L_{\mathrm{odom}}$ and $L_{\mathrm{loop}}$ denote the sets of odometry and loop-closure edges, respectively, and $e_{\mathrm{odom}}$ and $e_{\mathrm{loop}}$ are the corresponding error terms. This optimization corrects accumulated drift and ensures global trajectory consistency within the SLAM system.

4. Experiments

4.1. Experimental Setup and Datasets

We evaluated the proposed algorithmic framework on two publicly available datasets: the KITTI dataset [32] and the NCLT dataset [33]. For the KITTI dataset, sequences 00, 02, 05, 07, and 08 were selected, which contain 10 Hz 64-line LiDAR point clouds and 10 Hz camera image data. For the NCLT dataset, sequences NCLT1 (2012-01-15), NCLT2 (2012-05-26), NCLT3 (2012-11-04), and NCLT4 (2012-12-01) were used, which contain 10 Hz 32-line LiDAR point clouds and 5 Hz camera image data. In the experiments, we compared our method with four representative algorithms: the density-map-based Map Closure [21], the BEV contour–based Contour Context [7], the bag-of-words–based DBoW2 [9], and the binary feature–aided fusion method iBTC [27]. All experiments were conducted on a consumer-grade desktop computer running Ubuntu 20.04, equipped with an Intel(R) Core(TM) i5-12400 CPU and 16 GB RAM.

The evaluation metrics used to assess loop closure detection performance include Precision, Recall, and F1-score, which are defined as $\mathrm{Precision}=\mathrm{TP}/\left(\mathrm{TP}+\mathrm{FP}\right)$, $\mathrm{Recall}=\mathrm{TP}/\left(\mathrm{TP}+\mathrm{FN}\right)$, and $\mathrm{F}1=2\times \left(\mathrm{Precision}\times \mathrm{Recall}\right)/\left(\mathrm{Precision}+\mathrm{Recall}\right)$, respectively. Here, TP denotes the number of correctly detected loops (true positives), FP denotes the number of false loop detections (false positives), and FN denotes the number of missed loops (false negatives).

4.2. Results on the KITTI Dataset

Table 1 compares the F1-scores of various algorithms on KITTI00, KITTI02, and KITTI05 sequences. The proposed algorithm achieves the highest F1-score across all three sequences, with an average of 85.28%, showing significant improvements over the LiDAR-only Map Closure and vision-only DBoW2. This is attributed to enhanced cross-modal feature correlation via depth completion, which leverages multi-sensor advantages to boost detection accuracy and robustness. Figure 2 presents the Precision-Recall (PR) curves for each sequence. The proposed algorithm exhibits a larger enclosed area in most cases, maintaining higher precision at the same recall rate, thus outperforming others in loop closure detection performance.

Table 2 demonstrates the impact of incorporating vision into the original algorithm. It shows that the proposed algorithm, after fusing vision, increases the number of detections across all four sequences by an average of 5.25, with the maximum similarity improved by 0.38% on average. This verifies that radar-vision fusion enhances the robustness of loop closure detection. For instance, in the KITTI02 sequence with extensive tree occlusion (resulting in incomplete LiDAR point cloud features), the algorithm successfully detects more loops by supplementing visual textures.

Table 3 shows the comparison of ATE before and after loop closure correction for the KITTI07 sequence when the proposed algorithm is applied to the SLAM system, with trajectory comparisons presented in Figure 3. The corrected trajectory aligns better with the ground truth, especially in the loop region where the start and end points coincide (KITTI07 sequence). Cumulative errors are effectively suppressed: the endpoint offset reduces from 0.8m before correction to 0.4m after correction. Figure 4 compares maps of the KITTI07 sequence before and after correction, revealing obvious ghosting (e.g., blurred building boundaries) in the uncorrected map versus clear boundaries post-correction, verifying the algorithm’s effectiveness in enhancing map consistency.

4.3. Results on the NCLT Dataset

The NCLT dataset includes diverse indoor–outdoor mixed environments (e.g., campus corridors, outdoor lawns) and significant seasonal variations, imposing greater challenges on the robustness of loop closure detection algorithms.Table 4 compares the F1-scores of different methods on the NCLT1 and NCLT2 sequences, while Figure 5 shows their corresponding Precision–Recall (PR) curves. As shown in Table 4 and Figure 5, the proposed algorithm achieves the highest F1-scores across both sequences, with an average of 77.63%. This represents an improvement of 10.16% over the LiDAR-only Map Closure method and 47.75% over the vision-only DBoW2 approach. Compared with the multimodal iBTC method, the proposed algorithm achieves an additional 3.61% increase in average F1-score.The improvement arises primarily from the use of the enhanced RANSAC-based geometric verification, which increases robustness against noise and environmental changes. For instance, the NCLT2 sequence contains winter snow scenes, where LiDAR point clouds are heavily affected by reflections from snow surfaces. The proposed algorithm leverages visual texture information during verification to suppress false matches and improve detection reliability.Notably, the proposed method achieves an F1-score of 0.7884 on the NCLT2 sequence—significantly outperforming all other methods demonstrating its superior adaptability and robustness in unstructured and dynamic environments.

Table 5 presents the comparison of loop closure detection performance on the NCLT dataset before and after removing the visual modality from the proposed algorithm. As shown, integrating visual information leads to consistent improvements across all four sequences, with an average increase of 42.75 detected loops and an average rise of 0.56% in maximum similarity.These results confirm the algorithm’s enhanced capability for loop recognition in complex environments. For example, in the NCLT1 sequence, where dense tree occlusions cause sparse LiDAR point clouds and incomplete geometric features, the proposed method successfully identifies more loop closures by incorporating visual texture cues. This demonstrates the effectiveness of LiDAR–vision fusion in improving detection completeness and robustness under challenging outdoor conditions.

Table 6 compares the ATE before and after loop closure correction on the NCLT4 sequence, and Figure 6 illustrates the corresponding trajectory comparison.The NCLT4 sequence includes long indoor corridors (textureless regions) as well as open outdoor areas, posing significant challenges for loop closure detection. By leveraging LiDAR–vision fusion, the proposed algorithm effectively addresses both the lack of LiDAR features in indoor textureless regions and the visual matching errors caused by strong illumination in outdoor scenes.After applying loop closure correction, the trajectory deviation in the indoor corridor section is reduced from approximately 1.2 m before correction to 0.8 m after correction, indicating that the proposed method significantly improves pose accuracy and consistency in mixed indoor–outdoor environments.

5. Conclusion

This paper proposes a radar-visual depth fusion algorithm for loop closure detection in SLAM systems, addressing limitations of single-modality methods and flaws in existing fusion approaches. It achieves effective "geometry-texture" information association through spatiotemporal alignment, depth completion, hybrid feature fusion, and improved RANSAC verification. Limitations remain in dynamic environment robustness, depth fusion mechanisms, real-time performance, and embedded adaptation, which require further improvements.