樱花客栈

FAST-LIVO（docker）

作者：

大嘴巴小牙齿

在

一、FAST-LIVO是什么？

FAST-LIVO（Fast LiDAR-Inertial-Visual Odometry）是香港大学MARS实验室提出的激光-惯性-视觉紧耦合里程计算法。它的核心目标是用激光雷达、IMU和相机三种传感器的数据，实时估计机器人在三维空间中的位姿，并拼接出全局一致的点云地图。

FAST-LIVO发表于IEEE T-RO 2024（论文: FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry），是FAST-LIO的视觉增强版。相比FAST-LIO只使用LiDAR + IMU，FAST-LIVO增加了相机通道，能够在激光退化场景（长走廊、隧道、空旷大厅等）下依靠视觉特征继续稳定建图。

FAST-LIVO的核心改进包括：

  • 视觉通道：增加相机传感器，提供视觉重投影约束，在激光退化时补充纹理信息
  • VoxelOctoTree：地图存储升级为八叉树体素地图，内存更紧凑，查询更高效
  • 退化场景检测：自动检测几何特征单一的环境，动态调整激光和视觉的权重
  • 曝光补偿：在线估计相机曝光时间，提升视觉特征提取的鲁棒性
  • 多雷达支持：Livox AVIA / Mid-360 / Mid-70 / Velodyne / Hesai / RoboSense等

二、算法原理详解

2.1整体架构概览

FAST-LIVO的数据流可以概括为：

LiDAR点云 ──┐
              ├── 点云预处理 ── 特征提取 ──┐
IMU数据 ─────┤                           ├── ESKF更新 ── 位姿估计 ── 地图更新
              ├── IMU预积分 ────────────┘
相机图像 ─────┴── 视觉特征提取 ─────────┘

整个系统运行在一个ROS节点中（fastlio_mapping），订阅LiDAR、IMU和相机话题，发布里程计和地图。系统支持三种工作模式：

  • LIVO模式：LiDAR + IMU + Camera三传感器融合（默认模式）
  • LIO模式：仅LiDAR + IMU（相机数据不可用时自动切换）
  • LO模式：仅LiDAR（IMU和相机都不可用时）

2.2三传感器紧耦合框架

FAST-LIVO的核心创新是将LiDAR、IMU和Camera三个传感器的数据在误差状态卡尔曼滤波（ESKF）层面进行紧耦合融合。每个传感器提供不同类型的观测约束：

  • IMU：提供高频的姿态和速度预测（200-1000Hz），短时间内的相对运动约束
  • LiDAR：提供精确的几何相对位姿约束，对平移和旋转都有良好观测
  • Camera：提供视觉重投影约束，在几何退化场景下补充纹理信息

在ESKF的更新步骤中，FAST-LIVO根据当前环境动态调整各传感器的权重。当激光点云正常时，主要依赖LiDAR观测；当检测到激光退化时，自动增加视觉观测的权重。这种自适应机制使得系统在多种环境下都能保持鲁棒性。

FAST-LIVO的状态向量在FAST-LIO的基础上增加了视觉相关状态：

x = [p, v, R, b_a, b_g, g, T_cam_imu, inv_expo]
    p: 位置 (3D)
    v: 速度 (3D)
    R: 旋转 (四元数)
    b_a: 加速度计偏置 (3D)
    b_g: 陀螺仪偏置 (3D)
    g: 重力向量 (3D)
    T_cam_imu: 相机到IMU的外参 (6D)
    inv_expo: 逆曝光时间 (1D)

2.3视觉特征融合

FAST-LIVO的视觉通道采用基于稀疏直接法的轻量级方案。与传统的特征点匹配不同，FAST-LIVO使用光度误差（视觉重投影误差）作为观测残差，直接融合到ESKF中。

视觉处理的具体流程：

  • 特征提取：将图像划分为网格（grid），在每个网格中提取最亮的角点作为特征点
  • 特征跟踪：通过逆合成光流法（Inverse Compositional Optical Flow）在连续帧之间跟踪特征点
  • 重投影：将地图中的3D特征点投影到当前图像平面，计算光度误差（像素灰度差）
  • ESKF更新：将光度误差作为观测残差，更新状态估计

