FAST_LIO 与 FAST_LIO2 算法原理及 ROS2 环境复现
算法原理简介
FAST_LIO 提出了一种计算高效且鲁棒的激光雷达惯性里程计（LIO）框架。它采用紧耦合的迭代扩展卡尔曼滤波器，将激光雷达特征点与 IMU 数据进行融合，从而在快速运动、噪声较大或环境退化的场景下实现稳健导航。
FAST_LIO2 在此基础上进行了优化，构建了一个快速、强大且多功能的 LIO 框架。相比 FAST_LIO，其主要改进在于：
- 去掉了特征提取模块，直接使用原始点云进行匹配。
- 引入 ikd-tree 数据结构来存储和管理点云，提升了搜索效率。
运行环境配置
本文基于 Ubuntu 22.04 和 ROS2 Humble 版本进行演示。为了确保环境隔离性，推荐使用 Docker 容器。
创建 Docker 容器
docker run -it -p 6080:80 -p 10000:10000 \
-v /tmp/.X11-unix:/tmp/.X11-unix \
-e DISPLAY=host.docker.internal:0.0 \
-e PULSE_SERVER=host.docker.internal \
--name=DockerUnityRos2Car4 \
docker.1ms.run/ubuntu:22.04 /bin/bash
安装 ROS2
进入容器后，首先更新源并安装基础工具：
sudo apt update
sudo apt-get install sudo -y
sudo apt-get install software-properties-common -y
sudo add-apt-repository universe
接着添加 ROS2 官方密钥源（需确保网络连接正常）：
sudo apt-get install curl gnupg2 -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update && sudo apt upgrade -y
sudo apt-get install ros-humble-desktop -y
配置环境变量，避免每次手动 source：
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc
安装第三方依赖库
我们需要安装一些核心数学库和驱动支持：
# 通用工具
sudo apt-get install python3-colcon-common-extensions -y
sudo apt-get install ros-humble-rqt-tf-tree -y
sudo apt-get install gdb -y
sudo apt-get install ros-${ROS_DISTRO}-pcl-ros -y
Livox SDK
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd third_party_libraries/Livox-SDK2
mkdir build && cd build
cmake .. && make -j
sudo make install
Sophus
git clone https://github.com/strasdat/Sophus.git
cd third_party_libraries/Sophus
git checkout 1.22.10
mkdir build && cd build
cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON
make
sudo make install
GTSAM
git clone https://github.com/borglab/gtsam.git
cd third_party_libraries/gtsam
mkdir build && cd build
cmake ..
sudo make install
编译项目
首次编译需要初始化驱动：
cd 2_replicate_fast_lio_two_version/src/livox_ros_driver2
chmod +x build.sh
./build.sh humble
后续编译使用 colcon：
cd 2_replicate_fast_lio_two_version/
colcon build
运行测试
数据集准备
请提前准备好对应的 LiDAR 与 IMU 同步的 rosbag 数据文件，并将其放置于工作目录下的 src/docs/place_data_here/ 路径中。
FAST_LIO 算法运行
启动 bag 包并加载映射配置：
cd 2_replicate_fast_lio_two_version
source install/setup.bash
ros2 bag play /root/2_replicate_fast_lio_two_version/src/docs/place_data_here/rosbag2_2024_06_20-16_46_47
在另一个终端执行 launch 命令：
cd 2_replicate_fast_lio_two_version
source install/setup.bash
ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml
FAST_LIO2 算法运行
同样先播放数据：
cd 2_replicate_fast_lio_two_version
source install/setup.bash
ros2 bag play /root/2_replicate_fast_lio_two_version/src/docs/place_data_here/rosbag2_2024_06_20-16_46_47
然后启动 FastLio2 节点：
cd 2_replicate_fast_lio_two_version
source install/setup.bash
ros2 launch fastlio2 lio_launch.py
参考资料
- FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter (arXiv)
- Ericsii/FAST_LIO_ROS2: ROS2 version of FAST_LIO2 (GitHub)