Awesome

To be a better FAST-LIO2

0 Introdunction

• Front_end: FAST-LIO2 + Dynamic Removal + Yolo(Optional)
• Back_end: Scan Context + GPS(Optional) + GTSAM
• Application: Joint Pose-graph Optimization using iSAM2, Fast and Robust ICP relocalization

1 Prerequisites

• Ubuntu 20.04 and ROS Noetic
• PCL >= 1.10 (default for Ubuntu 20.04)
• Eigen >= 3.3.4 (default for Ubuntu 20.04)
• GTSAM >= 4.0.3 (test on 4.2(a))
• Livox_ros_driver
• Darknet_ros

3 Build

cd YOUR_WORKSPACE/src
git clone https://github.com/Yixin-F/better_fastlio2
cd ..
catkin_make

4 How to Use

4.1 LIO Mapping using Livox, Velodyne, Ouster or Robosense

In this section, we developed a more comprehensive FAST-LIO2 version including dynamic removal using SCV-OD (accepted by T-GRS) and YOLO(optional), optimization backend using Scan Context, GPS(optional) and GTSAM. At the same time, we rewritted the mechanism of i-kdtree reconstruction to be suitable for low-power embedded devices. The basic framework is illustrated in the following figure.

You can run it by the following commands.

source ./devel/setup.bash
roslaunch fast_lio_sam mapping_*.launch

