FAST_LIO_LC包报错：无效四元数变换问题排查求助

一、"aft_pgo"无效四元数错误排查与修复 #

• 错误本质：发布aft_pgo坐标系变换的节点输出了含NaN值的四元数，大概率是图优化模块（PGO）初始化阶段缺乏有效数据，或数据同步异常导致计算溢出。
• 排查步骤：
  1. 核验rosbag数据完整性：用rosbag info查看自制rosbag的LiDAR、IMU消息频率、总时长，对比示例rosbag，确认无丢帧、时间戳跳变情况。
  2. 验证数据同步性：用rqt_plot同时可视化LiDAR和IMU的时间戳序列，检查两者时间基准是否一致、有无明显偏移。
  3. 检查launch配置：打开mapping_velodyne.launch和fastlio_velodyne_VLP_16.launch，确认图优化模块的初始位姿、关键帧间隔等参数是否匹配硬件，避免因参数不合理触发异常计算。
  4. 查看节点详细日志：启动时加--screen参数（如roslaunch fast_lio mapping_velodyne.launch --screen），排查PGO相关节点的前置错误（比如IMU数据读取失败、LiDAR特征提取异常）。
• 修复方案：
  • 若为数据同步问题：用rosbag filter或自定义节点校准rosbag时间戳；
  • 若为参数问题：参考示例launch文件调整图优化初始化阈值，比如增大关键帧最小移动距离，避免初始阶段频繁触发优化；
  • 若为rosbag损坏：重新录制数据，确保录制时硬件连接稳定。

二、IMU frame_id差异问题处理 #

无需修改rosbag的frame_id，两种便捷解决方式：

• 修改包内配置参数：找到FAST_LIO_LC的硬件配置文件（如config/velodyne.yaml），将IMU对应的frame_id参数从imu改为imu_link，与rosbag数据保持一致。
• 发布静态变换：若不想改动包内配置，可在启动节点前执行以下命令（假设imu与imu_link坐标系完全重合）：

  rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 imu imu_link
  

  也可将该命令添加到launch文件中作为前置启动项。

内容的提问来源于stack exchange，提问作者ajim