使用 FAST-LIO2 算法

1 概述

FAST-LIO2 是一种紧耦合 LiDAR-Inertial Odometry 算法，可融合激光雷达点云和 IMU 数据，实时输出里程计、轨迹和点云地图。Hesai 已适配 FAST-LIO2 算法，支持 JT16 / JT128 在 ROS 1 和 ROS 2 环境下通过标准点云与 IMU 消息运行建图。

2 准备工作

开始前请确认满足以下条件：

Ubuntu + ROS 环境：ROS 1 建议 Melodic 或 Noetic，ROS 2 使用 Humble。
已安装并编译 Hesai 点云 SDK HesaiLidar_SDK_2.0（ROS 驱动的底层依赖）。
已安装并编译 Hesai ROS 驱动 HesaiLidar_ROS_2.0，可正常输出点云和 IMU。
已获取并编译 FAST_LIO_Hesai 中的 FAST-LIO2 适配代码（下文快速启动会逐步说明）。
实时连接雷达时，雷达、主机网卡与驱动配置处于同一网段。

2.1 版本支持

版本	推荐环境 / 分支	构建工具	启动方式
ROS 1	Ubuntu 18.04 / 20.04，Melodic / Noetic · `main`	`catkin_make`	`roslaunch`
ROS 2	Ubuntu 22.04，ROS 2 Humble · `ROS2`	`colcon build`	`ros2 launch`

2.2 输入数据要求

默认配置读取以下 Topic：

数据	ROS 1 消息类型	ROS 2 消息类型	默认 Topic
点云	`sensor_msgs/PointCloud2`	`sensor_msgs/msg/PointCloud2`	`/lidar_points`
IMU	`sensor_msgs/Imu`	`sensor_msgs/msg/Imu`	`/lidar_imu`

点云需要包含点级时间戳和线号字段：

字段	类型	说明
`x`	`float32`	点 X 坐标
`y`	`float32`	点 Y 坐标
`z`	`float32`	点 Z 坐标
`intensity`	`float32`	反射强度
`ring`	`uint16`	激光线号
`timestamp`	`float64`	点级相对时间戳

点级时间戳字段名必须为 timestamp。如果驱动或 rosbag 中使用了其他字段名，需要先转换为上述字段格式。

2.3 验证数据是否就绪

启动 FAST-LIO2 前，建议先运行 check_input.py 一键验证输入链路（topic 存在、点云字段、时间戳单调性与单位、丢帧检测、IMU 频率、陀螺仪单位、LiDAR/IMU 时间同步等）：

# ROS 2（驱动或 rosbag 已在发布时执行）
ros2 run fast_lio check_input.py --model jt128

# ROS 1
rosrun fast_lio check_input.py --model jt16

如需对照 yaml 中的 preprocess.timestamp_unit 做一致性核对，追加 --timestamp-unit <值>（0=秒、1=毫秒、2=微秒、3=纳秒）。全部检查通过后再启动 FAST-LIO2。

3 快速启动

以下是完成从零到跑通建图的全过程步骤说明。依赖安装详情、常用参数配置和常见问题排查等请参阅后续各章节。

3.1 ROS 1 (Noetic) — JT16 / JT128

第一步 — 安装依赖

sudo apt update
sudo apt install -y \
  ros-noetic-pcl-ros \
  ros-noetic-eigen-conversions \
  libeigen3-dev \
  libpcl-dev

第二步 — 克隆仓库并编译

mkdir -p ~/fast_lio_ws/src
cd ~/fast_lio_ws/src
git clone git@github.com:HesaiTechnology-Spatial-Perception/FAST_LIO_Hesai.git
cd FAST_LIO_Hesai
git submodule update --init --recursive

cd ~/fast_lio_ws
source /opt/ros/noetic/setup.bash
catkin_make
source devel/setup.bash

第三步 — 启动 Hesai ROS 驱动

启动 Hesai ROS 驱动，使其发布 /lidar_points 和 /lidar_imu。

ROS 1：

cd ~/hesai_ros_ws
source devel/setup.bash
roslaunch hesai_ros_driver start.launch

实时连接雷达时，驱动配置中需启用实时数据源，并开启 ROS Topic 输出：

lidar:
  - driver:
      source_type: 1
      lidar_udp_type:
        device_ip_address: 192.168.1.201
        udp_port: 2368
        ptc_port: 9347
      ros:
        ros_send_point_cloud_topic: /lidar_points
        ros_send_imu_topic: /lidar_imu
        send_point_cloud_ros: true
        send_imu_ros: true

