Real-time state estimation and mapping is a fundamental function of unmanned systems. In this paper, we propose a compact 3D Lidar odometry and mapping method (C-LOAM) for real-time 6-DOF pose estimation and map construction for unmanned ground vehicles. Our approach firstly remove dynamic objects from the environment by comparing differences between two consecutive range images. We then extract ground points from the range image. The remaining points are segmented in the range image to obtain robust feature points for the registration stage. After performing two-stage scan-matching, we use the extracted ground points to impose ground constraints and leverage the local feature submap for loop closure detection. By employing various data reuse strategies, we minimize memory consumption, creating a compact and accurate system. Experiments on multiple publicly available datasets demonstrate that our system provides more accurate localization and higher-quality maps compared to other state-of-the-art methods.
The system receives the raw point cloud from the Lidar as input and performs a spherical projection to obtain a range image. This range image is used for dynamic removal and efficient ground extraction. The remaining points are then segmented, and robust features are extracted for scan-to-scan registration. In the Mapping module, we use the spatial information of key frames to build local feature submaps and use the pose obtained from odometry as a prior for scan-to-map registration. The ground points Gk extracted from the front-end are used to impose ground constraints. Simultaneously, the local feature submap is reused for loop detection to correct the global pose. Finally, the global point cloud map and pose estimation are output.
Our method achieves the highest accuracy across all sequences evaluated. Among them, KITTI00 is an outdoor large-scale sequence with a total length of 3724m. Other methods exhibit significant drift that persist even with loop closure detection. In contrast, our approach achieves minimal drift of only 1.12 meters, benefiting ground constraints and a loop closure strategy based on local feature submap. Sequences like KITTI05 and KITTI07, which feature multiple dynamic vehicles posing challenges to localization, are effectively manage by our dynamic culling strategy. This approach successfully filters out dynamic effects, maintaining high accuracy in odometry estimation.
We compare the performance of our proposed method with LeGO-LOAM in mapping and dynamic object removal using the KITTI07 sequence. Our method utilizes a dynamic removal strategy based on range images to effectively mitigate the impact of dynamic objects in the environment and achieve clearer map construction.
We demonstrate the benefits of our enhanced ground extraction method using the KITTI05 sequence. It is evident that LeGO-LOAM exhibits numerous false positives during ground extraction, whereas our proposed method effectively mitigates this issue and obtains more accurate ground points. This improvement forms a crucial foundation for registration and ground constraint application.
SCLeGO-LOAM produces a large number of false detections, whereas our method ensures robust detection results. The local magnification figure shows SC-LeGO-LOAM results using the same parameters as our method. While using the same parameters reduces the number of false detections for SC-LeGO-LOAM, it also diminishes its ability to detect correct loops. Our method, benefiting from the large-scale properties of submap and the robustness of extracted features, effectively detects the correct loops.
Dynamic Removal: The comparisons between (a) and (f) demonstrate that the dynamic removal strategy enhances registration consistency, thereby improving localization accuracy. However, this does not bring much improvement, we analyze the reason is that KITI05 does not have too many dynamic objects, and the loop detection can correct for historical errors.
Ground Extraction: From the comparisons between (b), (c), and (f), it is evident that applying ground constraints effectively suppresses drift, leading to a significant improvement in positioning accuracy. Additionally, our method enables more accurate ground point extraction compared to LeGO-LOAM ground extraction method.
Loop Closure Detection: Interestingly, comparisons between (d), (e), and (f) reveal that our method achieves promising accuracy even without loop detection. Furthermore, loopback detection descriptors using local feature subgraphs are more effective and competitive than those using raw point clouds.
C-LOAM