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
- Quatro - fast, accurate and robust global registration which provides great initial guess of transform
- Quatro module - Quatro as a module, can be easily used in other packages
- Nano-GICP module - fast ICP combining FastGICP + NanoFLANN
Note: similar repositories already exist
- FAST_LIO_LOCALIZATION: FAST-LIO2 + Open3D's ICP
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

vzhr/FAST-LIO-Localization-QN

FAST-LIO-Localization-QN

Dependencies

How to build and use

Structure

Memo