☀️ DLC_SLAM 🔥 💪

This is the offical implementation of our project "A Robust and Effective LiDAR-SLAM System with Learning-based Denoising and Loop Closure", which achieves robust learning-based LiDAR SLAM in real-time on real robotic platforms.

• Front_end : Fast LiDAR-Inertial Odometry + D-Net 💪 💪 💪

• Back_end: Tightly-coupled Lidar Inertial Odometry + LC-Net 💪 💪 💪

Quick Navigation

For the systematic ROS implementation of LIO-SAM, please refer to LIO-SAM_DLC.

For the systematic ROS implementation of LIO-SAM and the software as well as hardware integration into the real robotics platforms such as UAVs and UGVs, please refer to LIO-SAM_Intergrated_DLC_Fast, and LIO-SAM_Intergrated_DLC.

For the systematic ROS implementation of LVI-SAM and the software as well as hardware integration with visual sensors such as RGB-D cameras, please refer to LVI-SAM_DLC.

Demo Results

Results on complicated large-scale real-world circumstances with each more than 50000 square meters

Results on public KITTI Benchmark

The Contributions

1. We have proposed the network D-Net for denoising and the LC-Net for loop closure detection and they are both integrated into our DLC-SLAM system to improve the robustness and accuracy compared with current state-of the art solutions. To the best of our knowledge, we propose the first LiDAR SLAM system that can operate in real-time for large-scale robotics localization and mapping in real-world noisy environments with low textures to date.

2. Use the externally connected PGO loopback detection module for back-end optimization. FAST_LIO_SAM transplants the back-end GTSAM optimization part of LIO-SAM to the code of FAST-LIO2, and the data transmission and processing link is clearer.

3. Add keyframe saving, and save maps and tracks through rosservice commands.

4. The back-end optimization in FAST_LIO_SLAM only uses the high-level GPS for constraints. The high-level GPS is generally noisy, so the XYZ three-dimensional postion of GPS is added to constrain the GPS a priori factor.

Summary of the merits of our works based on previous works:

1. We have improved the global consitency of the previous SLAM systems. The tightly coupled lio slam system lacks global consistency because of its lack of front-end. Refer to the back-end part of lio_sam and connect to GTSAM for back-end optimizations.

2. Added on the basis of FAST_LIO_SLAM: 1. Euclidean distance-based loop detection search based on Radius Search, which increases the robustness of loop closure search; 2. The optimization result of loop detection is updated to the current frame pose of FAST-LIO2, and Refactor ikdtree to update the local sub-map.

Prerequisites

• Ubuntu 18.04 and ROS Melodic
• PCL >= 1.8 (default for Ubuntu 18.04)
• Eigen >= 3.3.4 (default for Ubuntu 18.04)
• GTSAM >= 4.0.0(tested on 4.0.0-alpha2)

Build

cd YOUR_WORKSPACE/src
git clone https://github.com/kahowang/FAST_LIO_SAM.git
cd ..
catkin_make

Quick test

Loop clousre：

1 .For indoor dataset

The demo test dataset is from FAST_LIO_LC ,dataset which includes /velodyne_points(10Hz) and /imu/data(400Hz).

roslaunch fast_lio_sam mapping_velodyne16.launch
rosbag play  T3F2-2021-08-02-15-00-12.bag  

2 .For outdoor dataset

dataset is from LIO-SAM Walking dataset: [Google Drive]

roslaunch fast_lio_sam mapping_velodyne16_lio_sam_dataset.launch
rosbag  play  walking_dataset.bag

3.save_map

Enter the following command into the terminal, the map file will be saved in the appropriate folder:

rosservice call /save_map "resolution: 0.0
destination: ''" 
success: True

4.save_poes

Enter the following command into the terminal, the poes file will be saved in the corresponding folder:

rosservice call /save_pose "resolution: 0.0
destination: ''" 
success: False

evo plot trajectories.

evo_traj kitti optimized_pose.txt without_optimized_pose.txt -p

5.some config

# Loop closure
loopClosureEnableFlag: true		      # use loopclousre or not 
loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

# visual iktree_map  
visulize_IkdtreeMap: true

# visual iktree_map  
recontructKdTree: true

savePCDDirectory: "/fast_lio_sam_ws/src/FAST_LIO_SAM/PCD/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

Use GPS：

1.dataset

dataset is from LIO-SAM Park dataset: [Google Drive]

roslaunch fast_lio_sam mapping_velodyne16_lio_sam_dataset.launch
rosbag  play  parking_dataset.bag

Line Color define: path_no_optimized(blue)、path_updated(red)、path_gnss(green)

2.save_map

Enter the following command into the terminal, the map file will be saved in the appropriate folder:

rosservice call /save_map "resolution: 0.0
destination: ''" 
success: True

FAST-LIO Map (no gnss prior factor) Red ; FAST-LIO-SAM (with gnss prior factor) Blue

3.save_poes

Obtain the updated ENU and enter the following command into the terminal, the poes file will be saved in the corresponding folder:

rosservice call /save_pose "resolution: 0.0
destination: ''" 
success: False

evo plot trajectories:

evo_traj kitti gnss_pose.txt optimized_pose.txt  -p

4.some config

# GPS Settings
useImuHeadingInitialization: false           # if using GPS data, set to "true"
useGpsElevation: false                      # if GPS elevation is bad, set to "false"
gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
poseCovThreshold: 0 #25.0                      # m^2, threshold for using GPS data  Pose Covariance Threshold: from isam2

5.some fun

when you want to see the path in the Map，you can also use Mapviz plugin . You can refer to the clearly posted and documented blog on CSDN.

Some checklists during implementations:

1. In FAST-LIO2, the pose attitude is represented by so3, while in gtsam, the input relative_pose attitude is expressed in Euler RPY form, which needs to be converted and updated using Rodriguez's formula.

2. iktree reconstruct

3. In the walking data set, because some data are constantly holding the rotating lidar at the same place, the angle of the rotating lidar reaches the threshold for saving key frames. In a short period of time, multiple similar key frames are saved, causing ISAM2 to appear. If the feature degrades and the odometer runs away, the threshold parameters for key frame selection can be adjusted appropriately according to the data set.

4. Add the diamante part of the GPS prior prior factor, refer to the prior factor part of lio_sam, compared withFAST-LIO-SLAM, FAST-LIO-SLAM only uses the high-level constraints of GPS, and does not use constraints in the xy direction. However, the error of GPS in the high layer (Z axis) is relatively large, and it is easy to introduce errors in the optimization process.

5. Among the GPS prior factors, "useGpsElevation" selects the high-level constraint of GPS, which is not used by default, because the high-level noise of GPS is relatively large.

6. LIO-SAM uses ekf_localization_node this ROS Package to transfer the GPS WGS84 coordinate system to the World system. FAST-LIO-SAM considers decoupling with the external ROS package as much as possible, and calls GeographicLib for coordinate conversion.