FAST-LIVO的视觉特征提取策略非常高效。它不需要对每帧图像进行完整的特征提取，而是只在需要时从体素地图中检索视觉特征点。这大大降低了计算开销，使得视觉通道可以在普通CPU上实时运行。

视觉通道的关键参数：

visual:
    enable: true                    # 是否启用视觉通道

image_topic: "/camera/image_raw" # 相机话题名

max_iterations: 5               # 视觉ESKF最大迭代次数
img_point_cov: 100              # 视觉点协方差

normal_en: true                 # 是否启用法向量约束

raycast_en: false               # 是否启用光线投射

exposure_estimate_en: true      # 是否在线估计曝光时间

grid_size: 5                    # 特征提取网格大小

grid_n_height: 17               # 特征提取网格高度

patch_pyrimid_level: 3          # 图像金字塔层数

patch_size: 8                   # 图像补丁大小

outlier_threshold: 1000         # 外点阈值

2.4退化场景检测

激光SLAM在退化场景中容易失败。典型的退化场景包括：

  • 长直走廊：点云在走廊方向缺乏几何多样性，无法约束横向平移
  • 隧道：与走廊类似，且通常没有视觉纹理
  • 空旷大平层：大面积平面，缺乏角点和平面特征的组合
  • 重复结构：多个相似的环境，容易产生误匹配

FAST-LIVO通过分析点云的特征值来判断是否退化。具体方法：

  • 对当前帧点云的协方差矩阵进行特征值分解
  • 如果最小特征值小于阈值，说明当前环境在某个方向缺乏几何约束
  • 系统会自动降低该方向的观测权重，同时增加视觉通道的权重

退化检测的阈值可以通过参数调整。阈值设置过小会导致退化检测不敏感，阈值设置过大则可能误判正常场景为退化。

2.5 VoxelOctoTree八叉树体素地图

FAST-LIVO使用VoxelOctoTree（八叉树体素地图）作为全局地图的数据结构。八叉树是一种空间划分数据结构，它将三维空间递归地划分为8个子立方体，每个节点有8个子节点。

八叉树的工作原理：

  • 根节点代表整个三维空间
  • 每个节点如果包含太多点，就分裂为8个子节点
  • 每个子节点代表父节点空间的1/8
  • 当节点内的点数低于阈值时，可以合并子节点

相比FAST-LIO的ikd-Tree，VoxelOctoTree的优势：

  • 内存更紧凑：不需要存储分割平面信息，只需要存储体素中心点和子节点指针
  • 查询更快：层次化的结构使得近邻搜索更高效，时间复杂度接近O(log n)
  • 动态更新更灵活：可以方便地增删体素节点，不需要重新平衡树结构
  • 空间划分更均匀：八叉树的规则划分避免了k-d树在数据分布不均时的性能退化

FAST-LIVO的VoxelOctoTree还维护了每个体素的平面信息（法向量和协方差），用于激光点云的特征匹配和视觉特征的重投影。地图的降采样分辨率由voxel_size参数控制（通常0.5m）。

2.6曝光补偿

FAST-LIVO的一个独特设计是在线曝光补偿。相机在自动曝光模式下，曝光时间会随环境光变化而变化，这会影响视觉特征提取的稳定性。FAST-LIVO将逆曝光时间（inverse exposure time）加入状态向量，在ESKF中在线估计：

  • 当环境光变暗时，自动增加曝光时间，保持图像亮度稳定
  • 当环境光变亮时，自动减少曝光时间，避免过曝
  • 曝光估计的协方差由inv_expo_cov参数控制

曝光补偿使得FAST-LIVO在光照变化较大的环境中（如室内外切换、隧道出入口）也能保持稳定的视觉特征跟踪。

三、ROS使用指南

3.1话题与数据流

FAST-LIVO相比FAST-LIO增加了相机话题：

