- 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
- 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
cd YOUR_WORKSPACE/src
git clone https://github.com/Yixin-F/better_fastlio2
cd ..
catkin_makeIn 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_*.launchFor 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.
| Parameters | 中文解释 | default(默认值) |
|---|---|---|
| lid_topic | 雷达话题名称 | - |
| imu_topic | IMU话题名称 | - |
| time_sync_en | 是否进行时间软同步 | false |
| rootDir | 结果保存根路径 | - |
| savePCD | 是否保存点云帧PCD | true |
| savePCDDirectory | 点云帧PCD保存路径 | PCDs/ |
| saveSCD | 是否保存点云帧SCD | true |
| saveSCDDirectory | 点云帧SCD保存路径 | SCDs/ |
| saveLOG | 是否保存LOG文件 | true |
| saveLOGDirectory | LOG文件保存路径 | LOG/ |
| map_save_en | 是否保存地图 | true |
| pcd_save_interval | -（未使用） | - |
| lidar_type | 雷达类型 | - |
| livox_type | Livox类型 | - |
| scan_line | 雷达线数 | - |
| blind | 无效距离范围(m) | 1.0 |
| feature_enabled | 是否进行特征提取 | false |
| point_filter_num | 有效采样点步长 | 1 |
| scan_rate | 扫描频率 | - |
| time_unit | 时间单位 | - |
| camera_en | 是否使用相机 | false |
| camera_external | 相机到IMU外参 | - |
| camera_internal | 相机内参 | - |
| acc_cov | 线加速度协方差 | 0.1 |
| gyr_cov | 角速度协方差 | 0.1 |
| b_acc_cov | 线加速度偏置协方差 | 0.001 |
| b_gyr_cov | 角速度偏置协方差 | 0.001 |
| fov_degree | 视角范围(deg) | 180.0 |
| det_range | 最远探测距离(m) | 100.0 |
| cube_len | i-kdtree维护立方体边长(m) | 500 |
| extrinsic_est_en | 是否在线标定雷达到IMU外参 | false |
| mappingSurfLeafSize | 点云地图下采样分辨率(m) | 0.2 |
| keyframeAddingDistThreshold | 关键帧平移阈值(m) | 1.0 |
| keyframeAddingAngleThreshold | 关键帧旋转阈值(rad) | 0.2 |
| extrinsic_T | 雷达到IMU平移外参 | - |
| extrinsic_R | 雷达到IMU旋转外参 | - |
| max_iteration | ESEKF最大迭代次数 | 3 |
| recontructKdTree | 是否重建i-kdtree | false |
| kd_step | i-kdtree重建步长 | 50 |
| filter_size_map_min | i-kdtree下采样分辨率(m) | 0.2 |
| loopClosureEnableFlag | 是否开启回环检测 | false |
| loopClosureFrequency | 回环检测频率(hz) | 1.0 |
| historyKeyframeSearchRadius | 有效回环检测搜索半径(m) | 10.0 |
| historyKeyframeSearchTimeDiff | 有效回环检搜索时差(s) | 30.0 |
| historyKeyframeSearchNum | 历史帧搜索个数 | 1 |
| historyKeyframeFitnessScore | icp检验阈值 | 0.2 |
| ground_en | 是否进行地面约束 | false |
| tollerance_en | 是否使用自由度阈值约束 | false |
| sensor_height | 传感器高度(m) | - |
| z_tollerance | z轴约束阈值(m) | 2.0 |
| rotation_tollerance | pitch和roll约束阈值(rad) | 0.2 |
| path_en | 是否发布位姿轨迹 | true |
| scan_publish_en | 是否发布点云 | true |
| dense_publish_en | 是否发布稠密点云 | true |
| scan_bodyframe_pub_en | 是否发布IMU坐标系下点云 | false |
| globalMapVisualizationSearchRadius | i-kdtree搜索距离(m) | 10.0 |
| globalMapVisualizationPoseDensity | i-kdtree位姿采样步长 | 1 |
| globalMapVisualizationLeafSize | i-kdtree可视化下采样分辨率(m) | 0.2 |
| visulize_IkdtreeMap | 是否发布i-kdtree | true |
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.
- Here we list some important functions in src/laserMapping.cpp as follows:
- Here we list some important functions in include/dynamic-remove/tgrs.cpp as follows:
| Function Name | 中文解释 |
|---|---|
| mergeClusters | 聚类覆盖 |
| findVoxelNeighbors | 搜索近邻体素 |
| cluster | 聚类主函数 |
| getBoundingBoxOfCloud | 获取聚类物体boundingbox |
| getCloudByVec | 使用vector提取点云 |
| recognizePD | 识别潜在动态物体主函数 |
| trackPD | 跟踪潜在动态物体主函数 |
| saveColorCloud | 按照聚类类别保存彩色点云 |
- Here we list some important functions in include/sc-relo/Scancontext.cpp as follows:
| Function Name | 中文解释 |
|---|---|
| coreImportTest | - |
| rad2deg | rad2deg |
| deg2rad | deg2rad |
| xy2theta | xy2theta |
| circshift | 矩阵行平移 |
| eig2stdvec | 矩阵转换为vector |
| distDirectSC | sc矩阵距离计算 |
| fastAlignUsingVkey | sc矩阵列匹配 |
| distanceBtnScanContext | sc矩阵相似度计算 |
| makeScancontext | sc生成 |
| makeRingkeyFromScancontext | ring生成 |
| makeSectorkeyFromScancontext | sector对应 |
| makeAndSaveScancontextAndKeys | sc生成主函数 |
| saveScancontextAndKeys | sc插入 |
| detectLoopClosureIDBetweenSession | multi-session重定位检测主函数 |
| getConstRefRecentSCD | sc获取 |
| detectClosestKeyframeID | 回环帧ID获取 |
| detectLoopClosureID | 回环帧ID获取 |
| saveCurrentSCD | scd保存 |
| loadPriorSCD | multi-session加载先验scd |
| relocalize | multi-session重定位主函数 |
After you have run the command, there are several files being generated in the filefold "rootDir/*" as follows:
| File Name | 中文解释 |
|---|---|
| LOG | 日志文件 |
| PCDs | PCD格式 关键帧点云 |
| SCDs | SCD格式 关键帧Scan Context描述子 |
| globalMap.pcd | PCD格式 全局地图 |
| singlesession_posegraph.g2o | g2o格式 全局位姿图 |
| trajectory.pcd | PCD格式 xyz位姿轨迹 |
| transformations.pcd | PCD格式 xyz+rpy位姿轨迹 |
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.
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.launchYou also need to first check the Config/multi_session.yaml, we list parameters here. "-" means that it depends on your own project.
| Parameters | 中文解释 | default(默认值) |
|---|---|---|
| sessions_dir | 存储多个lio结果的根路径 | - |
| central_sess_name | 中心阶段lio文件名称 | - |
| query_sess_name | 子阶段lio文件名称 | - |
| save_directory | 多阶段结果保存路径 | - |
| iteration | isam2迭代优化次数 | 3 |
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.
| File Name | 中文解释 |
|---|---|
| 01 ~ 02 | 01 ~ 02 的单阶段 lio mapping 结果，文件格式同 4.1 |
| 01** | ** 对 01 的 multi-session 结果 |
We show the details in file "0102" as follows:
| File Name | 中文解释 |
|---|---|
| 01_central_aft_intersession_loops.txt | TXT格式 multi-session后 中心坐标系下01位姿轨迹 |
| 01_central_bfr_intersession_loops.txt | TXT格式 multi-session前 中心坐标系下01位姿轨迹 |
| 01_local_aft_intersession_loops.txt | TXT格式 multi-session后 子坐标系下01位姿轨迹 |
| 01_local_bfr_intersession_loops.txt | TXT格式 multi-session前 子坐标系下01位姿轨迹 |
| 02_central_aft_intersession_loops.txt | TXT格式 multi-session后 中心坐标系下02位姿轨迹 |
| 02_central_bfr_intersession_loops.txt | TXT格式 multi-session前 中心坐标系下02位姿轨迹 |
| 02_local_aft_intersession_loops.txt | TXT格式 multi-session后 子坐标系下02位姿轨迹 |
| 02_local_bfr_intersession_loops.txt | TXT格式 multi-session前 子坐标系下02位姿轨迹 |
| aft_map2.pcd | PCD格式 multi-session后 中心坐标系下0102拼接地图 |
| aft_transformation1.pcd | PCD格式 multi-session后 中心坐标系下01地图 |
| aft_transformation2.pcd | PCD格式 multi-session后 中心坐标系下02地图 |
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.
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.launchNote 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.
| File Name | 中文解释 |
|---|---|
| prior_map_select.pcd | PCD格式 先验被更新地图的区域 |
| cur_map_select.pcd | PCD格式 当前更新地图的区域 |
| result.pcd | PCD格式 更新地图结果 |
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.
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 * *.bagYou also need to first check the Config/online_relocalization.yaml, we list parameters here. "-" means that it depends on your own project.
| Parameters | 中文解释 | default(默认值) |
|---|---|---|
| priorDir | 先验知识根路径 | - |
| cloudTopic | lio点云发布话题 | - |
| poseTopic | lio位姿发布话题 | - |
| searchDis | 近邻先验关键帧搜索半径(m) | 5.0 |
| searchNum | 近邻先验关键帧搜索个数 | 3 |
| trustDis | 区域覆盖搜索半径(m) | 5.0 |
| regMode | 配准方法 | 4 |
| extrinsic_T | 雷达到IMU平移外参 | - |
| extrinsic_R | 雷达到IMU旋转外参 | - |
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.
- Here we list some important functions in include/online-relo/pose_estimator.cpp as follows:
| Function Name | 中文解释 |
|---|---|
| pose_estimator | 在线定位类构造函数 |
| allocateMemory | - |
| cloudCBK | 里程计点云回调函数 |
| poseCBK | 里程计位姿回调函数 |
| externalCBK | rviz设置位姿回调函数 |
| run | 在线定位主函数 |
| easyToRelo | 覆盖区域检测主函数 |
| globalRelo | 全局位姿出初始化主函数 |
| publish_odometry | 在线定位里程计发布 |
| publish_path | 在线定位轨迹发布 |
- Here we list some important functions in include/FRICP-toolkit/registration.h as follows:
| Function Name | 中文解释 |
|---|---|
| Registeration | 配准类构造函数 |
| run | 配准主函数 |
We show some simple results:
How to improve it? Nothing, actually it's meaningless.
.
├── 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
- 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