lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

使用 PMF 进行地面分割

Browse Source
This commit is contained in:
Alvin-lyq 2026-03-24 13:21:06 +08:00
parent 9ba7ef2c5f
commit 1ab85c336a
4 changed files with 153 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/perception/rslidar_pointcloud_merger/CMakeLists.txt
View File

@ -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()

67
src/perception/rslidar_pointcloud_merger/config/merger_config.yaml Normal file
View File

@ -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

39
src/perception/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py
View File

@ -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=[
{
"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轴的安装偏移
}
config_file,
],
output="screen",
),

101
src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp
View File

@ -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 (fx >= 0.85f && fx <= 1.25f && fy >= -0.96f && fy <= -0.60f && fz >= 1.20f && fz <= 1.65f) continue;
if (filter_brush_)
{
// 右扫刷
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_ 选择算法） */
pcl_out = applyOutlierRemoval(downsampled);
/* 3. 地面分割 (PMF) */
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.brush_r_min_x = 0.85f;
p.brush_r_max_x = 1.25f;
p.brush_r_min_y = 0.60f;
p.brush_r_max_y = 0.96f;
p.brush_r_min_z = 1.20f;
p.brush_r_max_z = 1.65f;
p.brush_l_min_x = 0.85f;
p.brush_l_max_x = 1.25f;
p.brush_l_min_y = -0.96f;
p.brush_l_max_y = -0.60f;
p.brush_l_min_z = 1.20f;
p.brush_l_max_z = 1.65f;
p.filter_brush = filter_brush_;
p.brush_r_min_x = brush_r_min_x_;
p.brush_r_max_x = brush_r_max_x_;
p.brush_r_min_y = brush_r_min_y_;
p.brush_r_max_y = brush_r_max_y_;
p.brush_r_min_z = brush_r_min_z_;
p.brush_r_max_z = brush_r_max_z_;
p.brush_l_min_x = brush_l_min_x_;
p.brush_l_max_x = brush_l_max_x_;
p.brush_l_min_y = brush_l_min_y_;
p.brush_l_max_y = brush_l_max_y_;
p.brush_l_min_z = brush_l_min_z_;
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_;