使用 PMF 进行地面分割

src/perception/rslidar_pointcloud_merger/CMakeLists.txt

@@ -22,7 +22,7 @@ find_package(pcl_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_sensor_msgs REQUIRED)
-find_package(PCL REQUIRED COMPONENTS common io)
+find_package(PCL REQUIRED COMPONENTS common io segmentation filters)
 
 # GPU加速相关配置
 if(USE_GPU)
@@ -79,6 +79,6 @@ endif()
 
 install(TARGETS merge_two_lidars DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
 
 ament_package()

src/perception/rslidar_pointcloud_merger/config/merger_config.yaml

@@ -0,0 +1,67 @@
+lidar_merger:
+  ros__parameters:
+    # 基本参数
+    front_topic: "/rslidar_points/front_lidar"
+    rear_topic: "/rslidar_points/rear_lidar"
+    output_topic: "/rslidar_points"
+    frame_id: "rslidar"
+    queue_size: 3
+    time_tolerance: 0.1
+    enable_debug: false
+
+    # 点云范围过滤
+    filter_min_x: -5.0
+    filter_max_x: 10.0
+    filter_min_y: -5.0
+    filter_max_y: 5.0
+    filter_min_z: 0.0
+    filter_max_z: 1.0
+
+    # 体素降采样
+    voxel_size: 0.1
+
+    # 离群点去除 (SOR)
+    stat_mean_k: 30
+    stat_std_thresh: 1.5
+    sor_method: 1
+    radius_search: 0.5
+    min_neighbors: 3
+
+    # 地面分割 (PMF)
+    enable_ground_segmentation: true
+    ground_max_window_size: 32
+    ground_slope: 1.0
+    ground_initial_distance: 0.3
+    ground_max_distance: 2.5
+
+    # 栅格可视化
+    grid_size: 50
+    grid_range: 15.0
+    enable_print: false
+
+    # GPU
+    use_gpu: true
+
+    # 车身过滤
+    filter_car: true
+    car_length: 1.8
+    car_width: 1.34
+    car_lidar_offset_x: 0.0
+    car_lidar_offset_y: 0.0
+
+    # 扫刷过滤
+    filter_brush: true
+    # 右扫刷
+    brush_r_min_x: 0.85
+    brush_r_max_x: 1.25
+    brush_r_min_y: 0.60
+    brush_r_max_y: 0.96
+    brush_r_min_z: 1.20
+    brush_r_max_z: 1.65
+    # 左扫刷
+    brush_l_min_x: 0.85
+    brush_l_max_x: 1.25
+    brush_l_min_y: -0.96
+    brush_l_max_y: -0.60
+    brush_l_min_z: 1.20
+    brush_l_max_z: 1.65

src/perception/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py

@@ -6,6 +6,7 @@ from pathlib import Path
 
 def generate_launch_description():
     pkg_share = get_package_share_directory("rslidar_pointcloud_merger")
+    config_file = Path(pkg_share) / "config" / "merger_config.yaml"
 
     # front_lidar（右前角） -> rslidar（车辆中心）
     # [x, y, z, yaw, pitch, roll]
@@ -37,7 +38,6 @@ def generate_launch_description():
                 executable="static_transform_publisher",
                 name="static_tf_front",
                 arguments=[*map(str, tf_front), "rslidar", "front_lidar"],
-                # arguments=[*map(str, tf_front), "rslidar", "front_lidar", "100"],
                 output="log",
             ),
             # ---------- 静态 TF (rear) ----------
@@ -46,7 +46,6 @@ def generate_launch_description():
                 executable="static_transform_publisher",
                 name="static_tf_rear",
                 arguments=[*map(str, tf_rear), "rslidar", "rear_lidar"],
-                # arguments=[*map(str, tf_rear), "rslidar", "rear_lidar", "100"],
                 output="log",
             ),
             # ---------- 点云合并节点 ----------
