engcang / fast-lio-sam

a SLAM implementation combining FAST-LIO2 with pose graph optimization and loop closing based on LIO-SAM paper

FAST-LIO-SAM

• This repository is a SLAM implementation combining FAST-LIO2 with pose graph optimization and loop closing based on LIO-SAM paper
  • Loop-detection is based on radius search and ICP is used to calc matching
• Note: similar repositories already exist
  • FAST_LIO_LC: FAST-LIO2 + SC-A-LOAM based SLAM
  • FAST_LIO_SLAM: FAST-LIO2 + ScanContext based SLAM
  • FAST_LIO_SAM: FAST-LIO2 + LIO-SAM
• Note2: main code (PGO) is modularized and hence can be combined with any other LIO / LO
  • This repo is to learn GTSAM myself!
  • and as GTSAM tutorial for beginners - GTSAM 튜토리얼 한글 포스팅

KITTI seq 05 top view - (left): FAST-LIO2 (right): FAST-LIO-SAM

KITTI seq 05 side view - (top): FAST-LIO2 (bottom): FAST-LIO-SAM

KITTI seq 05 trajectories - (blue): FAST-LIO2 (green): FAST-LIO-SAM

Note

• For better loop-detection and transform calculation, FAST-LIO-SAM-QN is also coded and opened.
  • It adopts Quatro - fast, accurate and robust global registration which provides great initial guess of transform
  • and Nano-GICP - fast and accurate ICP combining FastGICP + NanoFLANN

Dependencies

• ROS (it comes with Eigen and PCL)
• GTSAM

  wget -O gtsam.zip https://github.com/borglab/gtsam/archive/refs/tags/4.1.1.zip
  unzip gtsam.zip
  cd gtsam-4.1.1/
  mkdir build && cd build
  cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON ..
  sudo make install -j16

How to build and use

• Get the code and build

  cd ~/your_workspace/src
  git clone https://github.com/engcang/FAST-LIO-SAM --recursive
  
  cd ..
  catkin build -DCMAKE_BUILD_TYPE=Release
  . devel/setup.bash

• Then run (change config files in third_party/FAST_LIO)

  roslaunch fast_lio_sam run.launch lidar:=ouster
  roslaunch fast_lio_sam run.launch lidar:=velodyne
  roslaunch fast_lio_sam run.launch lidar:=livox

Structure

• odom_pcd_cb
  • pub realtime pose in corrected frame
  • keyframe detection -> if keyframe, add to pose graph + save to keyframe queue
  • pose graph optimization with iSAM2
• loop_timer_func
  • process a saved keyframe
    • detect loop -> if loop, add to pose graph
• vis_timer_func
  • visualize all (Note: global map is only visualized once uncheck/check the mapped_pcd in rviz to save comp.)

fast-lio-sam's Issues

"--Type <RET> for more, q to quit, c to continue without paging--" appears even after disabling

First of all, thanks for the great repository. I have the following issue:

roslaunch fast_lio_sam run.launch lidar:=velodyne

... logging to /home/catolica/.ros/log/dddf32c2-40fe-11ee-89e7-976de36976e8/roslaunch-catolica-Z790-UD-AC-17570.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
....
....
....
GNU gdb (Ubuntu 9.2-0ubuntu1~20.04.1) 9.2
Copyright (C) 2020 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.
Type "show copying" and "show warranty" for details.
This GDB was configured as "x86_64-linux-gnu".
Type "show configuration" for configuration details.
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
Find the GDB manual and other documentation resources online at:
    <http://www.gnu.org/software/gdb/documentation/>.

For help, type "help".
Type "apropos word" to search for commands related to "word"...
Reading symbols from /home/catolica/Documents/tumi_robotics_projects/chavito_escalamiento/ros/ws_fastlio_sam/devel/lib/fast_lio_sam/fast_lio_sam_node...
Multi thread started 
p_pre->lidar_type 2
~~~~/home/catolica/Documents/tumi_robotics_projects/chavito_escalamiento/ros/ws_fastlio_sam/src/FAST-LIO-SAM/third_party/FAST_LIO/ file opened
Starting program: /home/catolica/Documents/tumi_robotics_projects/chavito_escalamiento/ros/ws_fastlio_sam/devel/lib/fast_lio_sam/fast_lio_sam_node __name:=fast_lio_sam_node __log:=/home/catolica/.ros/log/dddf32c2-40fe-11ee-89e7-976de36976e8/fast_lio_sam_node-4.log
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7fffeeed2700 (LWP 17761)]
[New Thread 0x7fffee6d1700 (LWP 17762)]
[New Thread 0x7fffeded0700 (LWP 17763)]
[New Thread 0x7fffed6cf700 (LWP 17764)]
[New Thread 0x7fffecece700 (LWP 17765)]
[ WARN] [1692717391.061076048]: Main class, starting node...
[New Thread 0x7fffd7fff700 (LWP 17766)]
[New Thread 0x7fffdffff700 (LWP 17767)]
[New Thread 0x7fffdf7fe700 (LWP 17768)]
[New Thread 0x7fffdeffd700 (LWP 17769)]
[ INFO] [1692717403.395610851]: IMU Initial Done
[ WARN] [1692717403.397405751]: No point, skip this scan!

