ROS2 Fork repo maintainer: Ericsiii

Related Works and Extended Application

SLAM:

1. ikd-Tree: A state-of-art dynamic KD-Tree for 3D kNN search.
2. R2LIVE: A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
3. LI_Init: A robust, real-time LiDAR-IMU extrinsic initialization and synchronization package..
4. FAST-LIO-LOCALIZATION: The integration of FAST-LIO with Re-localization function module.

Control and Plan:

1. IKFOM: A Toolbox for fast and high-precision on-manifold Kalman filter.
2. UAV Avoiding Dynamic Obstacles: One of the implementation of FAST-LIO in robot's planning.
3. UGV Demo: Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
4. Bubble Planner: Planning High-speed Smooth Quadrotor Trajectories using Receding Corridors.

FAST-LIO

FAST-LIO (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues:

1. Fast iterated Kalman filter for odometry optimization;
2. Automaticaly initialized at most steady environments;
3. Parallel KD-Tree Search to decrease the computation;

FAST-LIO 2.0 (2021-07-05 Update)

Related video: FAST-LIO2, FAST-LIO1

Pipeline:

New Features:

1. Incremental mapping using ikd-Tree, achieve faster speed and over 100Hz LiDAR rate.
2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
3. Since no requirements for feature extraction, FAST-LIO2 can support many types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs.
4. Support external IMU.
5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM).

Related papers:

FAST-LIO2: Fast Direct LiDAR-inertial Odometry

FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

Contributors

Wei Xu 徐威，Yixi Cai 蔡逸熙，Dongjiao He 贺东娇，Fangcheng Zhu 朱方程，Jiarong Lin 林家荣，Zheng Liu 刘政, Borong Yuan

1. Prerequisites

1.1 Ubuntu and ROS

Ubuntu >= 20.04

The default from apt PCL and Eigen is enough for FAST-LIO to work normally.

ROS >= Foxy (Recommend to use ROS-Humble). ROS Installation

1.2. PCL && Eigen

PCL >= 1.8, Follow PCL Installation.

Eigen >= 3.3.4, Follow Eigen Installation.

1.3. livox_ros_driver2 (optional)

Install livox_ros_driver2 only if you plan to run a Livox LiDAR.

You can also use the modified fork livox_ros_driver2.

Remarks:

• FAST-LIO will now build and run without livox_ros_driver2. When the driver is not present, Livox-specific topics are disabled and you should use standard sensor_msgs/PointCloud2 topics (e.g., Ouster or Velodyne drivers).
• If you do use Livox hardware, install and source the driver workspace (e.g., add source <ws_livox>/install/setup.bash to your shell rc file) and build FAST-LIO with Livox support enabled (pass -DFASTLIO_REQUIRE_LIVOX=ON to CMake if you want the build to fail when the driver is missing).

2. Build

Clone the repository and colcon build:

    cd <ros2_ws>/src # cd into a ros2 workspace folder
    git clone https://github.com/Ericsii/FAST_LIO_ROS2.git --recursive
    cd ..
    rosdep install --from-paths src --ignore-src -y
    colcon build --symlink-install
    . ./install/setup.bash # use setup.zsh if use zsh

Optional: build with CUDA acceleration (Jetson Orin / desktop GPUs)

1. Install the NVIDIA CUDA toolkit (JetPack already ships with it on Jetson; on desktop Ubuntu you can sudo apt install nvidia-cuda-toolkit or install from NVIDIA drivers).
2. Ensure nvcc is on your PATH or export it explicitly:

   export CUDACXX=/usr/local/cuda/bin/nvcc

3. Build the package with CUDA kernels enabled:

   colcon build --packages-select fast_lio --cmake-args -DFASTLIO_USE_CUDA=ON -DCMAKE_BUILD_TYPE=Release

If the CUDA toolkit is not available the build will stop with No CMAKE_CUDA_COMPILER could be found. Rebuild with FASTLIO_USE_CUDA=OFF (default) or install the toolkit before retrying.

• Livox users only: source the Livox workspace before building so the custom message is discoverable.
• To force-enable Livox message support (and catch misconfiguration early) configure with colcon build --cmake-args -DFASTLIO_REQUIRE_LIVOX=ON.
• If you want to use a custom build of PCL, add the following line to ~/.bashrc export PCL_ROOT={CUSTOM_PCL_PATH}

3. Directly run

Noted:

A. Please make sure the IMU and LiDAR are Synchronized, that's important.

B. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important for the forward propagation and backwark propagation.

C. We recommend to set the extrinsic_est_en to false if the extrinsic is give. As for the extrinsic initiallization, please refer to our recent work: Robust Real-time LiDAR-inertial Initialization.

3.1 Run use ros launch

Connect to your PC to Livox LiDAR by following Livox-ros-driver2 installation, then

cd <ros2_ws>
. install/setup.bash # use setup.zsh if use zsh
ros2 launch fast_lio mapping.launch.py config_file:=avia.yaml