@@ -55,41 +54,7 @@ def generate_launch_description():
                 executable="merge_two_lidars",
                 name="lidar_merger",
                 parameters=[
-                    {
+                    config_file,
-                        "front_topic": "/rslidar_points/front_lidar",
-                        "rear_topic": "/rslidar_points/rear_lidar",
-                        "output_topic": "/rslidar_points",
-                        "frame_id": "rslidar",
-                        "queue_size": 3,
-                        "cache_size": 10,
-                        "time_tolerance": 0.1,
-                        "max_wait_time": 1.0,
-                        "enable_debug": False,
-                        # 点云处理参数
-                        "filter_min_x": -5.0,  # X轴最小坐标（米）
-                        "filter_max_x": 10.0,  # X轴最大坐标（米）
-                        "filter_min_y": -5.0,  # Y轴最小坐标（米）
-                        "filter_max_y": 5.0,  # Y轴最大坐标（米）
-                        "filter_min_z": 0.0,  # Z轴最小坐标（米）
-                        "filter_max_z": 1.0,  # Z轴最大坐标（米）1.0
-                        "voxel_size": 0.1,  # 体素网格大小（米） 0.1~0.3m
-                        "stat_mean_k": 30,  # 计算点的平均距离时考虑的邻居数
-                        "stat_std_thresh": 1.5,  # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。
-                        "grid_size": 50,  # 栅格矩阵的边长（单元格数）
-                        "grid_range": 15.0,  # 栅格覆盖的实际空间范围（米）
-                        # 单元格尺寸：由 grid_range / grid_size 决定
-                        "enable_print": False,  # 是否打印栅格
-                        "use_gpu": True,  # 是否使用GPU加速
-                        "sor_method": 1,  # 推荐先用1，效果不满意再试2
-                        "radius_search": 0.5,  # 搜索半径（米），越大越严格
-                        "min_neighbors": 3,  # 至少需要N个邻居才保留
-                        # 车身过滤参数
-                        "filter_car": True,  # 是否启用车身过滤
-                        "car_length": 1.8,  # 车长（米）
-                        "car_width": 1.34,  # 车宽（米）
-                        "car_lidar_offset_x": 0.0,  # LiDAR在x轴的安装偏移
-                        "car_lidar_offset_y": 0.0,  # LiDAR在y轴的安装偏移
-                    }
                 ],
                 output="screen",
             ),

src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp

@@ -26,11 +26,13 @@
  */
 
 #include <pcl/filters/crop_box.h>
+#include <pcl/filters/extract_indices.h>
 #include <pcl/filters/radius_outlier_removal.h>
 #include <pcl/filters/statistical_outlier_removal.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/segmentation/progressive_morphological_filter.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -211,6 +213,28 @@ class LidarMerger : public rclcpp::Node
         car_lidar_offset_x_ = declare_parameter<float>("car_lidar_offset_x", 0.0f);
         car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f);
 
+        // 扫刷过滤
+        filter_brush_ = declare_parameter<bool>("filter_brush", true);
+        brush_r_min_x_ = declare_parameter<float>("brush_r_min_x", 0.85f);
+        brush_r_max_x_ = declare_parameter<float>("brush_r_max_x", 1.25f);
+        brush_r_min_y_ = declare_parameter<float>("brush_r_min_y", 0.60f);
+        brush_r_max_y_ = declare_parameter<float>("brush_r_max_y", 0.96f);
+        brush_r_min_z_ = declare_parameter<float>("brush_r_min_z", 1.20f);
+        brush_r_max_z_ = declare_parameter<float>("brush_r_max_z", 1.65f);
+        brush_l_min_x_ = declare_parameter<float>("brush_l_min_x", 0.85f);
+        brush_l_max_x_ = declare_parameter<float>("brush_l_max_x", 1.25f);
+        brush_l_min_y_ = declare_parameter<float>("brush_l_min_y", -0.96f);
+        brush_l_max_y_ = declare_parameter<float>("brush_l_max_y", -0.60f);
+        brush_l_min_z_ = declare_parameter<float>("brush_l_min_z", 1.20f);
+        brush_l_max_z_ = declare_parameter<float>("brush_l_max_z", 1.65f);
+
+        // 地面分割
+        enable_ground_segmentation_ = declare_parameter<bool>("enable_ground_segmentation", true);
+        ground_max_window_size_ = declare_parameter<int>("ground_max_window_size", 32);
+        ground_slope_ = declare_parameter<double>("ground_slope", 1.0);
+        ground_initial_distance_ = declare_parameter<double>("ground_initial_distance", 0.3);
+        ground_max_distance_ = declare_parameter<double>("ground_max_distance", 2.5);
+
         /* ---- 预计算栅格参数 ---- */
         grid_resolution_ = grid_range_ / static_cast<float>(grid_size_);
         grid_inv_resolution_ = 1.0f / grid_resolution_;
@@ -687,9 +711,13 @@ class LidarMerger : public rclcpp::Node
             {
                 if (fx >= cmin_x && fx <= cmax_x && fy >= cmin_y && fy <= cmax_y) continue;
 
-                if (fx >= 0.85f && fx <= 1.25f && fy >= 0.60f && fy <= 0.96f && fz >= 1.20f && fz <= 1.65f) continue;
+                if (filter_brush_)
-
+                {
-                if (fx >= 0.85f && fx <= 1.25f && fy >= -0.96f && fy <= -0.60f && fz >= 1.20f && fz <= 1.65f) continue;
+                    // 右扫刷
+                    if (fx >= brush_r_min_x_ && fx <= brush_r_max_x_ && fy >= brush_r_min_y_ && fy <= brush_r_max_y_ && fz >= brush_r_min_z_ && fz <= brush_r_max_z_) continue;
+                    // 左扫刷
+                    if (fx >= brush_l_min_x_ && fx <= brush_l_max_x_ && fy >= brush_l_min_y_ && fy <= brush_l_max_y_ && fz >= brush_l_min_z_ && fz <= brush_l_max_z_) continue;
+                }
             }
 
             pcl::PointXYZ pt;