话题名消息类型方向说明
/livox/lidarlivox_ros_driver2/CustomMsg订阅Livox雷达原始数据
/livox/imusensor_msgs/Imu订阅Livox IMU数据
/velodyne_pointssensor_msgs/PointCloud2订阅Velodyne雷达数据
/imu/datasensor_msgs/Imu订阅外部IMU数据
/camera/image_rawsensor_msgs/Image订阅相机图像数据
/camera/camera_infosensor_msgs/CameraInfo订阅相机内参数据
/Odometrynav_msgs/Odometry发布里程计（高频位姿）
/cloud_registeredsensor_msgs/PointCloud2发布注册到世界坐标系的点云
/cloud_effectedsensor_msgs/PointCloud2发布有效约束点云
/pathnav_msgs/Path发布运动轨迹
/Laser_mapsensor_msgs/PointCloud2发布局部地图点云
/rgb_imgsensor_msgs/Image发布RGB图像（用于可视化）

3.2 Launch文件与参数配置

FAST-LIVO使用ROS的launch文件启动。不同雷达型号对应不同的launch文件：

Launch文件适用雷达说明
mapping_avia.launchLivox AVIALivox非重复扫描雷达
mapping_velodyne.launchVelodyne 16/32/64Velodyne机械旋转雷达
mapping_mid360.launchLivox Mid-360Livox中距雷达
mapping_mid70.launchLivox Mid-70Livox短距雷达

3.3参数配置详解

FAST-LIVO的参数文件相比FAST-LIO增加了视觉相关配置。以下是完整的参数说明：

common:
lid_topic:  "/livox/lidar"      # LiDAR话题名
imu_topic:  "/livox/imu"        # IMU话题名
img_topic:  "/camera/image_raw"  # 相机话题名
img_en: 1                       # 是否启用相机: 1=启用, 0=禁用
lidar_en: 1                     # 是否启用LiDAR: 1=启用, 0=禁用
preprocess:
    lidar_type: 1                   # 雷达类型: 1=AVIA, 2=Velodyne
    scan_line: 6                    # 扫描线数: AVIA=6, Velodyne=16/32/64
    blind: 0.5                      # 盲区距离 (m)

vio:
    normal_en: true                 # 是否启用法向量约束

inverse_composition_en: false   # 是否使用逆合成光流法

max_iterations: 5               # 视觉ESKF最大迭代次数

img_point_cov: 100              # 视觉点协方差

raycast_en: false               # 是否启用光线投射

exposure_estimate_en: true      # 是否在线估计曝光时间

inv_expo_cov: 0.2               # 逆曝光时间协方差

grid_size: 5                    # 特征提取网格大小

grid_n_height: 17               # 特征提取网格高度

patch_pyrimid_level: 3          # 图像金字塔层数

patch_size: 8                   # 图像补丁大小

outlier_threshold: 1000         # 外点阈值

time_offset:
exposure_time_init: 0.0         # 初始曝光时间

img_time_offset: 0.0            # 图像时间偏移

imu_time_offset: 0.0            # IMU时间偏移

lidar_time_offset: 0.0          # LiDAR时间偏移extrin_calib:
    extrinsic_T: [0, 0, 0]          # LiDAR到IMU的平移外参

extrinsic_R: [1,0,0, 0,1,0, 0,0,1]  # LiDAR到IMU的旋转外参
Pcl: [0, 0, 0]                  # 相机到LiDAR的平移外参

Rcl: [1,0,0, 0,1,0, 0,0,1]      # 相机到LiDAR的旋转外参
mapping:
    filter_size_surf: 0.5           # 平面特征降采样分辨率 (m)
    max_iteration: 3                # ESKF最大迭代次数

acc_cov: 0.1                    # 加速度噪声协方差

gyr_cov: 0.1                    # 陀螺仪噪声协方差b_acc_cov: 0.0001               # 加速度计偏置噪声
b_gyr_cov: 0.0001               # 陀螺仪偏置噪声fov_degree: 360                 # 视场角 (度)
    det_range: 100.0                # 最大探测距离 (m)
pcd_save:
    pcd_save_en: true               # 是否保存PCD
    interval: -1                    # 保存间隔: -1=结束时保存, >0=每N帧保存

视觉相关参数调优建议：

  • grid_size：特征提取网格大小。越小提取的特征点越多，但计算量越大。建议3-8
  • patch_size：图像补丁大小。越大特征越稳定，但计算量越大。建议6-10
  • patch_pyrimid_level：图像金字塔层数。越多对大运动越鲁棒，但计算量越大。建议2-4
  • exposure_estimate_en：是否在线估计曝光时间。光照变化大的场景建议开启
  • img_point_cov：视觉点协方差。越小视觉约束越强。建议50-200