For other application, you need to first check the Config/*.yaml about the settings for different LiDAR types, we list parameters here. "-" means that it depends on your own project.

Note that if you wanna use the dynamic removal module, you have to make the "src/laserMapping.cpp line 2271-2307" effect. But differnet from the origianl dynamic removal method in T-GRS paper, we utilized the voxel center to align consecutive keyframes, which improves the real-time performance but also sacrifices some precision. So, we do not recommend using the Velodyne VLP16 for testing because its scan point cloud is too sparse. This dynamic removal module can be showed by the following figure.

1. Here we list some important functions in src/laserMapping.cpp as follows:

2. Here we list some important functions in include/dynamic-remove/tgrs.cpp as follows:

3. Here we list some important functions in include/sc-relo/Scancontext.cpp as follows:

After you have run the command, there are several files being generated in the filefold "rootDir/*" as follows:

We show some simple results:

Now, we give some suggestion to improve this special LIO framework: Firstly, change the point-cloud map management mechanism. You can use the sparse method like hash table similar to Faster-LIO.

Secondly, improve the residual calculation method. The point-to-plane registrition during LiDAR measurement, which utilizes generalized patches instead of actual planes may lead to inconsistencies in registration. You can use true planes modeling by uncertainty.

Thirdly, find a better loop closure descriptor, such as STD, G3Reg.

Forthly, detect dynamic points during LiDAR points insertation.

4.2 Multi-session Mapping

In this section, we developed a multi-session mapping module using joint and anchor-based pose-graph optimization, which aims to reduce the cost of repeated mapping and detect differences between them. Seeing the following figure for more details.

You can run it by these commands.

source ./devel/setup.bash
roslaunch fast_lio_sam multi_session.launch

You also need to first check the Config/multi_session.yaml, we list parameters here. "-" means that it depends on your own project.

Here we list some important functions in include/multi-session/incremental_mapping.cpp as follows:

If you wanna run this multi-session module, you should have two-stage results from the LIO mapping module, more details can be found in the last section. We give examples on Parkinglot Dataset here.

We show the details in file "0102" as follows:

We show some simple results:

Here, we also give some suggestion to improve it:

Firstly, do not update the isam2 optimizer immediately upon receiving a relocalization message, because this will cost more memory and time. You can develop some mechanism to select more important relocalization anchor.

Secondly, transfer this offline multi-session code into online mode to adapt to multi-agent exploration like m-tare.

4.3 Object-level Updating

In this section, we developed a object-level updating module to solve the ineffective machanism of point-level method. The difference detection during this updating is similar to the dynamic detection in the period of dynamic removal. As shown by following figure.

You can run it by

source ./devel/setup.bash
roslaunch fast_lio_sam object_update.launch

Note that we just update the local map in similar area, so if you wanna test object-level updating, you should manually select these areas like that we show you in src/object_update.cpp line 235~383.

The upper part means that we choose the 0-50 frames with skip as 5 in 01 to update the 0-30 frames with skip as 3 in 02. Remember that you can change the skip by rewritting the "i" in for-loop. We finally get the updated map of 01.

The following is the expremental results:

As for how to improve it, I do not know. Maybe you can integrate it into LIO revisiting or online relocalization.

4.4 Online Relocalization and Incremental Mapping

In this section, we developed a online relocalization and incremental mapping module, which aims to utilize the prior map during navigation and exploration. More details referred to the following figure.

Yeah, run it by

source ./devel/setup.bash
roslaunch fast_lio_sam online_relocalization.launch
roslaunch fast_lio_sam mapping_*.launch
rosbag play -r * *.bag

You also need to first check the Config/online_relocalization.yaml, we list parameters here. "-" means that it depends on your own project.

The data structure in "priorDir" is similar to the result of lio mapping. Please do not open i-kdtree recontruction, loop closure detection or dynamic removal during online relocalization. You can set the manual pose in rviz by button 2D Pose Estimation. You can finally get the relocalization poses.txt and time.txt.

1. Here we list some important functions in include/online-relo/pose_estimator.cpp as follows:

2. Here we list some important functions in include/FRICP-toolkit/registration.h as follows:

We show some simple results:

How to improve it? Nothing, actually it's meaningless.

5 File Tree

.
├── build
│   ├── atomic_configure
│   ├── catkin
│   ├── catkin_generated
│   ├── CATKIN_IGNORE
│   ├── CMakeCache.txt
│   ├── CMakeFiles
│   ├── CTestConfiguration.ini
│   ├── CTestCustom.cmake
│   ├── devel
│   └── teaser-prefix
├── CMakeLists.txt  // bulid settings
├── config
│   ├── hap_livox.yaml
│   ├── hap_ros.yaml
│   ├── kitti.yaml
│   ├── mulran.yaml
│   ├── multi_session.yaml
│   ├── nclt.yaml
│   ├── online_relo.yaml
│   ├── velodyne16.yaml
│   └── velodyne64_kitti_dataset.yaml
├── include
│   ├── analysis  // test *.py
│   ├── common_lib.h
│   ├── dynamic-remove  // dynamic removal head
│   ├── FRICP-toolkit  // robust and fast ICP head
│   ├── ikd-Tree  // i-kdtree
│   ├── IKFoM_toolkit
│   ├── kitti2bag  // kitti to bag 
│   ├── math_tools.h  // math functions
│   ├── matplotlibcpp.h  // matplotlib
│   ├── multi-session  // multi-session head
│   ├── mutexDeque.h  // mutex tool
│   ├── nanoflann.hpp  // nanoflann
│   ├── online-relo  // online relocalization head
│   ├── sc-relo  // scan context head
│   ├── sophus
│   ├── teaser-toolkit  // teaser head
│   ├── tictoc.hpp  // tictoc
│   ├── tool_color_printf.h  // colerful print
│   └── use-ikfom.hpp
├── launch
│   ├── mapping_hap_livox.launch  // livox mapping
│   ├── mapping_hap_ros.launch
│   ├── mapping_mulran.launch  // mulran mapping
│   ├── mapping_velodyne16.launch  // velodyne mapping
│   ├── mapping_velodyne64_kitti_dataset.launch  // kitti mapping
│   ├── multi_session.launch  // multi-session
│   ├── object_update.launch  // object-level updating
│   └── online_relo.launch  // online relocalization
├── LICENSE
├── Log
│   ├── fast_lio_time_log_analysis.m  // time analysis
│   ├── guide.md
│   ├── imu.txt  // imu poses output
│   └── plot.py  // plot using matplotlib
├── msg
│   ├── cloud_info.msg  // cloud msg
│   └── Pose6D.msg  // pose msg
├── note.txt  // development note
├── package.xml
├── pic
│   ├── color.png
│   ├── lio_file.png
│   ├── multi_session_details.png
│   ├── multi-session.png
│   ├── update_details.png
│   ├── update.png
│   └── yolo.png
├── README.md
├── rviz_cfg
│   ├── fastlio_hk.rviz
│   ├── loam_livox.rviz
│   ├── loc_new.rviz
│   ├── loc.rviz
│   └── sc_relo.rviz
├── src
│   ├── IMU_Processing.hpp  // imu process -main
│   ├── laserMapping.cpp  // esekf mapping -main
│   ├── multi_session.cpp  // multi-session -main
│   ├── object_update.cpp  // object-level updating -main
│   ├── online_relocalization.cpp  // online relocalization -main
│   ├── preprocess.cpp  // lidar process -main
│   └── preprocess.h
└── srv
    ├── save_map.srv  // service to save map
    └── save_pose.srv  // service to save poses

6 References

• Baseline: https://github.com/JS-622/YOLO-fast-lio-sam
• PGO(GPS): https://github.com/gisbi-kim/FAST_LIO_SLAM
• FR-ICP：https://github.com/yaoyx689/Fast-Robust-ICP
• T-GRS: https://github.com/Yixin-F/DR-Using-SCV-OD
• Multi-session: https://github.com/Yixin-F/LT-mapper_fyx