根据实际网络修改 device_ip_address、udp_port 和 ptc_port。

第四步 — 启动 FAST-LIO2

cd ~/fast_lio_ws
source /opt/ros/noetic/setup.bash
source devel/setup.bash
roslaunch fast_lio mapping_jt16.launch    # JT16

# roslaunch fast_lio mapping_jt128.launch  # JT128

3.2 ROS 2 (Humble) — JT16 / JT128

第一步 — 安装依赖

sudo apt update
sudo apt install -y \
  ros-humble-pcl-ros \
  ros-humble-pcl-conversions \
  ros-humble-tf2-ros \
  libeigen3-dev \
  libpcl-dev

第二步 — 克隆仓库并编译

mkdir -p ~/fast_lio2_ws/src
cd ~/fast_lio2_ws/src
git clone -b ROS2 git@github.com:HesaiTechnology-Spatial-Perception/FAST_LIO_Hesai.git
cd FAST_LIO_Hesai
git submodule update --init --recursive

cd ~/fast_lio2_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select fast_lio --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash
ros2 pkg prefix fast_lio    # 确认编译产物已注册

第三步 — 启动 Hesai ROS 2 驱动

启动 Hesai ROS 2 驱动，使其发布 /lidar_points 和 /lidar_imu。 ROS 2：

cd ~/hesai_ros2_ws
source install/setup.bash
ros2 launch hesai_ros_driver start.launch.py

第四步 — 启动 FAST-LIO2

cd ~/fast_lio2_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch fast_lio mapping_jt16.launch.py    # JT16

# ros2 launch fast_lio mapping_jt128.launch.py  # JT128

第五步 — 查看效果

启动后，在 RViz 中将 Fixed Frame 设为 camera_init，订阅 /cloud_registered 和 /path，即可实时查看点云和轨迹。完整输出列表和累计地图配置见查看建图效果。以下是 JT16 运行 FAST-LIO2 的效果示意图：

4 更多用法

4.1 启动选项

将命令中的 jt16 替换为 jt128 即可启动 JT128。如不需要打开 RViz，可追加 rviz:=false：

# ROS 1

roslaunch fast_lio mapping_jt128.launch rviz:=false

# ROS 2

ros2 launch fast_lio mapping_jt128.launch.py rviz:=false

4.2 使用 rosbag 回放

ROS 1：

roslaunch fast_lio mapping_jt16.launch
rosbag play your_data.bag

ROS 2：

ros2 launch fast_lio mapping_jt16.launch.py
ros2 bag play /path/to/your_bag

如果 rosbag 中 Topic 名称不同，可修改配置文件中的 Topic：

common:
  lid_topic: "/your_points_topic"
  imu_topic: "/your_imu_topic"

如果 rosbag 中保存的是 Hesai 原始 packet，需要先通过 Hesai 驱动解析并发布 /lidar_points 和 /lidar_imu，再运行 FAST-LIO2。

4.3 将 PCAP 转换为 rosbag

使用PandarView录制的PCAP文件，可以先把它转换成 FAST-LIO2 可直接回放的 rosbag。转换链路为：

input.pcap
  ↓  Hesai ROS 驱动（source_type: 2，PCAP 解析模式）
/lidar_points + /lidar_imu
  ↓  rosbag record / ros2 bag record
output.bag（ROS 1）或 output/ 目录（ROS 2）
  ↓  FAST-LIO2

仓库在 tools/pcap_to_rosbag/ 下提供了封装脚本，自动完成「生成 PCAP 驱动配置 → 启动解析 → 等待 topic → 录制 → 检测播放结束 → 校验 bag」全过程，无需手动操作驱动配置。

4.3.1 从 PCAP 跑通 FAST-LIO2

第一步 — 转换 PCAP 为 rosbag

ROS 1（输出为 .bag 文件）：

bash tools/pcap_to_rosbag/pcap_to_rosbag_ros1.sh \
  --model      jt128 \
  --pcap       /data/input.pcap \
  --correction /data/correction.csv \
  --firetime   /data/firetime.csv \
  --output     /data/output.bag \
  --driver-ws  ~/hesai_ros_ws

ROS 2（输出为目录，rosbag2 格式）：

bash tools/pcap_to_rosbag/pcap_to_rosbag_ros2.sh \
  --model      jt128 \
  --pcap       /data/input.pcap \
  --correction /data/correction.csv \
  --firetime   /data/firetime.csv \
  --output     /data/output \
  --driver-ws  ~/hesai_ros2_ws

