简介：面向机器人感知、自动驾驶环境理解与点云处理方向的学习者，这是一份围绕 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/PointCloud2，PCL 侧用 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::PointCloud<pcl::PointXYZI>；过滤完再用pcl::toROSMsg转回去发布。发布时一定保留原始消息的header，尤其是frame_id和时间戳。很多人只改点云内容，忘了 header，结果 RViz 里看着正常，下游 TF 变换却报时间戳对不上。
索引组织是这套实现的核心。不要在处理过程中直接erase地面点，因为点云下标会变，后面再想对照原始点就麻烦了。更稳的做法是开一个std::vector<bool> 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_.param<std::string>("points_in", points_in_, "/points_raw"); pnh_.param<std::string>("points_no_ground", points_no_ground_, "/points_no_ground"); pnh_.param<std::string>("points_ground", points_ground_, "/points_ground"); sub_ = nh_.subscribe(points_in_, 1, &RayGroundFilterNode::cloudCallback, this); pub_no_ground_ = nh_.advertise<sensor_msgs::PointCloud2>(points_no_ground_, 1); pub_ground_ = nh_.advertise<sensor_msgs::PointCloud2>(points_ground_, 1); } private: void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); void filter(const pcl::PointCloud<pcl::PointXYZI>::Ptr& in, pcl::PointCloud<pcl::PointXYZI>::Ptr& ground, pcl::PointCloud<pcl::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::PointCloud<pcl::PointXYZI>::Ptr& in, pcl::PointCloud<pcl::PointXYZI>::Ptr& ground, pcl::PointCloud<pcl::PointXYZI>::Ptr& no_ground) { struct RayPoint { float r; float z; int idx; }; std::vector<std::vector<RayPoint>> rays(angular_div_); const float two_pi = 2.0f * static_cast<float>(M_PI); // 第一遍：只做极坐标分箱，不删点 for (int i = 0; i < static_cast<int>(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_cast<int>(theta / two_pi * angular_div_); ray = std::max(0, std::min(ray, angular_div_ - 1)); rays[ray].push_back({r, p.z, i}); } std::vector<bool> 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 里对应写<depend>roscpp</depend>、<depend>sensor_msgs</depend>、<depend>pcl_ros</depend>、<depend>pcl_conversions</depend>，版本号按你的工作空间规范填，不要照抄别人的。
3.4 launch 与参数文件
launch 只做两件事：加载 yaml，启动节点。参数文件把话题名和过滤参数集中管理，调参时不用重新编译。把 yaml 放在config/ray_filter.yaml，launch 放在launch/ray_filter.launch，包结构保持干净，后面换场景只需要换 yaml。
<launch> <node pkg="ray_ground_filter_demo" type="ray_ground_filter_node" name="ray_ground_filter_node" output="screen"> <rosparam command="load" file="$(find ray_ground_filter_demo)/config/ray_filter.yaml" /> </node> </launch>points_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 from="points_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_link，sensor_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 看地面
本文还有配套的精品资源，点击获取