[New Thread 0x7fffde7fc700 (LWP 17832)]
[New Thread 0x7fffde3fb700 (LWP 17833)]
[New Thread 0x7fffddffa700 (LWP 17834)]
[New Thread 0x7fffdd7f8700 (LWP 17837)]
[New Thread 0x7fffdd3f7700 (LWP 17835)]
[New Thread 0x7fffddbf9700 (LWP 17836)]
[New Thread 0x7fffdcff6700 (LWP 17839)]
[New Thread 0x7fffd73fd700 (LWP 17841)]
[New Thread 0x7fffd77fe700 (LWP 17842)]
[New Thread 0x7fffdc7f4700 (LWP 17840)]
[New Thread 0x7fffdcbf5700 (LWP 17838)]
[New Thread 0x7fffd5ff8700 (LWP 17848)]
[New Thread 0x7fffd5bf7700 (LWP 17847)]
[New Thread 0x7fffd63f9700 (LWP 17846)]
[New Thread 0x7fffd57f6700 (LWP 17849)]
[New Thread 0x7fffd67fa700 (LWP 17845)]
[New Thread 0x7fffd53f5700 (LWP 17850)]
[New Thread 0x7fffd6bfb700 (LWP 17844)]
[New Thread 0x7fffd6ffc700 (LWP 17843)]
[New Thread 0x7fffd4bf3700 (LWP 17853)]
[New Thread 0x7fff97bfe700 (LWP 17854)]
[New Thread 0x7fffd4ff4700 (LWP 17851)]
[New Thread 0x7fffd47f2700 (LWP 17852)]
[New Thread 0x7fff97fff700 (LWP 17855)]
[New Thread 0x7fff977fd700 (LWP 17856)]
[New Thread 0x7fff973fc700 (LWP 17857)]
[New Thread 0x7fff96ffb700 (LWP 17858)]
[New Thread 0x7fff967f9700 (LWP 17860)]
[New Thread 0x7fff96bfa700 (LWP 17859)]
[New Thread 0x7fff963f8700 (LWP 17861)]
[New Thread 0x7fff95ff7700 (LWP 17862)]
[ INFO] [1692717404.061472613]: copy: 0.1, loop: 0.0
[ INFO] [1692717404.064976845]: vis: 3.6ms
[ INFO] [1692717405.061335102]: vis: 0.1ms
[ INFO] [1692717406.061242500]: vis: 0.1ms
[ INFO] [1692717407.061174266]: vis: 0.0ms
[ INFO] [1692717408.061236380]: vis: 0.0ms
[ INFO] [1692717409.061190220]: vis: 0.0ms
[ INFO] [1692717410.061216372]: vis: 0.0ms
[ INFO] [1692717411.061264425]: vis: 0.0ms
[ INFO] [1692717412.061084622]: vis: 0.0ms
[ INFO] [1692717413.061350674]: vis: 0.1ms
**--Type <RET> for more, q to quit, c to continue without paging--**
Thread 9 "fast_lio_sam_no" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffdf7fe700 (LWP 17768)]
0x00007ffff6218f78 in std::_Rb_tree_node<std::pair<unsigned long const, int> >* std::_Rb_tree<unsigned long, std::pair<unsigned long const, int>, std::_Select1st<std::pair<unsigned long const, int> >, std::less<unsigned long>, tbb::tbb_allocator<std::pair<unsigned long const, int> > >::_M_copy<std::_Rb_tree<unsigned long, std::pair<unsigned long const, int>, std::_Select1st<std::pair<unsigned long const, int> >, std::less<unsigned long>, tbb::tbb_allocator<std::pair<unsigned long const, int> > >::_Alloc_node>(std::_Rb_tree_node<std::pair<unsigned long const, int> > const*, std::_Rb_tree_node_base*, std::_Rb_tree<unsigned long, std::pair<unsigned long const, int>, std::_Select1st<std::pair<unsigned long const, int> >, std::less<unsigned long>, tbb::tbb_allocator<std::pair<unsigned long const, int> > >::_Alloc_node&) () from /usr/local/lib/libgtsam.so.4
(gdb) 

How could I disable correctly "--Type for more, q to quit, c to continue without paging--" in order that it does not stop the algorithm?
The algorithms works well where this message appears:

[INFO] [1692717404.061472613]: copy: 0.1, loop: 0.0
[ INFO] [1692717404.064976845]: vis: 3.6ms
[ INFO] [1692717405.061335102]: vis: 0.1ms
[ INFO] [1692717406.061242500]: vis: 0.1ms
[ INFO] [1692717407.061174266]: vis: 0.0ms
[ INFO] [1692717408.061236380]: vis: 0.0ms
[ INFO] [1692717409.061190220]: vis: 0.0ms
[ INFO] [1692717410.061216372]: vis: 0.0ms
[ INFO] [1692717411.061264425]: vis: 0.0ms
[ INFO] [1692717412.061084622]: vis: 0.0ms
[ INFO] [1692717413.061350674]: vis: 0.1ms