Change config_file parameter to other yaml file under config directory as you need.

Ouster users: use config/ouster64.yaml (or adapt it) and run your Ouster driver (e.g., ros2 launch ouster_ros sensor.launch). FAST-LIO will subscribe to the standard /os_cloud_node/points and /os_cloud_node/imu topics; no Livox SDK is required.

3.1.1 Validate your custom Ouster profile

Pick whichever YAML under config/ matches your sensor (e.g., ouster32.yaml, ouster64.yaml, or your fork). To confirm it converges on your robot:

1. Launch your sensor driver with synchronized /ouster/points and /ouster/imu topics and per-point timestamps enabled.
2. Start FAST-LIO with your chosen config:

   ros2 launch fast_lio mapping.launch.py config_file:=<your_ouster_config>.yaml

3. Watch RViz /Odometry and the TF tree. Ensure the pose stabilizes within the first few scans; if not, revisit the extrinsic_T/R block in the same YAML.
4. Check IMU gating by plotting /imu/filtered (or your IMU topic) variance. Tune acc_cov / gyr_cov when spikes cause the EKF to stall.
5. Monitor solver health via ros2 topic hz /Odometry and the node log—persistent NOT_CONVERGED warnings normally indicate parameter mismatches.
6. Record a short rosbag (ros2 bag record /Odometry /ouster/imu /ouster/points) so you can replay while iterating on parameters.
7. Optional map save: call /map_save after a stable loop to export a .pcd map for offline inspection.

If convergence is still poor:

• Re-run LI-Init (or similar) to re-estimate IMU-LiDAR extrinsics and copy them into extrinsic_T/R while keeping extrinsic_est_en: false.
• Adjust voxel sizes (filter_size_map, filter_size_surf_min) to match your environment density; smaller leaf sizes preserve more structure indoors.
• On highly dynamic platforms lower max_iteration to keep latency bounded and rely on faster scan turnaround.

Frame IDs & TF integration

FAST-LIO internally works in a simple map -> odom -> body frame chain. By default those IDs are map, camera_init, and body, which works for the original datasets but often does not match a robot's base_link / odom frames. Every config file can now override the TF frame names:

frames:
    map_frame: "map"        # parent of the published odometry
    odom_frame: "odom"      # frame used in /Odometry, point clouds, and tf
    body_frame: "base_link" # child frame that should coincide with the IMU/body

Set body_frame to the robot's base_link (or IMU frame) to remove RViz warnings such as “No transform from [base_link] to [camera_init]”. If you keep the default names, FAST-LIO will continue to behave exactly as before.

Launch livox ros driver. Use MID360 as an example.

ros2 launch livox_ros_driver2 msg_MID360_launch.py

• For livox serials, FAST-LIO only support the data collected by the livox_lidar_msg.launch since only its livox_ros_driver2/CustomMsg data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. livox_lidar.launch can not produce it right now.
• If you want to change the frame rate, please modify the publish_freq parameter in the livox_lidar_msg.launch of Livox-ros-driver before make the livox_ros_driver pakage.

3.2 For Livox serials with external IMU

mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:

Edit config/avia.yaml to set the below parameters:

1. LiDAR point cloud topic name: lid_topic
2. IMU topic name: imu_topic
3. Translational extrinsic: extrinsic_T
4. Rotational extrinsic: extrinsic_R (only support rotation matrix)

• The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual.
• FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter time_sync_en to ture to turn on. But turn on ONLY IF external time synchronization is really not possible, since the software time sync cannot make sure accuracy.

3.4 PCD file save

1. Enable pcd_save.pcd_save_en in the config file and set the map_file_path to the path where the map will be saved.
2. Launch the fastlio2 according to README.
3. Open RQt and switch to Plugins->Services->Service Caller. Trigger the service /map_save, then the pcd map file will be generated

pcl_viewer scans.pcd can visualize the point clouds.

Tips for pcl_viewer:

• change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running.

    1 is all random
    2 is X values
    3 is Y values
    4 is Z values
    5 is intensity

4. Rosbag Example

4.1 Livox Avia Rosbag

Files: Can be downloaded from google drive!!!This ros1 bag should be convert to ros2!!!

Run:

ros2 launch fast_lio mapping.launch.py config_path:=<path_to_your_config_file>
ros2 bag play <your_bag_dir>

4.2 Velodyne HDL-32E Rosbag

NCLT Dataset: Original bin file can be found here.

We produce Rosbag Files and a python script to generate Rosbag files: python3 sensordata_to_rosbag_fastlio.py bin_file_dir bag_name.bag!!!This ros1 bag should be convert to ros2!!! To convert ros1 bag to ros2 bag, please follow the documentation Convert rosbag versions

Run:

roslaunch fast_lio mapping_velodyne.launch
rosbag play YOUR_DOWNLOADED.bag

5.Implementation on UAV

In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below.

The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future.

6.Acknowledgments

Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), Livox_Mapping, LINS and Loam_Livox.