/FAST-LIO-Localization-QN

A Map-based localization implementation combining FAST-LIO2 as an odometry with Quatro + Nano-GICP as a map matching method

Primary LanguageC++OtherNOASSERTION

FAST-LIO-Localization-QN

  • This repository is a map-based localization implementation combining FAST-LIO2 as an odometry with Quatro and Nano-GICP module as a map matching method
  • Note: similar repositories already exist
  • Note2: main code is modularized and hence can be combined with any other LIO / LO
  • Note3: this repo is to apply Quatro in localization. Quatro can be worked in scan-to-scan matching or submap-to-submap matching but not scan-to-submap, i.e., the numbers of pointclouds to be matched should be similar.
  • Note4: saved map file is needed. The map should be in .bag format. This .bag files can be built with FAST-LIO-SAM-QN and FAST-LIO-SAM

Dependencies

  • C++ >= 17, OpenMP >= 4.5, CMake >= 3.10.0, Eigen >= 3.2, Boost >= 1.54
  • ROS
  • 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-Localization-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
    roslaunch fast_lio_localization_qn run.launch lidar:=ouster
    roslaunch fast_lio_localization_qn run.launch lidar:=velodyne
    roslaunch fast_lio_localization_qn run.launch lidar:=livox
  • Change config files in
    • third_party/FAST_LIO/config
    • fast_lio_localization_qn/config

Structure

  • odom_pcd_cb
    • pub realtime pose in corrected frame
    • keyframe detection -> if keyframe, add to pose graph + save to keyframe queue
  • matching_timer_func
    • process a saved keyframe
      • detect map match -> if matched, correct TF
  • vis_timer_func
    • visualize all

Memo

  • Quatro module fixed for empty matches
  • Quatro module is updated with optimizedMatching which limits the number of correspondences and increased the speed