But always after 15 sec approximately the "--Type <RET> for more, q to quit, c to continue without paging--" stops only fastlio_sam_node.

I have tried to add some extra commands to launch-prefix:

<!-- run fast_lio_sam -->
	<rosparam command="load" file="$(find fast_lio_sam)/config/config.yaml" />
	<node pkg="fast_lio_sam" type="fast_lio_sam_node" name="fast_lio_sam_node" output="screen" **launch-prefix="gdb -ex 'set pagination off' -ex run --args"**/> 

and even removing launch-prefix from this launch line, that results that the output does not appear but in same way stops after 15 secs. Some recommendations to deal with that? It was tested in Jetson Orin AGX and PC with RTX 4080 + 13th Gen Intel® Core™ i9-13900KF, both have Ubuntu 20.04 and ROS Noetic.

refuse to save final pcd

when I am running on a long bag file and it finishes, the process continues forever , and then when I do ctrl+c the program it starts to save the result but sometimes it fails , should I wait until the end of THE process? Is there any end for that ?

how to reconstruction after optimization and PGO.

I am test your code, and result is good.
so i want reconstruction this.
This code publish keyframes after optimization, but keyframe to keyframe distance are about 1m.
so i want every pose versus 1 lidar frame. how to this implementation?

Questions about bulild

Thanks for creating this great resource.
I have a question regarding build: the following error is displayed

CMake Error at /home/wang/ws_fast_lio_sam/src/FAST-LIO-SAM/fast_lio_sam/CMakeLists.txt:60 (add_executable):
Target "fast_lio_sam_node" links to target "Boost::timer" but the target
was not found. Perhaps a find_package() call is missing for an IMPORTED
target, or an ALIAS target is missing?

CMake Error at /home/wang/ws_fast_lio_sam/src/FAST-LIO-SAM/fast_lio_sam/CMakeLists.txt:55 (add_library):
Target "fast_lio_sam_methods" links to target "Boost::timer" but the target
was not found. Perhaps a find_package() call is missing for an IMPORTED
target, or an ALIAS target is missing?

CMake Error at /home/wang/ws_fast_lio_sam/src/FAST-LIO-SAM/fast_lio_sam/CMakeLists.txt:53 (add_library):
Target "fast_lio_sam_constructor" links to target "Boost::timer" but the
target was not found. Perhaps a find_package() call is missing for an
IMPORTED target, or an ALIAS target is missing?

CMake Error at /home/wang/ws_fast_lio_sam/src/FAST-LIO-SAM/fast_lio_sam/CMakeLists.txt:57 (add_library):
Target "fast_lio_sam_callbacks" links to target "Boost::timer" but the
target was not found. Perhaps a find_package() call is missing for an
IMPORTED target, or an ALIAS target is missing?

How can I solve these problems？

/corrected_odom topic published sensor_msgs::PointCloud2 instead of nav_msgs::Odometry

Dear Dr. Eungchang-Mason-Lee,

First and foremost, I want to express my gratitude for your incredible work on the FAST-LIO-SAM package. It's been an invaluable tool for my project, and I truly appreciate the effort you've put into it.

Now, I have a question regarding the package's behavior. I've been using this package and have noticed that the /corrected_odom topic is currently publishing messages of type sensor_msgs::PointCloud2. However, based on the package's functionality, I expected it to publish nav_msgs::Odometry messages instead.

1. Is there a specific reason why the /corrected_odom topic is publishing sensor_msgs::PointCloud2 messages instead of nav_msgs::Odometry as expected?
2. Is there a recommended modification or workaround to make /corrected_odom publish nav_msgs::Odometry messages?
   I appreciate your help in resolving this matter. Thank you for your support.

Question about keyframes

Thanks for creating this great resource.

I have a question regarding keyframes: You add keyframes after a certain threshold here

FAST-LIO-SAM/fast_lio_sam/src/methods.cpp

Lines 41 to 44 in dd52362

	bool FAST_LIO_SAM_CLASS::check_if_keyframe(const pose_pcd &pose_pcd_in, const pose_pcd &latest_pose_pcd)
	{
	return m_keyframe_thr < (latest_pose_pcd.pose_corrected_eig.block<3, 1>(0, 3) - pose_pcd_in.pose_corrected_eig.block<3, 1>(0, 3)).norm();
	}

and update the poses here:

FAST-LIO-SAM/fast_lio_sam/src/callbacks.cpp

Lines 184 to 189 in dd52362

	for (int i = 0; i < corrected_esti_copy_.size(); ++i)
	{
	gtsam::Pose3 pose_ = corrected_esti_copy_.at<gtsam::Pose3>(i);
	corrected_odoms_.points.emplace_back(pose_.translation().x(), pose_.translation().y(), pose_.translation().z());
	corrected_path_.poses.push_back(gtsam_pose_to_pose_stamped(pose_, m_map_frame));
	}

Yet, the visualization looks like every single pose has been added as an OdomFactor (like in i.e. LIO-SAM). How is every single pose corrected, when the pose graph only includes keyframes? Thanks for your help