脚本完成后会打印 bag 路径和下一步命令。

第二步 — 检查 bag 输入（可选但推荐）

回放 bag 的同时运行检查工具，确认点云字段、IMU 频率、时间同步等均正常，再进入下一步：

# 终端 1
rosbag play /data/output.bag        # ROS 1
# ros2 bag play /data/output        # ROS 2

# 终端 2
rosrun fast_lio check_input.py --model jt128     # ROS 1
# ros2 run fast_lio check_input.py --model jt128  # ROS 2

第三步 — 启动 FAST-LIO2 并回放 bag，在 RViz 查看建图效果

按使用 rosbag 回放节的步骤启动 FAST-LIO2 并回放 bag。RViz 会随 launch 自动打开——将 Fixed Frame 设为 camera_init，订阅 /cloud_registered 和 /path，即可实时看到 FAST-LIO2 的建图结果。

前提：PCAP 能被 Hesai 驱动正常解析、其中包含点云和 IMU、点云含 ring 与 timestamp 字段；驱动配置文件中 firetime_file_path 等字段名以你当前驱动版本为准。脚本依赖 Python 3 的 PyYAML（pip3 install pyyaml）。

4.4 输入与配置检查工具

为减少因输入链路或配置错误导致的建图失败，仓库在 tools/ 下提供了三个检查脚本。

check_input.py — 运行时检查（驱动或 rosbag 已在发布数据时使用），自动诊断 topic 是否存在、点云字段、timestamp 单调性与单位、丢帧、ring 范围、IMU 频率、陀螺仪单位（deg/rad）、LiDAR 与 IMU 时间同步等：

# ROS 2
ros2 run fast_lio check_input.py --model jt128

# ROS 1
rosrun fast_lio check_input.py --model jt16

check_config.py — 静态检查 yaml 配置（不需要 ROS 运行），校验 lidar_type / scan_line 是否与型号和 ROS 版本匹配、timestamp_unit、imu_gyr_unit、blind、外参矩阵是否合法等：

python3 tools/check_config.py --config config/jt128.yaml --model jt128 --ros 2
python3 tools/check_config.py --config config/jt16.yaml  --model jt16  --ros 1

check_map.py — 分析建图输出质量并给出针对性建议。检测面厚度（重影）、点云密度、轨迹漂移等，并指向可能的原因（外参 / 时间同步 / imu_gyr_unit / point_filter_num）：

# 离线分析保存的 PCD（推荐）
python3 tools/check_map.py --pcd PCD/fast_lio2_jt_map.pcd

# 实时分析（需 publish.map_en 已开启）
ros2 run fast_lio check_map.py --map-topic /Laser_map --odom-topic /Odometry

更多选项见 tools/README.md。

4.5 使用 AI Agent Skill

仓库在 tools/agent/ 下提供了 Skill 文件（run_fastlio2_ros1.md / run_fastlio2_ros2.md），有两种使用方式：

让 agent 自动读取：将对应文件加入 agent 上下文，agent 会自动按 skill 流程完成环境检测、编译、数据转换和建图启动。
直接指令调用：使用 /run-fastlio2-hesai-ros1 或 /run-fastlio2-hesai-ros2 显式触发对应 skill。

4.6 查看建图效果

RVIZ 中建议将 Fixed Frame 设为 camera_init，重点观察以下输出：

输出	Topic	说明
里程计	`/Odometry`	FAST-LIO2 估计位姿
轨迹	`/path`	累计轨迹
配准点云	`/cloud_registered`	当前帧转换到世界坐标系后的点云
机体系点云	`/cloud_registered_body`	当前帧机体系点云
有效特征点	`/cloud_effected`	参与匹配的点云
累计地图	`/Laser_map`	可选发布的累计点云地图
坐标变换	`/tf`	`camera_init` 到 `body` 的 TF

默认配置主要用于实时查看 /cloud_registered 和 /path。

ROS 2 下 JT16 / JT128 默认差异（按点云负载设计）：JT16 为 16 线、单帧点数较少，默认开启累计地图发布、有效特征点发布和 PCD 保存，便于开箱在 RViz 查看完整建图效果；JT128 为 128 线、单帧点数约为 JT16 的 8 倍，默认关闭上述三项以降低 CPU、内存和 RViz 渲染压力，需要时再在 yaml 中打开。ROS 1 两型号 yaml 均未开启累计地图发布（源码中 publish_map 默认注释）。

