一、FAST-LIO2 是一个 LiDAR-IMU 紧耦合的实时里程计与建图系统：

• 输入：3D LiDAR 点云 + IMU 数据

• 输出：机器人位姿（odometry）、路径、全局地图、配准后的点云

• 核心算法：迭代扩展卡尔曼滤波（IEKF） + ikd-Tree 增量式地图 + 点到平面 ICP

• 特点：不提取特征（直接配准原始点）、计算效率高、支持多种 LiDAR

laserMapping.cpp	主节点：状态估计、地图管理、ICP 配准、ROS 发布
IMU_Processing.hpp	IMU 初始化、前向传播、点云去畸变
preprocess.cpp	点云预处理：格式转换、特征提取、滤波

二、laserMapping.cpp：主建图节点

这是整个系统的核心，对应论文 FAST-LIO2 的主循环。

1. 数据订阅与缓存

• 订阅 LiDAR：

  • 如果 lidar_type == AVIA，订阅 livox_ros_driver2::msg::CustomMsg（Livox 自定义消息）。

  • 否则订阅标准 sensor_msgs::msg::PointCloud2。

• 订阅 IMU：sensor_msgs::msg::Imu。

• 用 lidar_buffer 和 imu_buffer 缓存数据，sync_packages() 负责把同一时间窗口内的 LiDAR 帧和 IMU 数据打包成 MeasureGroup。

2. 点云预处理

调用 p_pre->process(msg, ptr)，根据 LiDAR 类型做格式转换、盲区过滤、下采样、时间戳计算。FAST-LIO2 默认不提取特征，直接使用原始点云做 ICP。

3. IMU 处理与去畸变

调用 p_imu->Process(Measures, kf, feats_undistort)：

• IMU 初始化：估计重力方向、陀螺仪零偏、加速度计零偏。

• 前向传播：对每个 IMU 测量做 EKF 预测。

• 点云去畸变：把一帧内不同时刻采集的点统一到帧尾时刻。

4. 局部地图管理

• 使用 ikd-Tree 存储全局地图。

• lasermap_fov_segment() 以机器人当前位置为中心，维护一个立方体局部地图（cube_side_length，默认 200m）。

• 当机器人接近地图边缘时，删除旧区域、移动立方体，保证地图始终覆盖当前 FOV。

5. 状态估计（IEKF）

• kf.update_iterated_dyn_share_modified() 执行迭代 EKF 更新。

• h_share_model() 是观测模型：对每个下采样点，在 ikd-Tree 中找最近邻，拟合平面，计算点到平面的残差。

• 构造雅可比矩阵 h_x 和残差向量 h，送入 EKF 求解。

• 状态量包括：位置、旋转、速度、陀螺仪零偏、加速度计零偏、重力向量、LiDAR-IMU 外参。

6. 地图增量更新

• map_incremental() 把当前帧去畸变后的点转换到世界坐标系，按体素下采样后加入 ikd-Tree。

• 使用 Nearest_Points 判断新点是否需要加入，避免重复。

7. 发布与保存

• /Odometry：里程计

• /path：轨迹

• /cloud_registered：世界坐标系下的当前帧点云，frame_id = camera_init

• /cloud_registered_body：IMU 坐标系下的当前帧点云，frame_id = body

• /cloud_effected：参与 ICP 的有效点

• /Laser_map：累积地图点云

• TF：camera_init → body

• map_save 服务：保存 PCD 地图

• 支持 runtime_pos_log 记录运行时间日志

• 支持 pcd_save 保存扫描（当前版本中 publish_frame_world 里的保存逻辑被注释掉了，只在 main 末尾保存 scans.pcd）

8. 固定坐标系

当前版本使用固定的坐标系名称：

• 世界坐标系：camera_init

• 机体坐标系：body

三、IMU_Processing.hpp：IMU 处理与去畸变

1. IMU 初始化

IMU_init()：

• 取第一帧 IMU 的加速度和角速度作为初始均值。

• 对后续 IMU 测量做滑动平均，估计：

  • 重力方向 grav

  • 陀螺仪零偏 bg

  • 加速度计协方差 cov_acc

  • 陀螺仪协方差 cov_gyr

• 设置初始协方差矩阵 init_P，其中外参、零偏的初始不确定性很小。

• 初始化完成后，imu_need_init_ 置 false，进入正常处理。

2. 前向传播

UndistortPcl()：

• 把上一帧最后一个 IMU 加入当前帧 IMU 序列头部，保证时间连续。

