/FAST-LIO-SAM-SC-QN

A SLAM implementation combining FAST-LIO2 with pose graph optimization and loop closing based on ScanContext, Quatro, and Nano-GICP

Primary LanguageC++OtherNOASSERTION

FAST-LIO-SAM-SC-QN


Main difference between FAST_LIO_SAM_QN and FAST_LIO_SAM_SC_QN

  • FAST_LIO_SAM_QN sets loop candidate pair as (current keyframe, the closest and old enough keyframe)
  • FAST_LIO_SAM_SC_QN gets loop candidate pair from ScanContext

Dependencies

  • C++ >= 17, OpenMP >= 4.5, CMake >= 3.10.0, Eigen >= 3.2, Boost >= 1.54
  • ROS
  • GTSAM >= 4.1.1
    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
  • Teaser++
    git clone https://github.com/MIT-SPARK/TEASER-plusplus.git
    cd TEASER-plusplus && mkdir build && cd build
    cmake .. -DENABLE_DIAGNOSTIC_PRINT=OFF
    sudo make install -j16
    sudo ldconfig
  • tbb (is used for faster Quatro)
    sudo apt install libtbb-dev

How to build and use

  • Get the code and then build the main code.
    cd ~/your_workspace/src
    git clone https://github.com/engcang/FAST-LIO-SAM-SC-QN --recursive
    
    cd ~/your_workspace
    # nano_gicp, quatro first
    catkin build nano_gicp -DCMAKE_BUILD_TYPE=Release
    
    # Note the option!
    catkin build quatro -DCMAKE_BUILD_TYPE=Release -DQUATRO_TBB=ON
    
    catkin build -DCMAKE_BUILD_TYPE=Release
    . devel/setup.bash
  • Then run (change config files in third_party/FAST_LIO)
    roslaunch fast_lio_sam_sc_qn run.launch lidar:=ouster
    roslaunch fast_lio_sam_sc_qn run.launch lidar:=velodyne
    roslaunch fast_lio_sam_sc_qn 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 + generate ScanContext
    • 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.)