首页导航slam

# FAST-LIVO

可参考教程：FAST-LIVO2
“ 一个极其高效的，抵抗各种极端退化环境，可达像素级精度，首个可以用到UAV自动导航的激光雷达-视觉-惯性里程计，并可以实时重建高精度彩色点云地图 ”

辨析

FAST-LIVO 在 FAST-LIO/LIO 的基础上加入了相机视觉约束，利用图像的光度信息和纹理信息辅助定位，属于 LiDAR-Inertial-Visual Odometry。后续 FAST-LIVO2 进一步把 LiDAR、IMU、图像测量统一到 ESIKF 中，并使用直接法处理 LiDAR 和视觉测量

# FAST-LIVO 笔记：原理、复现与拓展流程

# 1. FAST-LIVO 概念与核心思想

FAST-LIVO 全称为 Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry，是一种激光、惯性、视觉紧耦合里程计系统。

它在 FAST-LIO 的基础上加入相机信息，形成 LiDAR + IMU + Camera 的多传感器融合定位方法。
FAST-LIO2 主要依靠激光点云几何结构和 IMU 估计位姿，而 FAST-LIVO 进一步利用图像纹理和光度信息，在几何退化场景下提升定位鲁棒性。简单理解：FAST-LIO2 = LiDAR + IMU，FAST-LIVO = LiDAR + IMU + Camera。

整体流程：IMU 预测位姿 -> LiDAR 点云与地图匹配 -> Camera 进行直接法光度跟踪 -> 融合更新状态 -> 输出位姿和地图。

# 2. 为什么加入视觉信息

纯激光惯性系统在普通环境中已经很强，但在长走廊、空旷区域、重复结构、平面墙面等场景中，点云几何约束可能不足，容易出现退化。视觉信息可以提供纹理、颜色、边缘等信息，用来补充激光的不足。例如白墙走廊中，激光看到的几何结构比较相似，但相机可能看到墙上的海报、门框、纹理变化，这些信息可以帮助系统更稳定地估计位姿。因此 FAST-LIVO 更适合复杂室内环境、移动机器人、无人机和多传感器建图任务。

# 3. 系统组成

模块 输入 作用
IMU 预测 加速度、角速度 高频预测运动状态，提供初值
LiDAR 更新 点云 利用空间几何结构约束位姿
Camera 更新 图像 利用光度误差和纹理信息约束位姿
状态估计 IMU + LiDAR + Camera 融合更新最终位姿
地图维护 点云地图/视觉地图 保存环境结构信息，辅助后续匹配

FAST-LIVO 使用稀疏直接法处理视觉信息。直接法不依赖 ORB、SIFT 等特征点，而是直接比较图像块的灰度或亮度变化。常见光度误差形式为：

