激光雷达点云格式转换这件事说大不大说小也真不小。我见过太多人雷达装好了、驱动跑通了、rostopic echo也能看到数据在刷结果一接到 LIO-SAM 或者 FAST-LIO2 上就傻眼——要么直接报字段缺失要么建出来的图飘得亲妈都不认识。问题往往不在算法本身而是卡在最前面那一环点云格式没对上。速腾聚创和禾赛这两家的雷达在市面上保有量极大但它们的 ROS 驱动吐出来的点云结构、时间字段命名、坐标系定义各有各的习惯而 LIO-SAM 和 FAST-LIO2 这两个主流激光SLAM框架对输入点云的要求又各不相同。这篇内容就是把我自己在速腾和禾赛雷达上做格式转换、适配这两个框架的完整过程拆开来讲从点云字段的底层结构到转换代码的每一行逻辑再到实测中遇到的坑和解决办法尽量说透。不管你是刚拿到雷达的新手还是已经跑通建图但想搞清楚底层细节的老手应该都能从中找到有用的东西。1. 先搞清楚两家雷达的点云到底长什么样很多人拿到雷达驱动之后直接就把 topic 接到 SLAM 节点上了根本没看过点云里到底有哪些字段。这一步偷懒后面就要用几倍的时间来还债。速腾和禾赛的 ROS 驱动输出的sensor_msgs/PointCloud2消息虽然顶层类型一样但里面的字段定义差别不小而这些差别恰恰是导致 SLAM 框架报错或建图异常的根源。1.1 速腾聚创点云的消息结构速腾的 ROS 驱动以 rslidar_sdk 为例默认输出的点云格式通常是XYZI或者XYZIRT取决于你选的输出模式。所谓XYZI就是每个点包含 x、y、z 三个空间坐标加一个 intensity 强度值每个字段一般是FLOAT32类型单个点占 16 个字节。而XYZIRT在此基础上多了两个字段ring也叫 channel表示这个点属于哪条激光线束和timestamp该点的精确采集时间单个点占 24 个字节有的版本 timestamp 用 FLOAT64那就是 32 字节。这里有个很容易被忽略的细节速腾不同型号的雷达ring 的编号方式可能不一样。比如 RS-16 的 ring 是从 0 到 15而有些型号是从 1 开始编号的。这个差异在单雷达建图时影响不大但如果你做多雷达融合ring 编号冲突就会导致特征提取出问题。另外速腾的 timestamp 字段在早期驱动版本里是每个点相对于该帧起始时刻的偏移量单位秒后来有些版本改成了绝对时间戳。这个区别非常关键因为 LIO-SAM 对时间戳的处理逻辑是强依赖的如果你的 timestamp 是相对偏移而 LIO-SAM 以为是绝对时间那整个 IMU 预积分和点云去畸变都会错得离谱。1.2 禾赛点云的消息结构禾赛的 ROS 驱动hesai_lidar 或 hesai_ros_driver输出的点云格式一般是XYZIRT字段包括 x、y、z、intensity、ring、timestamp。看起来和速腾的XYZIRT差不多但魔鬼在细节里。禾赛的 timestamp 字段通常是FLOAT64类型表示的是绝对时间Unix 时间戳单位秒精确到微秒级别。而速腾的 timestamp 在很多配置下是FLOAT32表示相对时间偏移。这两种时间语义完全不同直接混用必然出问题。禾赛的 ring 字段编号通常从 0 开始按照激光线束的物理排列顺序递增。以 Pandar40 为例ring 从 0 到 39AT128 则是 0 到 127。这个编号方式本身没问题但当你把禾赛的点云喂给 FAST-LIO2 时如果 FAST-LIO2 的配置里没有正确处理 ring 字段它可能会忽略这个信息导致特征提取时无法区分不同线束的点影响建图精度。还有一点禾赛的点云在默认配置下可能包含无效点距离为 0 或 NaN 的点这些点如果不滤掉进入 SLAM 框架后会污染最近邻搜索导致配准失败或者建图出现噪点。1.3 两个 SLAM 框架对点云的真实需求LIO-SAM 对输入点云的要求比较明确它需要XYZI格式的点云并且需要ring字段来做特征提取。时间戳方面LIO-SAM 期望点云消息的 header.stamp 是该帧的起始采集时间同时每个点的 timestamp 字段用于运动补偿去畸变。如果你用的是XYZI格式没有逐点 timestampLIO-SAM 会退化为假设一帧内所有点在同一时刻采集对于低速场景勉强能用但快速运动时建图会明显飘。FAST-LIO2 的要求又不一样。它支持XYZI和XYZIRT两种输入但更推荐XYZIRT因为它需要逐点时间戳来做精确的运动补偿。FAST-LIO2 对 ring 字段的依赖没有 LIO-SAM 那么强但如果你要做基于线束的特征提取比如提取地面点、边缘点ring 就必不可少了。另外FAST-LIO2 对点云的密度和分布比较敏感如果点云中包含大量无效点它的 ikd-Tree 构建效率会大幅下降。总结一下两家雷达的原始点云和两个框架的需求可以用下面这张表来对照维度速腾典型禾赛典型LIO-SAM 需求FAST-LIO2 需求基础字段XYZI 或 XYZIRTXYZIRTXYZI ringXYZI 或 XYZIRTtimestamp 类型FLOAT32相对FLOAT64绝对逐点相对时间逐点时间戳ring 编号0 或 1 起始0 起始需要用于线束区分可选但建议保留无效点较少可能较多需滤除需滤除点云密度中等较高适中即可偏好较高密度这张表是我在实际适配过程中反复验证后总结的不同型号和驱动版本可能有细微差异但大方向是一致的。搞清楚这些差异之后转换的思路就清晰了把两家雷达的输出统一成目标框架需要的格式该补的字段补上该改的类型改掉该滤的噪点滤掉。2. 格式转换的核心逻辑与代码实现格式转换这件事听起来像是简单的字段映射但真正写起来坑比想象的多。我一开始也觉得不就是改个字段名吗结果调了整整两天才跑通。下面把转换的核心逻辑和代码实现拆开讲包括每一步为什么要这么做。2.1 转换的整体思路统一到中间格式我的做法是先把两家雷达的点云统一转换成一个中间格式然后再从这个中间格式分别适配 LIO-SAM 和 FAST-LIO2。这样做的好处是如果你手头同时有速腾和禾赛的雷达或者以后换了雷达型号只需要改最前面那一层解析逻辑后面的适配代码不用动。中间格式我定义为x, y, z, intensity, ring, time其中 time 统一为相对于该帧起始时刻的偏移量单位秒类型 FLOAT32。ring 统一从 0 开始编号。这个格式兼顾了两个框架的需求也方便后续扩展。转换的流程大致是订阅原始点云 topic → 解析 PointCloud2 的字段 → 逐点提取并转换 → 组装成新的 PointCloud2 → 发布到新 topic。整个过程在一个 ROS 节点里完成用 C 写性能最好Python 也能用但大点云下会有延迟。2.2 解析 PointCloud2 的字段偏移sensor_msgs/PointCloud2这个消息类型的设计比较底层它把所有点的数据放在一个大的二进制 buffer 里通过 fields 数组来描述每个字段的名称、偏移量、数据类型和数量。要正确解析必须严格按照 fields 里的 offset 来读取不能想当然地按顺序读。举个例子速腾的XYZIRT格式fields 可能是这样的// 速腾 XYZIRT 的 fields 定义示意 fields[0]: namex, offset0, datatypeFLOAT32, count1 fields[1]: namey, offset4, datatypeFLOAT32, count1 fields[2]: namez, offset8, datatypeFLOAT32, count1 fields[3]: nameintensity, offset12, datatypeFLOAT32, count1 fields[4]: namering, offset16, datatypeUINT16, count1 fields[5]: nametimestamp, offset18, datatypeFLOAT32, count1注意 ring 是 UINT16 类型占 2 个字节timestamp 的 offset 是 18 而不是 20因为前面 ring 只占了 2 字节。如果你按 4 字节对齐去读timestamp 就会读错。这种细节在官方文档里往往不会强调但不注意就会导致数据错乱。禾赛的 fields 定义又不同timestamp 是 FLOAT64offset 也不一样。所以解析代码必须动态读取 fields 数组根据 name 找到对应的 offset 和 datatype而不是硬编码偏移量。2.3 时间戳的归一化处理时间戳的处理是转换中最容易出错的地方。速腾的相对时间偏移和禾赛的绝对时间戳需要统一成同一种语义。我的做法是不管原始 timestamp 是什么类型和语义统一转换成相对于该帧 header.stamp 的偏移量。对于速腾的相对时间偏移如果它本身就是相对于帧起始时刻的那直接用就行。但要注意单位有的驱动输出的是秒有的是毫秒还有的是微秒。这个必须查驱动文档或者实际打印几个值来判断。我遇到过一种情况速腾某版本的驱动输出的 timestamp 单位是毫秒但字段类型是 FLOAT32值域在 0 到 100 之间如果不注意直接当秒用去畸变就会完全失效。对于禾赛的绝对时间戳需要减去该帧的 header.stamp 才能得到相对偏移。这里有个精度问题禾赛的 timestamp 是 FLOAT64header.stamp 是 ROS 的 time 类型sec nsec做减法时要注意精度损失。我的做法是先把两者都转成微秒级的整数再相减最后转回秒的浮点数这样精度损失最小。// 禾赛绝对时间戳转相对偏移的示例 double abs_time point_timestamp; // FLOAT64 绝对时间单位秒 double frame_time msg-header.stamp.toSec(); float relative_time static_castfloat(abs_time - frame_time); // 注意如果精度不够可以改用微秒整数运算还有一个坑如果点云的 header.stamp 设置得不准确比如用了接收时刻而不是采集时刻那即使逐点 timestamp 是对的相对偏移也会整体偏移。所以最好确认驱动是否正确设置了 header.stamp。2.4 组装新的 PointCloud2 消息解析和转换完每个点的数据之后需要重新组装成一个sensor_msgs/PointCloud2消息。这一步的关键是正确设置 fields、point_step、row_step 和 data 数组。point_step 是每个点占用的字节数必须等于所有字段大小之和并且要考虑内存对齐。比如x, y, z, intensity都是 FLOAT32各 4 字节ring 是 UINT162 字节time 是 FLOAT324 字节那 point_step 就是 444424 22 字节。但有些框架或库可能要求 4 字节对齐那就需要 padding 到 24 字节。这个要看目标框架的具体要求LIO-SAM 和 FAST-LIO2 一般对 padding 不敏感但为了性能建议对齐。// 组装 PointCloud2 的关键代码片段 sensor_msgs::PointCloud2 output; output.header input-header; output.height 1; output.width num_points; output.is_bigendian false; output.is_dense false; // 定义字段 sensor_msgs::PointField field; field.name x; field.offset 0; field.datatype sensor_msgs::PointField::FLOAT32; field.count 1; output.fields.push_back(field); // ... 依次添加 y, z, intensity, ring, time output.point_step 22; // 或 24对齐后 output.row_step output.point_step * output.width; output.data.resize(output.row_step * output.height); // 逐点写入 for (size_t i 0; i num_points; i) { float* ptr reinterpret_castfloat*(output.data[i * output.point_step]); ptr[0] x; ptr[1] y; ptr[2] z; ptr[3] intensity; uint16_t* ring_ptr reinterpret_castuint16_t*(output.data[i * output.point_step 16]); *ring_ptr ring; float* time_ptr reinterpret_castfloat*(output.data[i * output.point_step 18]); *time_ptr relative_time; }这段代码看起来简单但实际写的时候offset 的计算必须和 fields 里定义的一致否则数据就会错位。我建议在写完之后用pcl::fromROSMsg把转换后的点云转成 PCL 格式打印几个点的值和原始点云对比确认转换正确。3. 适配 LIO-SAM 的完整操作链路LIO-SAM 是我个人比较喜欢的一个框架它的模块化设计很清晰建图效果也稳定。但它对输入数据的要求比较严格格式不对就直接罢工。下面把适配 LIO-SAM 的完整链路讲清楚。3.1 LIO-SAM 的输入接口分析LIO-SAM 的核心节点是imageProjection它订阅的点云 topic 默认是/points_raw消息类型是sensor_msgs/PointCloud2。在imageProjection.cpp里它通过pcl::fromROSMsg把点云转成pcl::PointXYZI然后做去畸变和特征提取。关键来了LIO-SAM 在去畸变时需要每个点的时间信息。它读取时间的方式是查找点云中名为time或t或timestamp的字段不同版本可能不同。如果你的点云里没有这个字段或者字段名不对LIO-SAM 就会用默认值 0导致去畸变失效。另外LIO-SAM 的特征提取依赖 ring 字段来区分不同线束。它在代码里通过ring字段来判断点的线束编号进而提取边缘点和平面点。如果你的点云没有 ring 字段特征提取会退化成把所有点当成同一线束处理建图精度会明显下降。3.2 转换节点的参数配置我写了一个通用的转换节点通过参数来适配不同的雷达和框架。对于 LIO-SAM关键参数如下# 转换节点参数配置适配 LIO-SAM input_topic: /rslidar_points # 原始点云 topic output_topic: /points_raw # LIO-SAM 订阅的 topic input_format: velodyne # 输入格式velodyne / hesai / robosense output_format: lio_sam # 输出格式lio_sam / fast_lio time_field_name: time # 输出点云的时间字段名 ring_start_index: 0 # ring 起始编号 filter_invalid_points: true # 是否滤除无效点这里input_format决定了用哪套解析逻辑output_format决定了输出点云的字段布局。对于 LIO-SAM输出点云的字段名建议用time因为 LIO-SAM 的代码里默认查找这个名称。ring_start_index这个参数容易被忽略。如果你的速腾雷达 ring 从 1 开始编号而 LIO-SAM 期望从 0 开始那就需要在这里做偏移。我遇到过一台 RS-16ring 从 1 到 16直接喂给 LIO-SAM 后特征提取把第 16 线的点当成了不存在的线束导致边缘点提取异常。后来把 ring 统一减 1 就正常了。3.3 实测中的建图效果对比为了验证转换的效果我用同一段数据做了对比测试一段是速腾 RS-16 采集的园区道路数据分别用原始点云未转换和转换后的点云喂给 LIO-SAM其他配置完全一致。未转换的原始点云LIO-SAM 能跑起来但建图结果在转弯处有明显的重影回环检测也经常失败。分析原因是去畸变没有生效因为原始点云的 timestamp 字段名是timestamp而 LIO-SAM 找的是time导致时间信息丢失。转换后的点云建图轨迹平滑回环检测成功率明显提升。特别是在快速转弯的路段重影基本消失。这说明逐点时间戳对 LIO-SAM 的去畸变确实至关重要。还有一个细节LIO-SAM 对点云的密度比较敏感。如果点云太密比如禾赛 AT128 的原始点云LIO-SAM 的处理速度会明显下降甚至丢帧。我的做法是在转换节点里加一个降采样用体素滤波把点云降到合适的密度。体素大小一般设 0.2 到 0.5 米具体看场景。园区道路用 0.3 米效果不错室内场景可以小一点。4. 适配 FAST-LIO2 的差异化处理FAST-LIO2 和 LIO-SAM 虽然都是激光惯性里程计但它们对输入数据的处理方式差别不小。FAST-LIO2 基于 ikd-Tree 做增量式地图管理对点云的实时性和密度要求更高格式转换时需要注意的点也不一样。4.1 FAST-LIO2 对时间戳的特殊要求FAST-LIO2 在去畸变时对时间戳的精度要求比 LIO-SAM 更高。它假设每个点的时间戳是相对于该帧起始时刻的偏移量单位秒类型可以是 FLOAT32 或 FLOAT64。如果你的时间戳精度不够比如用 FLOAT32 表示绝对时间去畸变就会产生误差。我在适配禾赛 AT128 时遇到过这个问题禾赛的原始 timestamp 是 FLOAT64 绝对时间我一开始直接转成 FLOAT32 相对偏移结果在高速运动时建图出现轻微飘移。后来改成先用 FLOAT64 计算相对偏移再转成 FLOAT32 输出精度就够了。原因是 FLOAT32 在表示较大的绝对时间时有效位数不够但表示小范围的相对偏移时精度是足够的。另外FAST-LIO2 对 header.stamp 的准确性要求也高。它用 header.stamp 来做 IMU 和激光的时间对齐。如果 header.stamp 偏差超过几毫秒IMU 预积分就会和激光点云对不上导致建图飘移。所以转换节点里最好不要修改 header.stamp直接透传原始值。4.2 ring 字段在 FAST-LIO2 中的处理策略FAST-LIO2 本身对 ring 字段的依赖没有 LIO-SAM 那么强它的特征提取主要基于局部几何特征而不是线束编号。但这不意味着 ring 可以随便处理。如果你的点云里 ring 字段的值域很大比如禾赛 AT128 的 ring 从 0 到 127而 FAST-LIO2 的某些配置里对 ring 的范围有限制就可能导致问题。我一般建议在转换时把 ring 归一化到合理的范围或者直接保留原始值但确保类型正确UINT16 足够表示大多数雷达的线束数。还有一个实际经验FAST-LIO2 在处理没有 ring 字段的点云时会自动把所有点的 ring 设为 0。这本身不会导致报错但如果你后续要做基于线束的点云分割比如提取地面点没有 ring 就会很麻烦。所以即使 FAST-LIO2 不强制要求 ring我也建议在转换时保留这个字段。4.3 点云降采样与无效点滤除的实操参数FAST-LIO2 对点云密度比 LIO-SAM 更敏感。点云太密ikd-Tree 的构建和查询会变慢实时性下降点云太稀特征提取不够建图精度下降。所以降采样是必须的。我的做法是在转换节点里做两级处理先用距离滤波去掉太近和太远的点比如小于 0.5 米和大于 100 米的点再用体素滤波降采样。体素大小根据雷达型号和场景调整雷达型号建议体素大小适用场景速腾 RS-160.2 - 0.3 米园区、室内速腾 RS-320.3 - 0.4 米园区、城市道路禾赛 Pandar400.3 - 0.5 米城市道路、高速禾赛 AT1280.4 - 0.6 米城市道路、高速无效点的滤除也很关键。禾赛的点云里经常有距离为 0 或 NaN 的点这些点如果不滤掉进入 FAST-LIO2 后会污染 ikd-Tree导致最近邻搜索返回错误结果。滤除的方法很简单在逐点转换时判断距离是否有效无效就跳过。// 无效点滤除示例 float range std::sqrt(x*x y*y z*z); if (range min_range || range max_range || std::isnan(x) || std::isnan(y) || std::isnan(z)) { continue; // 跳过无效点 }这里min_range一般设 0.5 米max_range根据雷达型号设 50 到 200 米不等。太近的点可能是雷达自身的噪声太远的点信噪比低都不适合用于建图。5. 踩过的坑与排查思路这一部分是我觉得最有价值的内容因为这些都是文档里不会写、只有实际跑过才会遇到的问题。每个坑我都尽量还原当时的排查过程方便你遇到类似问题时参考。5.1 建图飘移的排查链路建图飘移是最常见的问题但原因可能有很多。我的一般排查顺序是先看时间戳再看外参最后看点云质量。有一次用禾赛 Pandar40 跑 FAST-LIO2建图在直道上还好一转弯就飘。我先检查了时间戳发现转换后的相对时间偏移都是对的。然后检查外参雷达和 IMU 的外参是标定过的应该没问题。最后把原始点云和转换后的点云分别可视化发现转换后的点云里有一批点的 z 坐标异常明显偏离了地面。进一步排查发现禾赛的原始点云里有一批点的距离值是负数表示无效我在转换时没有滤掉直接保留了。这些负距离的点转换后 z 坐标变成了很大的负值进入 FAST-LIO2 后被当成地面点导致地面拟合错误进而影响建图。加上无效点滤除后问题解决。这个坑的教训是不要假设原始点云都是有效的一定要做有效性检查。5.2 字段类型不匹配导致的诡异报错还有一个坑是字段类型不匹配。速腾某版本的驱动输出的 ring 字段是 UINT8 类型而我在转换代码里按 UINT16 读取结果读出来的 ring 值全是乱的。因为 UINT8 只占 1 字节我按 2 字节读把相邻的 timestamp 低字节也读进来了。这种问题的排查方法是打印原始点云的 fields 定义确认每个字段的 datatype 和 offset。不要凭经验假设不同驱动版本可能不一样。// 打印 fields 定义的调试代码 for (const auto field : msg-fields) { ROS_INFO(Field: %s, offset: %d, datatype: %d, count: %d, field.name.c_str(), field.offset, field.datatype, field.count); }这段代码在调试阶段非常有用建议在转换节点里加一个 debug 开关打开时打印 fields 信息。5.3 多雷达场景下的 ring 冲突如果你同时用两台雷达比如一台速腾加一台禾赛ring 冲突是必须处理的问题。两台雷达的 ring 都从 0 开始直接合并会导致线束编号重复特征提取时无法区分哪些点来自哪台雷达。我的做法是在转换时为不同雷达的 ring 加不同的偏移。比如速腾的 ring 保持 0 到 15禾赛的 ring 加 16变成 16 到 55。这样合并后的点云里ring 编号唯一特征提取就能正确区分。这个偏移量需要根据实际使用的雷达线束数来定确保不重叠。如果线束数很多比如两台 AT128ring 值域会很大但 UINT16 足够表示不用担心溢出。6. 转换节点的性能优化与工程化建议格式转换节点虽然逻辑不复杂但在实际部署中性能问题不容忽视。特别是高线束雷达如禾赛 AT128单帧点云可能有十几万个点如果转换效率不高会成为整个系统的瓶颈。6.1 减少内存拷贝的技巧ROS 的 PointCloud2 消息在发布和订阅时默认会做一次内存拷贝。对于大点云这个拷贝开销不小。我的做法是使用nodelet或者intra-process communication来减少拷贝。如果条件不允许至少在转换节点内部避免不必要的拷贝。具体来说解析原始点云时直接用指针读取 data 数组不要先把 data 转成 vector 再处理。组装新点云时一次性 resize 好 data 数组然后逐点写入不要频繁 push_back。// 高效的内存操作示例 output.data.resize(num_valid_points * output.point_step); uint8_t* dst output.data.data(); for (size_t i 0; i num_valid_points; i) { // 直接写入 dst避免中间拷贝 memcpy(dst, point_data, output.point_step); dst output.point_step; }6.2 多线程与流水线处理如果单线程转换跟不上雷达的出帧率可以考虑多线程。一个线程负责解析原始点云另一个线程负责组装和发布。两者之间用无锁队列或者双缓冲来传递数据。不过要注意多线程会引入线程安全问题特别是 ROS 的消息回调本身可能在不同线程里执行。我的建议是先用单线程跑确认性能瓶颈确实在转换节点上再考虑多线程优化。很多时候瓶颈其实在 SLAM 算法本身而不是转换节点。6.3 参数化配置与多雷达适配最后一点工程化建议把转换节点做成高度参数化的通过 YAML 配置文件来适配不同的雷达和框架。这样换雷达或者换框架时只需要改配置不用重新编译代码。我目前的配置结构是这样的# 雷达配置 lidar: type: robosense # robosense / hesai model: RS-16 input_topic: /rslidar_points ring_start: 0 time_unit: seconds # seconds / milliseconds / microseconds # 输出配置 output: topic: /points_raw format: lio_sam # lio_sam / fast_lio time_field: time ring_offset: 0 # 滤波配置 filter: min_range: 0.5 max_range: 100.0 voxel_size: 0.3 filter_invalid: true这套配置在我用过的几种雷达和框架组合上都能跑通切换时只需要改几行 YAML非常方便。7. 一些实测数据与效果验证说了这么多理论最后还是得看实际效果。我用速腾 RS-16 和禾赛 Pandar40 分别采集了同一段园区道路的数据分别适配 LIO-SAM 和 FAST-LIO2做了几组对比测试。从建图轨迹的平滑度来看转换后的点云在两个框架上都明显优于未转换的原始点云。特别是在快速转弯和上下坡路段转换后的轨迹没有出现明显的跳变。从回环检测的成功率来看LIO-SAM 在转换后基本能稳定检测到回环而转换前经常失败。FAST-LIO2 本身对回环的依赖不强但转换后的建图精度也有提升。从 CPU 占用来看转换节点本身的开销大约占单核的 10% 到 20%对于高线束雷达会更高一些。如果加上降采样和滤波开销会增加到 20% 到 30%。这个开销在大多数平台上是可以接受的但如果你的计算平台资源紧张可以考虑把转换和滤波分开到不同节点或者用 GPU 加速。我个人在实际操作中的体会是格式转换这件事看起来是脏活累活但它决定了整个 SLAM 系统的上限。点云格式不对后面的算法再优秀也发挥不出来。所以花时间把这一层做扎实是非常值得的。另外不同雷达和驱动的版本差异很大遇到问题时不要凭经验假设多打印、多对比、多验证往往能更快定位问题。