• 对每两个相邻 IMU 测量，计算平均角速度和平均加速度。

• 调用 kf_state.predict(dt, Q, in) 做 EKF 预测。

• 保存每个 IMU 时刻的位姿到 IMUpose，用于后续反向传播去畸变。

• 对帧尾时刻做一次额外预测，得到帧尾位姿。

3. 反向传播去畸变

• 从帧尾向帧头遍历 IMUpose。

• 对每个 LiDAR 点，根据其时间戳找到对应的 IMU 区间。

• 用该区间的旋转、速度、加速度，把点变换到帧尾时刻：

Pcompensate=RL−1(Rend−1(Ri(RLPi+TL)+Tei)−TL)Pcompensate​=RL−1​(Rend−1​(Ri​(RL​Pi​+TL​)+Tei​)−TL​)

• 这样一帧内所有点都统一到帧尾时刻，消除运动畸变。

4. 外参与协方差设置

• set_extrinsic()：设置 LiDAR 到 IMU 的平移和旋转。

• set_gyr_cov()、set_acc_cov()、set_gyr_bias_cov()、set_acc_bias_cov()：设置过程噪声协方差。

---

四、preprocess.cpp：点云预处理

1. 多 LiDAR 支持

process() 根据 lidar_type 分派到不同 handler：

类型	Handler	说明
AVIA	avia_handler	Livox AVIA，用 CustomMsg
OUST64	oust64_handler	Ouster 64 线
VELO16	velodyne_handler	Velodyne 16 线
MID360	mid360_handler	Livox Mid-360
默认	default_handler	标准 PointXYZI

2. 通用处理

• 清除盲区点（blind 参数，默认 0.01m）。

• 按 point_filter_num 下采样。

• 把点的时间戳写入 curvature 字段（单位 ms），供去畸变使用。

3. 时间戳处理

• 如果点云自带时间戳（given_offset_time），直接用。

• 否则根据水平角速度和 yaw 角推算每个点的时间偏移。

• 对 Mid360，由于 Livox 驱动不提供每点时间戳，用 yaw 角推算。

4. 特征提取（可选）

give_feature() 在 feature_enabled=true 时启用：

• 把点按扫描线分组。

• 对每组做平面判断：plane_judge() 检查局部点是否共面。

• 标记点类型：Real_Plane、Poss_Plane、Edge_Jump、Edge_Plane、Wire、Nor。

• 提取平面点和边缘点，分别存入 pl_surf 和 pl_corn。

• FAST-LIO2 默认 feature_extract_enable=false，直接用原始点做 ICP。

5. 关键参数

参数	含义
blind	盲区距离
point_filter_num	下采样间隔
N_SCANS	扫描线数
SCAN_RATE	扫描频率
group_size	平面判断的邻域大小
disA、disB	平面判断的距离阈值
p2l_ratio	点到线距离比阈值
limit_maxmid、limit_midmin、limit_maxmin	平面判断的深度比例阈值

LiDAR 点云 ──► preprocess.cpp ──► 去盲区、下采样、时间戳
                                      │
IMU 数据 ─────────────────────────────┤
                                      ▼
                              IMU_Processing.hpp
                          初始化、前向传播、去畸变
                                      │
                                      ▼
                              laserMapping.cpp
                    ┌─────────────────┼─────────────────┐
                    ▼                 ▼                 ▼
              ikd-Tree 地图      IEKF 状态估计      ROS 发布
              增量更新           点到平面 ICP        odom/path/cloud

六、与 FAST-LIO2 论文的对应

论文概念	代码实现
迭代扩展卡尔曼滤波	kf.update_iterated_dyn_share_modified()
点到平面残差	h_share_model()
ikd-Tree	ikdtree 对象，Add_Points()、Delete_Point_Boxes()
局部地图 FOV 分割	lasermap_fov_segment()
IMU 前向传播	ImuProcess::UndistortPcl()
点云去畸变	UndistortPcl() 中的反向传播
外参在线估计	extrinsic_est_en，状态量含 offset_R_L_I、offset_T_L_I

fastlio2-二次开发：

对比项	（标准 FAST-LIO2）	（二次开发版）
定位模式	纯在线建图与里程计	增加 locate_in_prior_map 先验地图定位模式
先验地图	无	可加载 PCD 先验地图，下采样后构建 ikd-Tree
初始位姿	不需要	订阅 /icp_result 获取初始位姿，等待后才开始
增量建图	始终执行 map_incremental()	先验地图模式下不执行增量建图
坐标系	固定 camera_init / body	可配置 odom_frame_id、sensor_frame_id、base_frame_id
odom→base TF	无	支持 send_odom_base_tf，发布 odom→base_frame
倾斜补偿	无	支持 apply_tilt_correction、tilt_angle_deg，用 compute_tilt_correction() 补偿安装倾斜
TF 监听	无	有 tf_buffer_ 和 tf_listener_，用于查询 base_frame→sensor_frame
里程计话题	/Odometry	/odom
发布地图方式	publish_map() 用 pcl_wait_pub 累积当前帧世界点云	publish_map() 直接发布 featsFromMap（ikd-Tree 内部点云）
PCD 保存	publish_frame_world 中保存逻辑被注释，只在 main 末尾保存 scans.pcd	publish_frame_world 中保存逻辑启用，按 pcd_save_interval 保存 scans_*.pcd
头文件	无 tf2_ros/transform_listener.h、tf2_geometry_msgs、pcl_ros/transforms.hpp、pose_with_covariance_stamped.hpp	增加了这些头文件
参数声明	只包含标准 FAST-LIO2 参数	增加了 common.odom_frame_id、common.sensor_frame_id、common.base_frame_id、common.send_odom_base_tf、common.apply_tilt_correction、common.tilt_angle_deg、locate_in_prior_map、prior_map_path
核心算法	IEKF、ikd-Tree、点到平面 ICP、IMU 去畸变	完全相同，没有改动
IMU 处理	调用同一个 ImuProcess	调用同一个 ImuProcess
预处理	调用同一个 Preprocess	调用同一个 Preprocess

1. 支持先验地图定位（重定位）

问题：标准 FAST-LIO2 每次启动都从零开始建图，无法利用已有地图。在室内导航中，机器人通常已经在已知环境中运行，不需要每次重新建图，而是需要在地图中定位自己。

修改：

• 增加 locate_in_prior_map 参数。

• 加载 PCD 先验地图，下采样后构建 ikd-Tree。

• 订阅 /icp_result 获取初始位姿。

• 收到初始位姿后，把先验地图变换到初始位姿坐标系，开始定位。

• 该模式下不执行增量建图，只做定位。

目的：让 FAST-LIO2 变成一个纯定位模块，类似 AMCL 但用 LiDAR，适用于已有地图的室内导航。

2. 适配 ROS 导航栈的坐标系与 TF

问题：标准 FAST-LIO2 使用固定的 camera_init 和 body 坐标系，且只发布 camera_init → body 的 TF。ROS 2 Nav2 期望的是 odom → base_link 的 TF，以及可配置的 base_frame、odom_frame、sensor_frame。

修改：

• 增加 common.odom_frame_id、common.sensor_frame_id、common.base_frame_id 参数。

• 增加 send_odom_base_tf 参数。

• 当启用时，通过 TF 查询 base_frame → sensor_frame 的变换，再结合 odom → sensor 的位姿，计算出 odom → base 的位姿并发布。

• 里程计话题从 /Odometry 改为 /odom。

目的：让 FAST-LIO2 的输出能直接接入 Nav2，不需要额外的 TF 转换节点。

3. 处理传感器安装倾斜

问题：实际机器人上，LiDAR/IMU 安装时可能存在倾斜（比如雷达不是水平安装的）。标准 FAST-LIO2 假设传感器坐标系与机体坐标系对齐，倾斜会导致 odom → base 的 TF 有固定偏差，影响导航。

修改：

• 增加 common.apply_tilt_correction 和 common.tilt_angle_deg 参数。

• compute_tilt_correction() 对 odom → sensor 的旋转乘以一个绕 Y 轴的旋转补偿。

• 在发布 TF 时应用该补偿。

目的：补偿安装倾斜，让 odom → base 更准确。

局限：只补偿绕 Y 轴的倾斜，且需要手动指定角度。如果倾斜是绕 X 轴或 Z 轴，或者角度未知，这个补偿就不够。更好的做法是标定外参，或者在线估计安装角。

4. 工程化与可配置性

问题：标准 FAST-LIO2 的很多参数是硬编码的，话题名、坐标系名固定，部署时不够灵活。

修改：

• 增加大量 ROS 参数声明，覆盖坐标系、定位模式、倾斜补偿、先验地图路径等。

• 增加 TF listener 和 buffer，用于查询 base→sensor 变换。

• 增加 /icp_result 订阅，等待初始位姿。

目的：让节点更容易部署到不同机器人平台，减少改代码的需求。