简介面向机器人感知、自动驾驶环境理解与点云处理方向的学习者这是一份围绕 Ray Filter 地面过滤搭建的 ROS 节点工程核心用途是对 LiDAR 点云去除平坦地面点、保留建筑与树木等非地面特征适合已具备 ROS 与 PCL 基础、希望理解地面分割实现并快速复现实验的开发者。压缩包共 8 个文件整体约 4KB包含 cpp 源码、头文件、launch 启动配置、xml 与 json 工程配置以及 txt 说明等覆盖节点核心逻辑、编译组织与运行配置体量轻便但结构完整。已有 7714 人学习说明其在雷达点云地面过滤主题上具有较好的参考价值。读者可据此观察 Ray Filter 在 ROS 节点中的工程落地方式结合源码与配置梳理类结构、参数设置和数据处理链路并用于课程实验、项目原型验证、算法调参对比或机器人三维环境感知方案的入门借鉴。1. 地面点不先剔掉聚类和障碍物检测都会跟着“粘锅”基于 ray filter 的雷达点云地面过滤 ROS 节点处理的是室外激光雷达扫描里最烦人的那层地面点。它订阅 PointCloud2把每帧点云按射线和半径关系拆成地面与非地面两路再交给 PCL 做后续聚类或障碍物检测。适合正在搭感知预处理链路的人手里有 PCL 和 ROS 的基础能跑 bag但被路面残留、路沿误删、坡度变化折腾过。它不替代分割网络胜在轻量、可解释、参数能落地。2. ray filter 的地面判定从射线斜率到 PCL 点云索引室外激光雷达一帧点云里地面点经常占到一半以上。直接上欧式聚类路面会把相邻车辆、路沿和锥桶连成一片先做地面过滤后面的分割和跟踪才有稳定的输入。ray filter 的思路不复杂同一角度射线上地面点随半径连续变化障碍物则会突然抬高。把这种连续性写成斜率判断再用 PCL 的点云容器和索引组织起来就能做成一个可调、可回放的 ROS 节点。2.1 为什么先看射线而不是先套高度阈值高度阈值是最容易上手的做法传感器装高一点把 z 小于某个值的点全判成地面。短距离、平路、传感器水平安装时它确实能跑一旦遇到上坡、下坡、车辆俯仰或悬挂变化固定高度就会翻车。上坡时远处地面 z 抬高固定阈值会把地面留成一条带下坡时地面 z 降低阈值又会把低矮障碍物误删。 ray filter 不跟绝对高度较劲它看的是同一条射线上相邻地面点的高度变化是否连续。更具体地说激光雷达的扫描线在极坐标里天然有角度和半径两个维度。同一个方位角附近地面点从近到远的高度变化通常平缓车辆、路沿、行人会在某个半径处突然产生较大的高度差。把每个点按方位角分到不同射线再按半径排序地面点就变成一条可以逐点追踪的曲线。这个曲线不需要全局平面拟合局部斜率就能表达坡度。对室外低速场景这种局部判断比一次性 RANSAC 平面更稳因为路面很少是单一平面。PCL 在这里的角色不是“算法魔法”而是把点云容器、索引和输出通道管好。ROS 收到的是 sensor_msgs/PointCloud2PCL 侧用 pcl::PointCloud pcl::PointXYZI 承接过滤时用一个与输入点一一对应的索引数组标记地面点最后再拆成两路点云发布。这样地面点和非地面点都保留原始坐标和强度后续想复查也方便。索引数组比反复 erase 点云更安全避免边遍历边删点导致的迭代器错乱。2.2 极坐标分箱角度扇区决定“射线”有多粗ray filter 的第一刀是角度分箱。对每个点计算theta atan2(y, x)把[0, 2π)切成angular_div份每份就是一个角度扇区。angular_div越大单条射线越窄地面连续性判断越细但空射线也越多排序和遍历的固定开销会上来。常见起点是 3600 份相当于 0.1 度一格雷达线束稀疏或点云已经降采样时720 到 1440 份也够用。这个参数不是越大越好得看点云密度和水平分辨率。分箱之后每个扇区内按半径r sqrt(x*x y*y)排序。排序的目的不是好看而是让相邻点在地面高度上可以比较。没有排序同一个扇区里的远近点混在一起斜率计算就没有物理意义。代码里一般用struct RayPoint { float r; float z; int idx; }保存半径、高度和原始索引排序只动这个小结构最后再按索引回填地面标记。这样比直接排序整个点云轻也避免丢失强度、ring 等字段。角度归一化有一个小边界atan2返回[-π, π]负角度要加上2π再分箱。靠近2π和0的点其实在空间上相邻但会被分到首尾两个扇区。如果雷达正前方正好落在边界上可能出现一条细缝。常见处理是把首尾扇区合并或者把角度偏移半个扇区再分箱。这个细节不影响大部分场景但在调试地面残留时值得看一眼。参数作用调大后的效果调小后的效果angular_div方位角分箱数射线更细边界更敏感开销增加射线更粗地面更连续细节容易混min_range最小处理半径排除近处车体遮挡近处地面参与判断可能被车体干扰max_range最大处理半径远处地面参与判断噪声也进来远处不处理适合局部感知2.3 地面种子和两级斜率local_slope 与 general_slope每条射线开始过滤时得先找到一个可信的地面种子。常见做法是用传感器安装高度做初始判断点云在传感器坐标系下地面高度大约在-sensor_height附近如果某个点的|z sensor_height|小于initial_distance就把它当作该射线的第一个地面点。种子找错后面整条射线都会歪所以initial_distance不能给得太抠尤其在车辆载重变化或标定有偏差时。找到种子后逐点比较当前点与上一个地面点的高度差和半径差。local_slope管局部坡度适合平路和缓坡general_slope管更宽松的连续性用来兜住路面起伏。两级判断可以这样写先看高度差是否小于local_height_threshold且斜率小于local_slope满足就判地面否则再看高度差是否小于max_height_threshold且斜率小于general_slope。第一级收紧减少路沿误删第二级放宽减少上坡地面残留。这里最容易犯的错是把local_slope当成“地面坡度”直接给一个很大的值。斜率阈值本质上是“相邻地面点允许的高度变化率”不是道路设计坡度。给大了车辆保险杠、路沿、锥桶底部会被吸进地面给小了减速带和坑洼会被留成障碍物。我一般会先把general_slope放在 0.08 到 0.12 附近再把local_slope放到 0.15 左右用一段有坡度的 bag 反复看 ground 话题确认路沿没有被大段吃掉。2.4 PCL 与 ROS 的分工点云容器、索引和话题契约这份节点里ROS 负责输入输出和参数PCL 负责点云数据结构和后续可插拔的滤波。订阅端拿到sensor_msgs::PointCloud2后用pcl::fromROSMsg转成pcl::PointCloudpcl::PointXYZI过滤完再用pcl::toROSMsg转回去发布。发布时一定保留原始消息的header尤其是frame_id和时间戳。很多人只改点云内容忘了 header结果 RViz 里看着正常下游 TF 变换却报时间戳对不上。索引组织是这套实现的核心。不要在处理过程中直接erase地面点因为点云下标会变后面再想对照原始点就麻烦了。更稳的做法是开一个std::vectorbool is_ground(input-size(), false)遍历时只写标记最后一次性拆成ground和no_ground两路。这样地面点也能发布出来调试时直接看/points_ground是否把路沿吃进去比只看过滤后的障碍物点云直观得多。PCL 的VoxelGrid可以放在过滤前也可以放在过滤后。放在过滤前能降点数但会改变射线上的点密度斜率判断的相邻关系会变放在过滤后对 ray filter 本身影响小但下游聚类仍然吃大点云。我的习惯是ray filter 先吃原始点云输出非地面点后再做体素降采样地面点单独保留低分辨率版本用于可视化。这样调参时不会被降采样引入的“断线”误导性能也能接受。2.5 选型边界ray filter 不是万能地面分割ray filter 适合激光雷达扫描线清晰、地面连续、坡度变化不算剧烈的室外场景。它对路沿、减速带、车辆底部比较敏感参数调得好能兼顾地面残留和误删调不好就会在“地面留一条带”和“路沿被吃掉”之间来回摆。固定高度阈值更轻但面对坡道和俯仰几乎没法用RANSAC 平面拟合适合大面积平整地面遇到连续起伏和多个平面时容易只拟合到其中一块形态学方法对参数和网格分辨率依赖更重调参成本不低。如果场景里有大量非地面连续结构比如长坡道旁的挡墙、连续路沿、近处车体ray filter 只能作为第一道粗过滤。后面仍然需要聚类、分类或占用栅格来兜底。把 ray filter 当成“把地面点从点云里剥掉”的预处理而不是“理解整个场景”的模块选型和调参都会更务实。PCL 和 ROS 的组合优势也在这里节点可以独立跑 bag参数可以落 yaml输出两路话题方便和后面的聚类节点拼成链路。3. ROS 节点落地订阅、滤波、发布与参数服务器把算法写成节点难点不在公式而在话题契约和参数入口。输入输出名称、坐标系、时间戳、参数命名一旦定下来后面调参和回放才不会乱。下面这套骨架用 ROS1 的 catkin 工作空间举例ROS2 里把节点句柄和发布订阅接口换掉即可PCL 侧的数据结构和过滤逻辑可以保留。包名用ray_ground_filter_demo节点名用ray_ground_filter_node避免和已有包重名。3.1 节点骨架与话题契约先定三个话题输入/points_raw非地面输出/points_no_ground地面输出/points_ground。输入输出都用sensor_msgs/PointCloud2节点内部转 PCL。参数从私有命名空间读取launch 里加载 yaml。这样同一个节点可以起多份分别吃不同雷达或不同区域不会互相覆盖参数。#include ros/ros.h #include sensor_msgs/PointCloud2.h #include pcl_conversions/pcl_conversions.h #include pcl/point_cloud.h #include pcl/point_types.h #include vector #include cmath #include algorithm class RayGroundFilterNode { public: RayGroundFilterNode() : nh_(), pnh_(~) { // 话题名走参数方便同一节点多实例 pnh_.paramstd::string(points_in, points_in_, /points_raw); pnh_.paramstd::string(points_no_ground, points_no_ground_, /points_no_ground); pnh_.paramstd::string(points_ground, points_ground_, /points_ground); sub_ nh_.subscribe(points_in_, 1, RayGroundFilterNode::cloudCallback, this); pub_no_ground_ nh_.advertisesensor_msgs::PointCloud2(points_no_ground_, 1); pub_ground_ nh_.advertisesensor_msgs::PointCloud2(points_ground_, 1); } private: void cloudCallback(const sensor_msgs::PointCloud2ConstPtr msg); void filter(const pcl::PointCloudpcl::PointXYZI::Ptr in, pcl::PointCloudpcl::PointXYZI::Ptr ground, pcl::PointCloudpcl::PointXYZI::Ptr no_ground); ros::NodeHandle nh_; ros::NodeHandle pnh_; ros::Subscriber sub_; ros::Publisher pub_no_ground_; ros::Publisher pub_ground_; std::string points_in_, points_no_ground_, points_ground_; double sensor_height_ 1.8; double min_range_ 1.0; double max_range_ 80.0; int angular_div_ 3600; double initial_distance_ 0.25; double local_slope_ 0.15; double general_slope_ 0.08; double local_height_threshold_ 0.20; double max_height_threshold_ 1.20; };这段骨架只做三件事读参数、订阅输入、准备两个发布器。参数默认值只是起点真正调参靠 yaml 覆盖。pnh_(~)让参数落在节点私有命名空间launch 里用rosparam加载时不会和全局参数打架。订阅队列设成 1是因为地面过滤要跟实时帧积压旧帧反而增加延迟如果做离线回放可以按需要改成 2 到 4但不要太大。3.2 核心过滤函数核心函数先把输入点按角度分到射线再在每条射线内按半径排序最后用种子和两级斜率标记地面。地面和非地面都保留原始点输出前分别设置width、height和is_dense。这样 RViz 和下游 PCL 节点都能正常读取。void RayGroundFilterNode::filter( const pcl::PointCloudpcl::PointXYZI::Ptr in, pcl::PointCloudpcl::PointXYZI::Ptr ground, pcl::PointCloudpcl::PointXYZI::Ptr no_ground) { struct RayPoint { float r; float z; int idx; }; std::vectorstd::vectorRayPoint rays(angular_div_); const float two_pi 2.0f * static_castfloat(M_PI); // 第一遍只做极坐标分箱不删点 for (int i 0; i static_castint(in-size()); i) { const auto p in-points[i]; if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue; float r std::sqrt(p.x * p.x p.y * p.y); if (r min_range_ || r max_range_) continue; float theta std::atan2(p.y, p.x); if (theta 0.0f) theta two_pi; int ray static_castint(theta / two_pi * angular_div_); ray std::max(0, std::min(ray, angular_div_ - 1)); rays[ray].push_back({r, p.z, i}); } std::vectorbool is_ground(in-size(), false); // 第二遍每条射线按半径排序追踪地面高度 for (auto ray : rays) { if (ray.empty()) continue; std::sort(ray.begin(), ray.end(), [](const RayPoint a, const RayPoint b) { return a.r b.r; }); bool has_seed false; float last_ground_r 0.0f; float last_ground_z 0.0f; for (const auto rp : ray) { bool ground_flag false; if (!has_seed) { // 用传感器安装高度找第一条射线的地面种子 if (std::abs(rp.z sensor_height_) initial_distance_) { ground_flag true; has_seed true; } } else { float dr rp.r - last_ground_r; if (dr 1e-3f) { float slope (rp.z - last_ground_z) / dr; float dz std::abs(rp.z - last_ground_z); // 先收紧局部阈值再用全局阈值兜底 if (dz local_height_threshold_ std::abs(slope) local_slope_) { ground_flag true; } else if (dz max_height_threshold_ std::abs(slope) general_slope_) { ground_flag true; } } } if (ground_flag) { is_ground[rp.idx] true; last_ground_r rp.r; last_ground_z rp.z; } } } // 第三遍按索引拆成两路避免边遍历边删点 for (size_t i 0; i in-size(); i) { if (is_ground[i]) ground-push_back(in-points[i]); else no_ground-push_back(in-points[i]); } ground-width ground-size(); ground-height 1; ground-is_dense false; no_ground-width no_ground-size(); no_ground-height 1; no_ground-is_dense false; }这段代码的关键在三个“不”不在遍历时删点不在分箱时排序整个点云不把地面点丢掉。RayPoint只存半径、高度和原始索引排序成本低is_ground与输入点一一对应最后统一拆分。dr 1e-3f是防止两个点半径几乎相同导致斜率爆炸室外点云里这种近邻点不少少了这个保护会出现偶发的地面误判。参数含义要对着代码看sensor_height_用于找种子不是用来做全局高度裁剪initial_distance_是种子容差太小会找不到地面太大会把低矮障碍物当种子local_height_threshold_和local_slope_管第一级判断max_height_threshold_和general_slope_管兜底。调参时不要同时改四个阈值先固定general_slope_再调local_slope_最后看高度阈值。3.3 CMake 与依赖CMake 里把roscpp、sensor_msgs、pcl_ros、pcl_conversions和 PCL 的 common/io/filters 组件找齐。pcl_ros负责 ROS 与 PCL 的桥接pcl_conversions提供消息转换。链接时把 catkin 和 PCL 的库都带上避免运行时报 undefined reference。cmake_minimum_required(VERSION 3.0.2) project(ray_ground_filter_demo) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs pcl_ros pcl_conversions ) find_package(PCL REQUIRED COMPONENTS common io filters) catkin_package( CATKIN_DEPENDS roscpp sensor_msgs pcl_ros pcl_conversions DEPENDS PCL ) include_directories( ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) add_executable(ray_ground_filter_node src/ray_ground_filter_node.cpp) target_link_libraries(ray_ground_filter_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} )CMake 这段没有花活重点是依赖别漏。pcl_ros和pcl_conversions经常被只写一个结果编译能过、链接报错。catkin_package里声明依赖是为了让下游包依赖这个节点时能正确传递头文件和库。PCL的组件按需找不要一口气把所有组件都拉进来编译时间会明显变长。package.xml 里对应写dependroscpp/depend、dependsensor_msgs/depend、dependpcl_ros/depend、dependpcl_conversions/depend版本号按你的工作空间规范填不要照抄别人的。3.4 launch 与参数文件launch 只做两件事加载 yaml启动节点。参数文件把话题名和过滤参数集中管理调参时不用重新编译。把 yaml 放在config/ray_filter.yamllaunch 放在launch/ray_filter.launch包结构保持干净后面换场景只需要换 yaml。launch node pkgray_ground_filter_demo typeray_ground_filter_node nameray_ground_filter_node outputscreen rosparam commandload file$(find ray_ground_filter_demo)/config/ray_filter.yaml / /node /launchpoints_in: /points_raw points_no_ground: /points_no_ground points_ground: /points_ground sensor_height: 1.8 min_range: 1.0 max_range: 80.0 angular_div: 3600 initial_distance: 0.25 local_slope: 0.15 general_slope: 0.08 local_height_threshold: 0.20 max_height_threshold: 1.20launch 里没有 remap因为话题名直接从 yaml 读。这样做的好处是同一份 launch 可以配多份 yaml起多个节点实例坏处是话题名改起来不如 remap 直观。实际项目里如果输入话题经常变可以保留 yaml 里的默认值再在 launch 里加remap frompoints_in to$(arg points_topic) /但要注意节点里订阅的是参数值还是相对话题名别让 remap 和参数打架。参数文件里的数值只是起手式。sensor_height必须和实际安装高度一致点云如果在base_link而不是传感器坐标系这个值要重新折算angular_div先按雷达水平分辨率给3600 不是硬标准initial_distance从 0.25 开始地面种子找不到就加到 0.35 到 0.4 再试。改完 yaml 不需要编译重启节点或者rosparam load后重新订阅即可。4. 参数标定与点云链路角度阈值、传感器高度和坐标系ray filter 的效果七成看参数有没有跟场景对上。同一套代码sensor_height差 20 厘米、local_slope差 0.05地面残留和路沿误删就会换一副面孔。调参不要从“看起来差不多”开始先把坐标系和安装高度钉死再用地面样本估斜率最后才动角度分箱和距离范围。下面这些步骤我一般会在新场景里按顺序走一遍。4.1 先定坐标系sensor frame 还是 base_link代码里的种子判断默认点云在传感器坐标系下地面高度约等于-sensor_height。如果输入点云已经被转换到base_link地面高度接近 0这时还把sensor_height设成 1.8种子就会找不到表现是地面点大量残留。先确认points_raw的frame_id再用 TF 看雷达到底盘的变换别靠猜。# 看输入点云的坐标系 rostopic echo -n1 /points_raw/header # 看雷达到 base_link 的变换 rosrun tf tf_echo base_link lidar_link第一条命令看frame_id第二条看平移的 z 分量。如果点云在lidar_linksensor_height填雷达到地面的垂直距离如果点云在base_link要么把sensor_height改成 0 附近要么在节点里先做一次 TF 变换再过滤。两种做法都能用但不要混着来点云在底盘坐标系、参数却按传感器坐标系填种子判断会整体偏移。提示调参前先固定一帧点云用 RViz 看/points_ground。地面点应该连续覆盖路面路沿和车辆底部不应大段进入地面话题。只看/points_no_ground容易把误删当成“过滤干净”。4.2 用地面样本估斜率local_slope和general_slope不要拍脑袋。把一段平路、一段上坡、一段下坡的 bag 各截几帧导出地面点样本按半径排序后算相邻高度差与半径差的比值看 90 分位和 95 分位。地面样本的斜率分布会告诉你阈值该放在哪尤其是坡道场景固定一个 0.1 往往不够。import numpy as np # CSV 三列x,y,z来自人工截取的地面点 p np.loadtxt(ground_samples.csv, delimiter,) r np.hypot(p[:, 0], p[:, 1]) z p[:, 2] order np.argsort(r) r r[order] z z[order] # 相邻地面点的高度变化率 dr np.maximum(np.diff(r), 1e-3) slope np.abs(np.diff(z) / dr) print(P50/P90/P95/P99 slope:, np.percentile(slope, [50, 90, 95, 99]))这段脚本只做一件事把地面样本按半径排序统计相邻点斜率的分位数。P90附近可以作为general_slope的起点P95到P99用来判断坡道和坑洼会不会被误删。样本里不要混入路沿和车辆底部否则分位数会被拉高。dr加1e-3是防止重复半径点导致除零和节点里的保护逻辑保持一致。如果样本里上坡和下坡都有分开统计不要混成一个分布。上坡的斜率符号和下坡相反取绝对值后分布会变宽节点里用的是std::abs(slope)所以阈值要覆盖最陡的那段但又不能大到把路沿吃进去。我的习惯是先把general_slope定在P95附近再用local_slope收紧到P90附近跑 bag 看地面本文还有配套的精品资源点击获取