ROS2 Fork repo maintainer: Ericsiii
Related Works and Extended Application
SLAM:
- ikd-Tree: A state-of-art dynamic KD-Tree for 3D kNN search.
- R2LIVE: A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
- LI_Init: A robust, real-time LiDAR-IMU extrinsic initialization and synchronization package..
- FAST-LIO-LOCALIZATION: The integration of FAST-LIO with Re-localization function module.
Control and Plan:
- IKFOM: A Toolbox for fast and high-precision on-manifold Kalman filter.
- UAV Avoiding Dynamic Obstacles: One of the implementation of FAST-LIO in robot's planning.
- UGV Demo: Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
- 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:
- Fast iterated Kalman filter for odometry optimization;
- Automaticaly initialized at most steady environments;
- 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:
- Incremental mapping using ikd-Tree, achieve faster speed and over 100Hz LiDAR rate.
- Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
- 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.
- Support external IMU.
- 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
Follow livox_ros_driver2 Installation.
You can also use the one I modified livox_ros_driver2
Remarks:
- Since the FAST-LIO must support Livox serials LiDAR firstly, so the livox_ros_driver must be installed and sourced before run any FAST-LIO launch file.
- How to source? The easiest way is add the line
source $Licox_ros_driver_dir$/devel/setup.bashto the end of file~/.bashrc, where$Licox_ros_driver_dir$is the directory of the livox ros driver workspace (should be thews_livoxdirectory if you completely followed the livox official document).
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.git --recursive
cd ..
rosdep install --from-paths src --ignore-src -y
colcon build --symlink-install
. ./install/setup.bash # use setup.zsh if use zsh- Remember to source the livox_ros_driver before build (follow 1.3 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}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.yamlChange config_file parameter to other yaml file under config directory as you need.
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.launchsince only itslivox_ros_driver2/CustomMsgdata structure produces the timestamp of each LiDAR point which is very important for the motion undistortion.livox_lidar.launchcan 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:
- LiDAR point cloud topic name:
lid_topic - IMU topic name:
imu_topic - Translational extrinsic:
extrinsic_T - 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_ento 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
Set pcd_save_enable in launchfile to 1. All the scans (in global frame) will be accumulated and saved to the file FAST_LIO/PCD/scans.pcd after the FAST-LIO is terminated. 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](https://drive.google.com/drive/folders/1CGYEJ9-wWjr8INyan6q1BZz_5VtGB-fP?usp=sharing)**!!!This ros1 bag should be convert to ros2!!!**
Run:
```bash
ros2 launch fast_lio mapping.launch.py config_path:=
