FAST-LIO-LOCALIZATION2

A simple localization framework that can re-localize in built maps based on FAST-LIO-ROS2.

1. Features

• Realtime 3D global localization in a pre-built point cloud map. By fusing low-frequency global localization (about 0.5~0.2Hz), and high-frequency odometry from FAST-LIO, the entire system is computationally efficient.

• Eliminate the accumulative error of the odometry.

• The initial localization can be provided either by rough manual estimation from RVIZ2, or pose from another sensor/algorithm.

2. Prerequisites

2.1 Dependencies for FAST-LIO-ROS2

Technically, if you have built and run FAST-LIO-ROS2 before, you may skip section 2.1.

This part of dependency is consistent with FAST-LIO-ROS2, please refer to the documentation https://github.com/Ericsii/FAST_LIO_ROS2

2.2 Dependencies for localization module

• python 3.8

• tf_transformations

pip install tf_transformations

• ros2_numpy

pip install ros2-numpy

check your numpy version, must be <1.24

• Open3D

pip install open3d

• pcl_ros

sudo apt-get install ros-$distro-pcl-ros

• To resolve numpy float issue - replace 'np.float' with 'np.float64' or just 'float' in the following file

cd /usr/lib/python3/dist-packages/transforms3d/
sudo nano quaternions.py
# replace 'np.float' with 'np.float64' or just 'float'

3. Build

Clone the repository and colcon build:

    cd ~/$A_ROS_DIR$/src
    git clone https://github.com/Smart-Wheelchair-RRC/FAST_LIO_LOCALIZATION2.git
    cd FAST_LIO_LOCALIZATION2
    git submodule update --init
    cd ../..
    colcon build --symlink-install
    source install/setup.bash

• Remember to source the livox_ros_driver before build (follow livox_ros_driver)
• If you want to use a custom build of PCL, add the following line to ~/.bashrc export PCL_ROOT={CUSTOM_PCL_PATH}

4. Run Localization

4.2 Run

1. Run localization, here we take Livox Mid360 as an example:

ros2 launch fast_lio_localization localization.launch.py pcd_map_topic:=cloud_pcd map:=/path/to/your/map.pcd

Please modify /path/to/your/map.pcd to your own map point cloud file path (downsample it for fast rviz2 visualisation, we noticed a performance issue with dense pointclouds).

Wait for 3~5 seconds until the map cloud shows up in RVIZ;

Or if you are running realtime

roslaunch livox_ros_driver livox_lidar_msg.launch

Please set the publish_freq in livox_lidar_rviz.launch to 10Hz, to ensure there are enough points for global localization in a single scan. Support for higher frequency is coming soon.

2. Provide initial pose Use Rviz2 to provide an initial pose by using the '2D Pose Estimate' Tool in RVIZ2.

Note that, during the initialization stage, it's better to keep the robot still until the initialization succeeds.

Related Works

1. FAST-LIO: A computationally efficient and robust LiDAR-inertial odometry (LIO) package
2. ikd-Tree: A state-of-art dynamic KD-Tree for 3D kNN search.
3. FAST-LIO-SLAM: The integration of FAST-LIO with Scan-Context loop closure module.
4. LIO-SAM_based_relocalization: A simple system that can relocalize a robot on a built map based on LIO-SAM.

Acknowledgments

Thanks for the authors of FAST-LIO and LIO-SAM_based_relocalization. This package is build on top of the work done by the ROS1 package of Fast-Lio-Localization - https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION