better_fastlio2

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

Note

• The offline dynamic removal module using segmented curved-voxel occupancy descriptor (T-GRS 2024) can be found in repo
• The modified multi-session mapping module based on LT-Mapping (ICRA 2022) can be found in repo
• The new version of online relocalization module name LiLoc (ICRA 2025) can be found in repo
• Some details of sensor platform and application can be found in repo

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

3.1 Source Code

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

3.2 Docker

docker pull jonathonyin/better-fast-lio2:0.1.0
xhost +local:docker
docker run -it \
    --gpus all \
    --privileged \
    --env="NVIDIA_DRIVER_CAPABILITIES=all" \
    --network host \
    -e DISPLAY=$DISPLAY \
    -e QT_X11_NO_MITSHM=1 \
    -v ~/catkin_ws:/workspace/catkin_ws \
    -v /tmp/.X11-unix:/tmp/.X11-unix \
    -v /dev/video0:/dev/video0 \
    -v /dev/video1:/dev/video1 \
    -v /dev/snd:/dev/snd \
    --device=/dev/dri:/dev/dri \
    --name="better-fast-lio2" \
    jonathonyin/better-fast-lio2:0.1.0
#Now you've entered the Docker container
cd /workspace/catkin_ws/src
git clone https://github.com/Ian-YJX/better_fastlio2
mv better_fastlio2 fast_lio_sam #the package name in the original code is fast_lio_sam
cd ..
catkin_make #if you have met an error like "fatal error: fast_lio_sam/Pose6D.h: No such file or directory",just try again. This is due to the improper order of some lines in CMakeLists.txt

One problem: if you want to use RViz or Gazebo, exit the container and enter again

docker start better-fast-lio2
docker exec -it better-fast-lio2 /bin/bash

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.

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.

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

Function Name	中文解释
imageCallback	usb图像回调函数 720x1280
paramSetting	usb相机内参与外参设置
BoxCallback	yolo目标检测包络框
publish_frame_world_color	彩色点云发布
updatePath	更新里程计轨迹
constraintTransformation	位姿变换限制
getCurPose	获得当前位姿
visualizeLoopClosure	发布回环检测marker
saveFrame	保存关键帧
addOdomFactor	添加激光里程计因子
addLoopFactor	添加回环因子
recontructIKdTree	i-kdtree重建
saveKeyFramesAndFactor	关键帧保存、因子添加和因子图优化主函数
correctPoses	更新优化后的位姿
detectLoopClosureDistance	回环检测--近邻搜索
loopFindNearKeyframes	搜索最近关键帧
performLoopClosure	回环检测--scan context，回环检测执行
loopClosureThread	回环检测主函数
SigHandle	ctrl+c 终止函数
dump_lio_state_to_log	保存log文件
pointBodyToWorld_ikfom	点云变换到世界坐标系下
pointBodyToWorld	点云变换到世界坐标系下
pointBodyToWorld	点云变换到世界坐标系下
RGBpointBodyToWorld	彩色点云变换到世界坐标系下
RGBpointBodyLidarToIMU	彩色点云变换到IMU坐标系下
points_cache_collect	删除i-kdtree缓存
lasermap_fov_segment	FoV视角分割
standard_pcl_cbk	标准pcl点云回调函数
livox_pcl_cbk	livox pcl点云回调函数
livox_ros_cbk	ros pcl点云回调函数
imu_cbk	imu回调函数
LivoxRepubCallback	-
map_incremental	i-kdtree增量管理
publish_frame_world	发布世界坐标系下点云
publish_frame_body	发布IMU坐标系下点云
publish_effect_world	发布世界坐标系下有效点云
publish_map	发布世界坐标系下i-kdtree点云
publish_effect_world	设置位姿
set_posestamp	发布世界坐标系下有效点云
publish_odometry	发布里程计
publish_path_imu	发布IMU轨迹
publish_path	发布未优化位姿
publish_path_update	发布优化后位姿
CreateFile	创建文件夹
savePoseService	位姿保存服务
saveMapService	地图保存服务
savePoseService	位姿保存服务
saveMap	地图保存触发
publishGlobalMap	发布世界坐标系下关键帧点云
h_share_model	观测更新主函数
main	里程计主函数

2. 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	按照聚类类别保存彩色点云

3. 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.

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.

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:

Function Name	中文解释
fileNameSort	文件名称排序
pairIntAndStringSort	std::pair排序
Session	单阶段类构建函数
initKeyPoses	位姿初始化
updateKeyPoses	位姿更新
loopFindNearKeyframesCentralCoord	搜索中心位姿图中的近邻位姿
loopFindNearKeyframesLocalCoord	搜索子位姿图中的近邻位姿
loadSessionKeyframePointclouds	加载关键帧点云 PCD格式
loadSessionScanContextDescriptors	加载关键帧sc SCD格式
loadSessionGraph	加载位姿图 PCD格式
loadGlobalMap	加载点云地图
getPoseOfIsamUsingKey	从isam2中获取位姿
writeAllSessionsTrajectories	保存单阶段位姿图 G2O格式
run	联合位姿图优化主函数
initNoiseConstants	初始化噪声
initOptimizer	初始化isam2优化器
updateSessionsPoses	根据anchor更新位姿图
optimizeMultisesseionGraph	位姿图更新主函数
doICPVirtualRelative	ICP验证
doICPGlobalRelative	ICP验证
detectInterSessionSCloops	sc重定位主函数
detectInterSessionRSloops	rs重定位主函数
equisampleElements	sc重定位std::pair保存
addSCloops	sc重定位因子添加主函数
calcInformationGainBtnTwoNodes	重定位优化残差主函数
findNearestRSLoopsTargetNodeIdx	rs重定位std::pair保存
addRSloops	rs重定位因子添加主函数
addSClinitTrajectoryByAnchoringoops	anchor生成主函数
addSessionToCentralGraph	添加节点
loadAllSessions	加载文件名称
visualizeLoopClosure	可视化重定位因子边
loadCentralMap	加载中心地图
getReloKeyFrames	获得重定位关键帧主函数

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.

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.

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.

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.

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.

1. 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	在线定位轨迹发布

2. 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.

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

citation

If you use any code in this project, please cite paper

@article{fang2024segmented,
  title={Segmented Curved-Voxel Occupancy Descriptor for Dynamic-Aware LiDAR Odometry and Mapping},
  author={Fang, Yixin and Qian, Kun and Zhang, Yun and Shi, Tong and Yu, Hai},
  journal={IEEE Transactions on Geoscience and Remote Sensing},
  year={2024},
  publisher={IEEE}
}

and our paper

@article{fang2024liloc,
  title={LiLoc: Lifelong Localization using Adaptive Submap Joining and Egocentric Factor Graph},
  author={Fang, Yixin and Li, Yanyan and Qian, Kun and Tombari, Federico and Wang, Yue and Lee, Gim Hee},
  journal={arXiv preprint arXiv:2409.10172},
  year={2024}
}