@@ -709,8 +737,35 @@ class LidarMerger : public rclcpp::Node
         vg_local.filter(*downsampled);
         if (downsampled->empty()) return nullptr;
 
-        /* 3. 离群点去除（根据 sor_method_ 选择算法） */
+        /* 3. 地面分割 (PMF) */
-        pcl_out = applyOutlierRemoval(downsampled);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr non_ground(new pcl::PointCloud<pcl::PointXYZ>);
+        if (enable_ground_segmentation_)
+        {
+            pcl::PointIndicesPtr ground_indices(new pcl::PointIndices);
+            pcl::ProgressiveMorphologicalFilter<pcl::PointXYZ> pmf;
+            pmf.setInputCloud(downsampled);
+            pmf.setMaxWindowSize(ground_max_window_size_);
+            pmf.setSlope(ground_slope_);
+            pmf.setInitialDistance(ground_initial_distance_);
+            pmf.setMaxDistance(ground_max_distance_);
+            pmf.extract(ground_indices->indices);
+
+            pcl::ExtractIndices<pcl::PointXYZ> extractor;
+            extractor.setInputCloud(downsampled);
+            extractor.setIndices(ground_indices);
+            extractor.setNegative(true);  // 提取非地面
+            extractor.filter(*non_ground);
+        }
+        else
+        {
+            // 不进行地面分割，直接使用体素化后的点云
+            non_ground = downsampled;
+        }
+
+        if (non_ground->empty()) return nullptr;
+
+        /* 4. 离群点去除（根据 sor_method_ 选择算法） */
+        pcl_out = applyOutlierRemoval(non_ground);
         if (!pcl_out || pcl_out->empty()) return nullptr;
 
         /* 4. 直接构建 PointCloud2（不走 toROSMsg，减少一次拷贝） */
@@ -1035,19 +1090,19 @@ class LidarMerger : public rclcpp::Node
             p.car_max_y = car_lidar_offset_y_ + car_width_ * 0.5f;
             p.car_min_z = filter_min_z_;
             p.car_max_z = filter_max_z_;
-            p.filter_brush = true;
+            p.filter_brush = filter_brush_;
-            p.brush_r_min_x = 0.85f;
+            p.brush_r_min_x = brush_r_min_x_;
-            p.brush_r_max_x = 1.25f;
+            p.brush_r_max_x = brush_r_max_x_;
-            p.brush_r_min_y = 0.60f;
+            p.brush_r_min_y = brush_r_min_y_;
-            p.brush_r_max_y = 0.96f;
+            p.brush_r_max_y = brush_r_max_y_;
-            p.brush_r_min_z = 1.20f;
+            p.brush_r_min_z = brush_r_min_z_;
-            p.brush_r_max_z = 1.65f;
+            p.brush_r_max_z = brush_r_max_z_;
-            p.brush_l_min_x = 0.85f;
+            p.brush_l_min_x = brush_l_min_x_;
-            p.brush_l_max_x = 1.25f;
+            p.brush_l_max_x = brush_l_max_x_;
-            p.brush_l_min_y = -0.96f;
+            p.brush_l_min_y = brush_l_min_y_;
-            p.brush_l_max_y = -0.60f;
+            p.brush_l_max_y = brush_l_max_y_;
-            p.brush_l_min_z = 1.20f;
+            p.brush_l_min_z = brush_l_min_z_;
-            p.brush_l_max_z = 1.65f;
+            p.brush_l_max_z = brush_l_max_z_;
         }
         return p;
     }
@@ -1101,12 +1156,20 @@ class LidarMerger : public rclcpp::Node
     int sor_method_, min_neighbors_;
     float radius_search_;
     double time_tolerance_;
-    bool enable_debug_, enable_print_, use_gpu_, filter_car_;
+    bool enable_debug_, enable_print_, use_gpu_, filter_car_, filter_brush_, enable_ground_segmentation_;
     float filter_min_x_, filter_max_x_, filter_min_y_, filter_max_y_;
     float filter_min_z_, filter_max_z_;
     float voxel_size_, stat_std_thresh_, grid_range_;
     float car_length_, car_width_, car_lidar_offset_x_, car_lidar_offset_y_;
     float grid_resolution_, grid_inv_resolution_;
+    // 扫刷参数
+    float brush_r_min_x_, brush_r_max_x_, brush_r_min_y_, brush_r_max_y_, brush_r_min_z_, brush_r_max_z_;
+    float brush_l_min_x_, brush_l_max_x_, brush_l_min_y_, brush_l_max_y_, brush_l_min_z_, brush_l_max_z_;
+    // 地面分割参数 (PMF)
+    int ground_max_window_size_;
+    double ground_slope_;
+    double ground_initial_distance_;
+    double ground_max_distance_;
 
     /* -- PCL 过滤器（仅作为模板，处理线程使用线程本地副本） -- */
     pcl::VoxelGrid<pcl::PointXYZ> vg_;