E=∑i(It(ui)−Ir(ui′))2E=\sum_i \left(I_t(u_i)-I_r(u_i')\right)^2

其中，ItI_t 表示当前图像，IrI_r 表示参考图像，uiu_i 和 ui′u_i' 表示同一空间点在不同图像中的像素位置。优化目标是让同一位置在不同图像中的亮度尽可能一致，从而反推出相机和机器人的运动。

# 4. FAST-LIVO 与常见 SLAM 方法对比

方法 传感器 类型 特点
ORB-SLAM3 Camera / Camera + IMU 视觉 SLAM 特征点法，依赖图像特征
VINS-Fusion Camera + IMU VIO 经典视觉惯性系统
FAST-LIO2 LiDAR + IMU LIO 速度快，依赖点云几何
LVI-SAM LiDAR + Camera + IMU LVI 融合 激光和视觉相对模块化
FAST-LIVO LiDAR + Camera + IMU LIVO 紧耦合，融合几何与视觉纹理

简单来说，FAST-LIO2 更偏向高效激光惯性建图，FAST-LIVO 更偏向复杂场景下的多传感器鲁棒定位。

# 5. 复现准备

复现 FAST-LIVO 前，需要先准备 LiDAR、IMU、Camera 三类传感器，并保证三件事：话题正常、时间同步正常、外参标定正确。推荐不要一开始直接跑 FAST-LIVO，而是按传感器和模块逐步验证。

硬件准备：LiDAR + IMU + Camera -> 工控机/笔记本/Jetson -> ROS 环境 -> FAST-LIVO 源码。

常见传感器组合：Livox Mid360 + 相机 + IMU，或 Velodyne/Ouster + 工业相机 + IMU。若使用 RealSense D435i，需要注意它自带 IMU 和相机，但 LiDAR 与相机之间仍然需要外参标定。

# 6. 复现流程

复现建议顺序：安装依赖 -> 编译源码 -> 启动传感器驱动 -> 检查 ROS 话题 -> 标定外参 -> 先跑 FAST-LIO2 -> 再接入相机跑 FAST-LIVO -> 录包与回放调试 -> 参数优化。

# 6.1 环境与源码

先确认 ROS 版本、PCL、Eigen、OpenCV、Livox SDK 或对应 LiDAR 驱动是否安装正常。然后创建工作空间并编译：

mkdir -p ~/fast_livo_ws/src
cd ~/fast_livo_ws/src
git clone <FAST-LIVO 仓库地址>
cd ..
colcon build
source install/setup.bash

colcon build 用于编译 ROS2 工作空间，source install/setup.bash 用于加载当前工作空间环境。如果找不到包或 launch 文件，优先检查是否执行了 source。

# 6.2 传感器话题检查

启动传感器驱动后，检查关键话题是否存在：

ros2 topic list
ros2 topic hz /imu/data
ros2 topic hz /camera/image_raw
ros2 topic hz /livox/lidar

ros2 topic list 用于查看当前 ROS 系统中的话题；ros2 topic hz 用于查看话题发布频率。IMU 频率通常较高，相机频率一般为 20～30 Hz，LiDAR 频率根据型号不同可能为 10 Hz 或更高。如果频率不稳定，后续定位容易漂移或失败。

# 6.3 时间同步

时间同步是 FAST-LIVO 复现的关键。IMU、LiDAR、Camera 的时间戳必须尽量一致，否则会出现轨迹漂移、地图重影、图像跟踪失败等问题。理想情况是硬件同步，例如触发同步、PPS 同步或统一硬件时钟；如果没有硬同步，至少要保证系统时间一致，并检查 ROS 消息时间戳是否正常。

检查思路：查看每个话题 header.stamp -> 对比传感器时间差 -> 判断是否存在明显延迟 -> 必要时使用 rosbag 回放分析。

# 6.4 外参标定

FAST-LIVO 至少需要 LiDAR-IMU 外参和 Camera-IMU 外参，也可以理解为要知道三个传感器之间的空间安装关系。常见外参包括：

TLIT_{LI}

表示 LiDAR 到 IMU 的变换，$$
T_

表示 Camera 到 IMU 的变换。外参错误会直接导致地图扭曲、轨迹发散、视觉投影不准等问题。 外参调试顺序：先保证 IMU 方向正确 -> 再保证 LiDAR 点云不扭曲 -> 再检查相机图像与点云投影是否对齐。 ### 6.5 先跑 FAST-LIO2 在接入视觉前，建议先单独跑 FAST-LIO2 或 FAST-LIVO 中的 LIO 部分。原因是 FAST-LIVO 建立在激光惯性里程计稳定的基础上，如果 LIO 本身就不稳定，加入视觉后问题会更难排查。 判断标准：点云地图不明显重影，轨迹连续，机器人静止时位姿不剧烈抖动，快速转动时地图不发散。 ### 6.6 再跑 FAST-LIVO 当 LiDAR + IMU 已经稳定后，再接入相机图像。启动顺序通常是：传感器驱动 -> FAST-LIVO 配置文件 -> FAST-LIVO launch -> RViz 可视化。重点观察轨迹是否连续、点云地图是否扭曲、视觉跟踪是否稳定、CPU 占用是否过高。 如果视觉加入后效果变差，优先检查相机外参、时间同步、图像曝光、图像模糊和相机内参。 ## 7. 配置文件重点参数 |参数类别|关注内容|说明| |---|---|---| |话题名称|LiDAR、IMU、Camera topic|必须和实际 ROS 话题一致| |相机内参|$f_x,f_y,c_x,c_y$|决定图像投影是否准确| |相机畸变|$k_1,k_2,p_1,p_2$ 等|畸变不准会影响视觉误差| |外参矩阵|$T_{LI},T_{CI}$|决定传感器空间关系| |IMU 噪声|gyro、acc noise|影响预测和滤波稳定性| |图像参数|分辨率、曝光、帧率|影响视觉跟踪效果| |点云参数|过滤范围、体素大小|影响速度和建图质量| 参数调试原则：先保证话题和外参正确，再调噪声参数和滤波参数；先低速运动测试，再增加运动幅度；先室内简单场景测试，再进入复杂环境。 ## 8. 常见问题与排查 |问题|可能原因|排查方向| |---|---|---| |地图重影|时间不同步、外参错误|检查时间戳和 $T_{LI}$| |轨迹漂移严重|IMU 参数错误、LiDAR 退化|检查 IMU 噪声和点云质量| |加入视觉后变差|相机外参错误、曝光不合适|检查 $T_{CI}$、图像质量| |图像跟踪失败|运动模糊、纹理太弱|提高快门速度，降低运动速度| |点云扭曲|IMU-LiDAR 同步问题|检查 deskew 和时间戳| |编译失败|依赖缺失、ROS 版本不匹配|检查 PCL、OpenCV、Eigen、SDK| |CPU 占用高|图像分辨率高、点云过密|降分辨率、加体素滤波| ## 9. 拓展流程 ### 9.1 FAST-LIVO2 FAST-LIVO2 是 FAST-LIVO 的后续改进版本，通常在速度、精度和系统统一性上更强。如果目标是做长期研究或工程部署，可以优先关注 FAST-LIVO2。学习路线：FAST-LIO2 -> FAST-LIVO -> FAST-LIVO2。 ### 9.2 接入 Nav2 FAST-LIVO 可以作为定位模块，为机器人导航提供位姿估计。典型链路为：FAST-LIVO 输出 odom/pose -> 转换为 Nav2 可用 TF -> Nav2 使用地图和定位进行规划 -> 输出 `/cmd_vel` -> 底盘执行。重点是保证 TF 链完整，例如 `map -> odom -> base_link -> lidar/camera`。 ### 9.3 生成彩色点云地图 由于 FAST-LIVO 同时有点云和图像，可以进一步把图像颜色投影到点云上，生成彩色点云地图。流程为：LiDAR 点云 -> 根据外参投影到图像平面 -> 读取像素颜色 -> 给点云赋 RGB -> 保存彩色点云地图。 ### 9.4 加入语义信息 可以结合 YOLO、语义分割或目标检测模型，把图像中的目标类别加入地图。流程为：相机图像 -> 目标检测/语义分割 -> 与点云关联 -> 生成带类别标签的语义地图。这个方向适合机器人导航、具身智能、场景理解和安全避障。 ### 9.5 加入 GPS/RTK 如果在室外使用，可以加入 GPS 或 RTK，形成局部高精度 + 全局无漂移的系统。流程为：FAST-LIVO 提供局部连续位姿 -> RTK 提供全局位置约束 -> 融合优化 -> 输出全局一致轨迹。 ## 10. 学习与复现建议 推荐学习顺序：FAST-LIO2 原理 -> ESKF/ESIKF -> 点云配准与 ikd-Tree -> 视觉直接法 -> FAST-LIVO 系统结构 -> 传感器标定与同步 -> 真机部署。复现时不要一上来改源码，先跑通官方数据集，再跑自己的 rosbag，最后上真机实时运行。 最重要的复现原则：先让 IMU 正常，再让 LiDAR + IMU 稳定，再加入 Camera；先解决时间同步和外参，再考虑调算法参数。