3.4相机内参与外参标定

FAST-LIVO需要准确的相机内参和相机-外参。推荐使用Kalibr工具包进行标定：

# 安装Kalibr
sudo apt-get install ros-noetic-kalibr

# 录制标定数据（多棋盘格标定板）rosbag record /camera/image_raw /camera/camera_info /livox/imu

# 标定相机内参kalibr_calibrate_cameras --cameras cam0 --target aprilgrid.yaml \
  --bag calibration.bag

# 标定相机-IMU外参kalibr_calibrate_imu_camera --cam cam0 --imu imu0 \
  --target aprilgrid.yaml --bag calibration.bag --bag-from-to 5 50

标定完成后，将得到的内参填入相机驱动节点，外参填入YAML配置文件的extrin_calib部分。FAST-LIVO也推荐使用FAST-Calib工具包进行LiDAR-Camera联合标定，其输出参数可以直接填入YAML文件。

如果标定精度不够，FAST-LIVO也支持在线估计相机-IMU外参，但初始值不能偏差太大，否则会收敛到错误值。

3.5多雷达型号适配

FAST-LIVO支持的雷达型号与FAST-LIO相同：

雷达型号lidar_typescan_line话题名消息类型
Livox AVIA16/livox/lidarCustomMsg
Velodyne 16/32/64216/32/64/velodyne_pointsPointCloud2
Livox Mid-36024/livox/lidarCustomMsg
Livox Mid-7034/livox/lidarCustomMsg
Livox Horizon46/livox/lidarCustomMsg
Livox Tele51/livox/lidarCustomMsg

3.6使用rosbag离线建图

FAST-LIVO的离线建图流程与FAST-LIO类似，但需要额外提供相机数据：

# 终端1: 启动FAST-LIVO
roslaunch fast_livo mapping_avia.launch rviz:=false

# 终端2: 播放rosbag（包含LiDAR + IMU + Camera数据）rosbag play -r 10 your_bag_file.bag

# 播完后按Ctrl+C保存PCD

注意：如果bag包中不包含相机数据，FAST-LIVO会自动退化为LIO模式，只使用LiDAR和IMU进行建图。如果连IMU数据也没有，则会退化为LO模式。

四、FAST-LIO vs FAST-LIVO对比

特性FAST-LIOFAST-LIVO
传感器LiDAR + IMULiDAR + IMU + Camera
地图结构ikd-TreeVoxelOctoTree（八叉树）
退化检测无有，自动切换到视觉通道
曝光补偿无有，在线估计逆曝光时间
运行频率100Hz100Hz（激光为主）
内存占用较高较低（八叉树更紧凑）
适用场景一般环境含退化场景的复杂环境
ROS话题LiDAR + IMULiDAR + IMU + Camera
参数复杂度较低较高（增加视觉参数）
工作模式LIOLIVO / LIO / LO（自动切换）

五、常见问题与排查

5.1视觉通道不工作

检查以下几点：

  • 确认bag包中包含相机数据（rostopic list检查）
  • 确认相机话题名与配置一致
  • 确认img_en设为1
  • 确认相机内参正确填写

5.2退化检测过于敏感

如果系统频繁切换到视觉通道，可能是退化检测阈值过高。可以调整：

  • 增大退化检测的特征值阈值
  • 降低视觉通道的权重

5.3视觉特征跟踪丢失

视觉特征跟踪丢失的常见原因：

  • 相机曝光时间过长，导致运动模糊
  • 环境缺乏纹理（白墙、纯色地面）
  • 相机帧率过低（建议至少20Hz）
  • 光照变化过大（开启exposure_estimate_en可缓解）

5.4没有生成PCD

PCD文件保存在退出钩子（Exit Hook）中，只有在按Ctrl+C正常退出时才会触发。如果直接关闭终端或kill进程，PCD不会保存。

5.5建图漂移严重

建图漂移通常由以下原因导致：

  • IMU和LiDAR的外参不准确，需要重新标定外参
  • 相机和LiDAR的外参不准确，需要重新标定
  • IMU噪声参数设置不合理，调整acc_cov和gyr_cov
  • 环境缺乏几何特征（长走廊、白墙），开启视觉通道可缓解

FAST-LIVO论文: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry

更多文章