如需在 RViz 中查看完整累计地图，仅 ROS 2 版本 支持通过 yaml 控制：

publish:
  map_en: true          # 周期性发布 /Laser_map
  effect_map_en: true   # 周期性发布 /cloud_effected

ROS 1 版本目前没有 publish.map_en / publish.effect_map_en 参数，源码中 publish_map(...) 调用默认是注释掉的；/Laser_map topic 已 advertise 但默认不会发布数据。如需在 ROS 1 上查看完整累计地图，可以：

直接看 PCD 离线结果（见保存 PCD 地图）。
或修改 src/laserMapping.cpp 中被注释掉的 publish_map(pubLaserCloudMap); 调用后重新编译。

无论哪种方式，/Laser_map 会随运行时间累积点云，点数较多时会增加 CPU、内存和 RViz 渲染压力。

4.7 保存 PCD 地图

4.7.1 ROS 1

ROS 1 版本支持在 yaml 中打开 PCD 保存，节点可在退出时（或运行中按帧间隔）落盘，也提供 /map_save 服务用于运行中主动保存：

map_file_path: "PCD/fast_lio2_jt_map.pcd"   # 可选；不配置时默认保存到 PCD/scans.pcd

pcd_save:
  pcd_save_en: true
  interval: -1     # -1 表示节点退出时一次性保存；>0 表示每 N 帧保存一次

运行中主动保存：

rosservice call /map_save "{}"

实际输出路径：

`interval`	输出路径
`-1`（默认）	`map_file_path` 指定路径；未配置时为 `<包路径>/PCD/scans.pcd`
`> 0`	`<包路径>/PCD/scans_<index>.pcd`（每 `interval` 帧追加一份）

map_file_path 支持绝对路径和相对路径。相对路径会基于 FAST-LIO2 包根目录拼接。按帧间隔保存时仍使用 PCD/scans_<index>.pcd 命名。

要触发"节点退出时保存"，关闭 FAST-LIO2 节点（Ctrl+C）即可。要在节点运行过程中立即保存，调用 /map_save。

4.7.2 ROS 2

ROS 2 版本同样支持 yaml 退出落盘和 /map_save 服务运行中主动保存。其中 jt16.yaml 默认已开启 pcd_save_en 并将 map_file_path 配置为 PCD/fast_lio2_lidar_only_map.pcd；jt128.yaml 默认关闭 PCD 保存，如需保存请按下例打开：

map_file_path: "PCD/fast_lio2_lidar_only_map.pcd"

pcd_save:
  pcd_save_en: true
  interval: -1

ros2 service call /map_save std_srvs/srv/Trigger '{}'

服务返回 success=True 后，地图会写入 map_file_path 指定的位置。

5 常用参数

参数	JT16	JT128
`common.lid_topic` 点云 Topic	`/lidar_points`	`/lidar_points`
`common.imu_topic` IMU Topic	`/lidar_imu`	`/lidar_imu`
`common.imu_gyr_unit` 角速度单位	`deg`	`deg`
`preprocess.lidar_type` 类型枚举¹	ROS 1: `5`，ROS 2: `1`	ROS 1: `6`，ROS 2: `2`
`preprocess.scan_line` 扫描线数	`16`	`128`
`preprocess.timestamp_unit` 时间戳单位²	`0`	`0`
`preprocess.blind` 盲区距离(m)	`1.0`	`0.5`
`mapping.fov_degree` 视场角(度)	`90.0`	`360.0`
`mapping.det_range` 检测距离(m)	`200.0`	`200.0`
`publish.map_en` 发布累计地图³	ROS 2: `true`	ROS 2: `false`
`publish.effect_map_en` 发布有效点云³	ROS 2: `true`	ROS 2: `false`
`pcd_save.pcd_save_en` 保存 PCD 文件	ROS 1: `false`，ROS 2: `true`	`false`
`map_file_path` PCD 输出路径⁴	ROS 2: `PCD/fast_lio2_lidar_only_map.pcd` ROS 1: 未设置	未设置

脚注：

¹ ROS 1 中 JT16=5、JT128=6；ROS 2 中 JT16=1、JT128=2
² 0=秒、1=毫秒、2=微秒、3=纳秒
³ ROS 2 仅支持；ROS 1 无此参数
⁴ 支持相对路径（相对于包根目录）；未设置时默认 PCD/scans.pcd

publish.map_en、publish.effect_map_en、pcd_save.pcd_save_en 在 ROS 2 上 JT16 默认 true、JT128 默认 false，与 point_filter_num（JT16 为 1、JT128 为 6）同属「点少轻量、点多省资源」的默认策略。

