180 lines
8.9 KiB
Markdown
180 lines
8.9 KiB
Markdown
## Related Works
|
||
|
||
1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search.
|
||
2. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter.
|
||
3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.
|
||
4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
|
||
5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
|
||
|
||
## 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)
|
||
<!--  -->
|
||
<!-- [](https://youtu.be/2OvjGnxszf8) -->
|
||
<div align="left">
|
||
<img src="doc/real_experiment2.gif" width=49% />
|
||
<img src="doc/ulhkwh_fastlio.gif" width = 49% >
|
||
</div>
|
||
|
||
**Related video:**
|
||
|
||
[FAST-LIO2](https://youtu.be/2OvjGnxszf8)
|
||
|
||
[FAST-LIO1](https://youtu.be/iYCY6T79oNU)
|
||
|
||
**New features:**
|
||
1. Incremental mapping using ikd-Tree, achieve faster speed and over 100Hz LiDAR rate.
|
||
2. Direct odometry on Raw LiDAR points (feature extraction can be closed), achieving better accuracy.
|
||
3. Since no need for feature extraction, FAST-LIO2 can support different LiDAR Types including spinning (Velodyne, Ouster) and solid-state (Avia, horizon) LiDARs, and can be easily extended to support more LiDAR.
|
||
4. Support external IMU.
|
||
5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry 4B with 8G RAM.
|
||
|
||
|
||
**Contributors**
|
||
|
||
[Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan)
|
||
|
||
**Related papers**:
|
||
|
||
[FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf)
|
||
|
||
[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196)
|
||
|
||
<!-- <div align="center">
|
||
<img src="doc/results/HKU_HW.png" width = 49% >
|
||
<img src="doc/results/HKU_MB_001.png" width = 49% >
|
||
</div> -->
|
||
|
||
## 1. Prerequisites
|
||
### 1.1 **Ubuntu** and **ROS**
|
||
**Ubuntu >= 16.04**
|
||
|
||
For **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally.
|
||
|
||
ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
|
||
|
||
### 1.2. **PCL && Eigen && openCV**
|
||
PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).
|
||
|
||
Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
|
||
|
||
### 1.3. **livox_ros_driver**
|
||
Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).
|
||
|
||
*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 luanch file.
|
||
- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
|
||
|
||
|
||
## 2. Build
|
||
Clone the repository and catkin_make:
|
||
|
||
```
|
||
cd ~/$A_ROS_DIR$/src
|
||
git clone https://github.com/hku-mars/FAST_LIO.git
|
||
cd FAST_LIO
|
||
git submodule update --init
|
||
cd ../..
|
||
catkin_make
|
||
source devel/setup.bash
|
||
```
|
||
- 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
|
||
|
||
Please make sure the IMU and LiDAR are **Synchronized**, that's important.
|
||
### 3.1 For Avia
|
||
Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
|
||
```
|
||
cd ~/$FAST_LIO_ROS_DIR$
|
||
source devel/setup.bash
|
||
roslaunch fast_lio mapping_avia.launch
|
||
roslaunch livox_ros_driver livox_lidar_msg.launch
|
||
```
|
||
- For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/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](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/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.
|
||
|
||
### 3.3 For Velodyne or Ouster (Velodyne as an example)
|
||
|
||
Step A: Setup before run
|
||
|
||
Edit ``` config/velodyne.yaml ``` to set the below parameters:
|
||
|
||
1. LiDAR point cloud topic name: ``` lid_topic ```
|
||
2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
|
||
3. Line number (we tested 16 and 32 line, but not tested 64 or above): ``` scan_line ```
|
||
4. Translational extrinsic: ``` extrinsic_T ```
|
||
5. 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).
|
||
|
||
Step B: Run below
|
||
```
|
||
cd ~/$FAST_LIO_ROS_DIR$
|
||
source devel/setup.bash
|
||
roslaunch fast_lio mapping_velodyne.launch
|
||
```
|
||
|
||
Step C: Run LiDAR's ros driver or play rosbag.
|
||
|
||
*Remarks:*
|
||
- We will produce some velodyne datasets which is already transfered to Rosbags, please wait for a while.
|
||
|
||
### 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.
|
||
|
||
## 4. Rosbag Example
|
||
### 4.1 Indoor rosbag (Livox Avia LiDAR)
|
||
|
||
<div align="center"><img src="doc/results/HKU_LG_Indoor.png" width=100% /></div>
|
||
|
||
Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWmrwlUD5FlyA-bTr1rakIYx1GxS4xNl/view?usp=sharing) or [avia_indoor_quick_shake_example2](https://drive.google.com/file/d/1wD485CIbzZlNs4z8e20Dv2Q1q-7Gv_AT/view?usp=sharing) and then
|
||
```
|
||
roslaunch fast_lio mapping_avia.launch
|
||
rosbag play YOUR_DOWNLOADED.bag
|
||
|
||
```
|
||
|
||
### 4.2 Outdoor rosbag (Livox Avia LiDAR)
|
||
|
||
<div align="center"><img src="doc/results/HKU_MB_002.png" width=100% /></div>
|
||
|
||
<!-- <div align="center"><img src="doc/results/mid40_outdoor.png" width=90% /></div> -->
|
||
|
||
Download [avia_hku_main building_mapping](https://drive.google.com/file/d/1GSb9eLQuwqmgI3VWSB5ApEUhOCFG_Sv5/view?usp=sharing) and then
|
||
```
|
||
roslaunch fast_lio mapping_avia.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.
|
||
|
||
<div align="center">
|
||
<img src="doc/uav01.jpg" width=40.5% >
|
||
<img src="doc/uav_system.png" width=57% >
|
||
</div>
|
||
|
||
## 6.Acknowledgments
|
||
|
||
Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox).
|