FAST-LIO 开源仓库：hku-mars/FAST_LIO

FAST-LIO 是港大MaRS实验室在2021年提出的一个紧耦合迭代扩展卡尔曼滤波高计算效率、高鲁棒性的雷达里程计，使用了紧密耦合的迭代扩展卡尔曼滤波器将 Li DAR 特征点与 IMU 数据融合，以便在发生退化的快速运动、嘈杂或混乱环境中实现稳健导航，解决了许多关键问题：

  1. 用于里程计优化的快速迭代卡尔曼滤波器
  2. 将IMU和Lidar特征点紧耦合在一起
  3. 使用反向传播考虑到了运动补偿
  4. 使用并行 KD 树搜索以减少计算

image.png

这里更多着眼于实现运行和观察算法效果，具体步骤参考了仓库Readme，内容如下。

环境要求

Ubuntu >= 16.04

ROS >= Melodic、PCL >= 1.8、Eigen >= 3.3.4

livox_ros_driver 安装配置

教程地址：github.com/Livox-SDK/livox_ros_driver/README

由于 FAST-LIO 必须首先支持 Livox 系列 LiDAR，因此在运行任何 FAST-LIO luanch 文件之前必须安装并获取livox_ros_driver，览沃ROS驱动程序是一个全新的 ROS 包，需要先配置 ROS 和 Livox-SDK：

1
2
3
4
5
6
7
8
git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src

# 构建览沃 ROS 驱动程序
cd ws_livox
catkin_make

# 更新ROS包环境
source ./devel/setup.sh

构建运行

有两种方法，一种是使用 Docker 容器构建，另一种是从源代码构建，这里采用第二种：

克隆存储库和catkin_make​，对于catkin_make​ ，可以提高虚拟机内存大小来缩短时间：

1
2
3
4
5
6
7
8
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

注意点一：

需要把submodule下到虚拟机中，而简单地下载zip再解压是不行的，仓库clone到本地再执行git submodule update --init​。

注意点二：

其中catkin_make​可能会遇到的另一个问题是：fatal error: fast_lio/Pose6D.h: No such file or directory​，可以在原仓库的Issue中找到解决方案：Pose6D.h problem during catkin_make · Issue #138 · hku-mars/FAST_LIO

将pose6D.h​下载到devel/include/fast_lio​中即可，最好多加点虚拟机内存，还可以加上-j1来加速处理。

注意点三：

这里最后的source devel/setup.bash​是需要每次运行FAST-LIO的任何launch文件前，都需要激活的环境。所以，最简单的方法是添加source $Livox_ros_driver_dir$/devel/setup.bash​到~/.bashrc文件末尾​，其中$Livox_ros_driver_dir$​是 livox ros 驱动程序工作区的目录。

之后就可以直接使用仓库默认的配置文件mapping_avia.launch​来运行，参考仓库readme，开启两个终端来跑即可，具体参数也可以在rviz中再调整，比如调整显示风格等。

结果分析

使用Rosbag数据集测试算法效果：测试集可以在 https://drive.google.com/drive/folders/1CGYEJ9-wWjr8INyan6q1BZz_5VtGB-fP​ 中取得。

具体操作：将数据集放入 dataset 目录，然后对该数据集应用FAST-LIO算法测试，这里将以下的YOUR_DOWNLOADED.bag​替换为具体数据集，比如src/fast_lio/datasets/HKU_MB_2020-09-20-13-34-51.bag​。

1
2
roslaunch fast_lio mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag

激光建图的过程如下：

image.png
image.png

最后成果图如下，旋转视角可以看见后面的城市建筑群：

image.png

运行测试集可以发现，FAST-LIO算法实现了高精度的定位和建图，利用了LiDAR点云数据和IMU数据的融合，有效减少累积误差，说明在复杂场景下也能保持较高的鲁棒性和稳定性。

而且可以看到算法的性能也足够优秀，即使是1G多的数据集，在内存分配不多、计算资源有限的情况下，还可以快速处理大规模点云数据，这也说明了其适用于实时应用场景，比如自动驾驶等。

参考文献

  1. Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 3317-3324.
  2. Fast-lio2: Fast direct lidar-inertial odometry[J]. IEEE Transactions on Robotics, 2022, 38(4): 2053-2073.
  3. Faster-LIO: Lightweight tightly coupled LiDAR-inertial odometry using parallel sparse incremental voxels[J]. IEEE Robotics and Automation Letters, 2022, 7(2): 4861-4868.
  4. https://github.com/hku-mars/FAST_LIO
  5. FAST-LIO论文阅读精讲：https://blog.csdn.net/wxc_1998/article/details/130909635
  6. 进一步的：激光SLAM：Faster-Lio 算法编译与测试