5.1 外参配置

FAST-LIO2 中外参表示 LiDAR 坐标系相对于 IMU 坐标系的位姿。已有准确外参时，建议关闭在线外参估计：

mapping:
  extrinsic_est_en: false
  extrinsic_T: [0.0, 0.0, 0.0]
  extrinsic_R: [1.0, 0.0, 0.0,
                0.0, 1.0, 0.0,
                0.0, 0.0, 1.0]

如果外参未知，可先保持 extrinsic_est_en: true，但运动激励不足或时间同步不稳定时，在线估计可能导致轨迹漂移。

6 常见问题

6.1 FAST-LIO2 没有输出

运行 check_input.py 自动诊断（驱动已启动或 rosbag 正在播放）：

ros2 run fast_lio check_input.py --model jt128   # ROS 2
rosrun fast_lio check_input.py --model jt16       # ROS 1

脚本会按顺序输出：topic 是否存在 → 频率是否正常 → 字段是否完整 → 配置是否与数据一致，并给出对应的修复方向。如果 topic 不存在，说明驱动未启动或配置有误；如果频率为 0，说明驱动未正常发布数据；如果字段或配置不匹配，脚本会直接指出。

6.2 提示 IMU 和 LiDAR 不同步

运行 check_input.py，其中的时间同步检查（check 9）会自动比较 LiDAR 与 IMU 的 header 时间戳偏差：偏差 >1000ms 报 FAIL（时钟源不同），100ms–1000ms 报 WARN（建议配置 common.time_offset_lidar_to_imu）。其余需要手动确认的项：

rosbag 播放时是否使用了 --clock 以正确传递时间戳。
common.time_offset_lidar_to_imu 是否需要配置固定时间偏移。

6.3 轨迹发散或地图重影

先运行两个检查工具排查可自动检测的原因：

# 1. 检查输入数据（gyro 单位、timestamp 字段与单位、ring 范围、丢帧）
ros2 run fast_lio check_input.py --model jt128 --timestamp-unit 0

# 2. 检查 yaml 配置（lidar_type、scan_line、imu_gyr_unit、extrinsic_R 合法性）
python3 tools/check_config.py --config config/jt128.yaml --model jt128 --ros 2

如果两个工具均未报错，再手动排查以下无法自动检测的项：

LiDAR-IMU 外参不正确（需重新标定）。
雷达静止时间过短或初始化阶段运动过大。

6.4 RViz 中看不到完整地图

ROS 1：目前没有 publish.map_en 参数，且源码中 publish_map(...) 调用默认被注释。可改为通过 PCD 离线查看建图结果（见上文"保存 PCD 地图"），或修改源码取消注释后重新编译。
ROS 2（JT16）：点少，默认已开启 publish.map_en，可直接在 RViz 中查看 /Laser_map。
ROS 2（JT128）：点多，默认关闭以减轻负载；在 yaml 中设置 publish.map_en: true 后即可查看 /Laser_map。

6.5 建图出现结构边缘发散和点云不重合问题

JT128 点云量较大，当平台算力不足时，可能出现局部地图发散、点云不重合、建图质量下降等现象。在确认 IMU 与点云数据连续、无明显丢帧或时间戳异常后，可在默认值（JT128 为 6）基础上适当增大 point_filter_num，以减小单帧点数、降低算力压力：

preprocess:
    point_filter_num: 8

1 概述​

2 准备工作​

2.1 版本支持​

2.2 输入数据要求​

2.3 验证数据是否就绪​

3 快速启动​

3.1 ROS 1 (Noetic) — JT16 / JT128​

3.2 ROS 2 (Humble) — JT16 / JT128​

4 更多用法​

4.1 启动选项​

4.2 使用 rosbag 回放​

4.3 将 PCAP 转换为 rosbag​

4.3.1 从 PCAP 跑通 FAST-LIO2​

4.4 输入与配置检查工具​

4.5 使用 AI Agent Skill​

4.6 查看建图效果​

4.7 保存 PCD 地图​

4.7.1 ROS 1​

4.7.2 ROS 2​

5 常用参数​

5.1 外参配置​

6 常见问题​

6.1 FAST-LIO2 没有输出​

6.2 提示 IMU 和 LiDAR 不同步​

6.3 轨迹发散或地图重影​

6.4 RViz 中看不到完整地图​

6.5 建图出现结构边缘发散和点云不重合问题​