毕设里写的一个点云转栅格脚本,整理成了独立的开源工具:支持 PCD、PLY、LAS/LAZ、NPY,能处理倾斜的地图和不平的地面。
FAST-LIO2 建出来的是三维点云,而 Nav2 的 map_server 要的是二维占据栅格图(.pgm 和 .yaml)。最早在毕设里写了个脚本做转换,后来觉得别的项目也用得上,就拆出来整理成了 pcd2grid。
怎么转换
做法就是按离地高度区间投影:
- 先估计地面,可以拟合一个全局平面,碰到坡道或不平地面就分块拟合局部平面,或者干脆不估计,直接用绝对高度;
- 每个栅格里,离地高度落在
[--obs-min, --obs-max]之间的点够多,就标成障碍; - 有地面点、又没被标成障碍的栅格标成空闲;
- 剩下的全部留成未知。
pcd2grid map.pcd out/office --res 0.05 --obs-min 0.1 --obs-max 0.7
只用 numpy、scipy 和 Pillow,不依赖 ROS。
做的过程中发现的问题
整理工具的时候顺便把原脚本里的几个问题全翻了出来。首先是之前的 free_thresh 一直写着 0.25,导致 map_server 会把未知区域(像素值 205)当成空闲,机器人可能往没扫到的地方规划路径,新工具里改成了 0.15。
第二个问题是点云本身可能是斜的。拿 M3DGR 的一份 FAST-LIO 地图一试,整张图歪了大约 28°,因为坐标系跟着开机时 IMU 的姿态走。后来加了 --level,先检测地面再把点云转平,顺便把变换矩阵存下来备用。
地面判定上也踩过坑。用三脚架架着雷达扫房间时,天花板的点常常比地面还密,现在直接取高度直方图里最低的一个强峰当地面。
版本
11 月 3 日发了 v0.1.0,README 里带了办公室和仓库两个例子;4 日给分块地面模式提了速,把原来每个分块都要扫一遍全部点改成先排序一次;5 日加上了高度图地面模型、连通域过滤、射线投射,还有用来评估地图质量的 pcd2grid-eval 命令。
每次发版前,我都会拿真实的 Nav2 map_server 把生成的地图加载一遍,再看一眼 --compare 生成的对比图。
封缄于 2026 年 11 月 5 日