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

提升雷达数据处理性能

Browse Source
This commit is contained in:
lyq 2026-03-09 13:39:01 +08:00
parent 1960fc8805
commit 7d3ecde581
1 changed files with 116 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,6 +27,7 @@ struct CloudFrame
{
std::shared_ptr<PointCloud2> cloud; // 原始点云
std::shared_ptr<PointCloud2> processed_cloud; // 处理后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr processed_pcl; // 处理后的PCL点云（用于栅格化）
rclcpp::Time stamp;
rclcpp::Time received_time; // 添加接收时间用于延时分析
std::string source;
@ -109,6 +110,43 @@ class LidarMerger : public rclcpp::Node
car_lidar_offset_x_ = declare_parameter<float>("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移
car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移
// 初始化过滤器对象（避免每次重新创建）
// 配置体素网格过滤器
vg_.setLeafSize(voxel_size_, voxel_size_, voxel_size_);
// 配置统计离群点去除器
sor_.setMeanK(stat_mean_k_);
sor_.setStddevMulThresh(stat_std_thresh_);
// 配置车身过滤
if (filter_car_)
{
// 车身区域
Eigen::Vector4f car_min(car_lidar_offset_x_ - car_length_ / 2.0f,
car_lidar_offset_y_ - car_width_ / 2.0f,
filter_min_z_, 1.0);
Eigen::Vector4f car_max(car_lidar_offset_x_ + car_length_ / 2.0f,
car_lidar_offset_y_ + car_width_ / 2.0f,
filter_max_z_, 1.0);
car_crop_.setMin(car_min);
car_crop_.setMax(car_max);
car_crop_.setNegative(true);
// 右侧主刷
Eigen::Vector4f right_brush_min(0.85f, 0.6f, 1.2f, 1.0);
Eigen::Vector4f right_brush_max(1.25f, 0.96f, 1.65f, 1.0);
right_brush_crop_.setMin(right_brush_min);
right_brush_crop_.setMax(right_brush_max);
right_brush_crop_.setNegative(true);
// 左侧主刷
Eigen::Vector4f left_brush_min(0.85f, -0.96f, 1.2f, 1.0);
Eigen::Vector4f left_brush_max(1.25f, -0.6f, 1.65f, 1.0);
left_brush_crop_.setMin(left_brush_min);
left_brush_crop_.setMax(left_brush_max);
left_brush_crop_.setNegative(true);
}
// 打印所有参数值（添加到构造函数末尾）
RCLCPP_INFO_STREAM(
this->get_logger(),
@ -203,16 +241,17 @@ class LidarMerger : public rclcpp::Node
}
// 2. 对变换后的原始点云进行处理（过滤、降采样、离群点去除）
auto processed_cloud = processSinglePointCloud(transformed_cloud);
auto processed_pcl = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
auto processed_cloud = processSinglePointCloud(transformed_cloud, processed_pcl);
if (!processed_cloud)
{
RCLCPP_WARN(get_logger(), "Failed to process %s cloud", source.c_str());
return;
}
// 3. 创建CloudFrame（存储原始+处理后的点云）
// 3. 创建CloudFrame（存储原始+处理后的点云+处理后的PCL点云）
auto cloud_frame = std::make_shared<CloudFrame>(
CloudFrame{transformed_cloud, processed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
CloudFrame{transformed_cloud, processed_cloud, processed_pcl, rclcpp::Time(msg->header.stamp), receive_time, source});
// 4. 添加到缓存并立即尝试合并
{
@ -228,7 +267,8 @@ class LidarMerger : public rclcpp::Node
}
/* ---------- 单雷达点云处理（独立处理前后雷达点云） ---------- */
std::shared_ptr<PointCloud2> processSinglePointCloud(const std::shared_ptr<PointCloud2>& cloud_msg)
std::shared_ptr<PointCloud2> processSinglePointCloud(const std::shared_ptr<PointCloud2>& cloud_msg,
pcl::PointCloud<pcl::PointXYZ>::Ptr& processed_pcl_out)
{
if (!cloud_msg || cloud_msg->data.empty()) return nullptr;
@ -236,25 +276,66 @@ class LidarMerger : public rclcpp::Node
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pcl(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud_pcl);
// 1. 条件过滤（含车身/主刷过滤）
auto filtered = applyConditionalFiltering(cloud_pcl);
// 1. 条件过滤（空间过滤）
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
filtered->reserve(cloud_pcl->size());
for (const auto& point : cloud_pcl->points)
{
if (point.x > filter_min_x_ && point.x < filter_max_x_ &&
point.y > filter_min_y_ && point.y < filter_max_y_ &&
point.z > filter_min_z_ && point.z < filter_max_z_)
{
filtered->points.push_back(point);
}
}
filtered->width = filtered->points.size();
filtered->height = 1;
filtered->is_dense = true;
if (filtered->empty())
{
RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Filtered cloud is empty!");
return nullptr;
}
// 2. 降采样
auto downsampled = applyVoxelGridFiltering(filtered);
// 2. 车身和主刷过滤（使用预配置的CropBox）
if (filter_car_)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr temp(new pcl::PointCloud<pcl::PointXYZ>);
car_crop_.setInputCloud(filtered);
car_crop_.filter(*temp);
right_brush_crop_.setInputCloud(temp);
right_brush_crop_.filter(*filtered);
left_brush_crop_.setInputCloud(filtered);
left_brush_crop_.filter(*temp);
filtered.swap(temp);
}
if (filtered->empty())
{
RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Filtered cloud is empty after car/brush filter!");
return nullptr;
}
// 3. 降采样（使用预配置的VoxelGrid）
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>);
vg_.setInputCloud(filtered);
vg_.filter(*downsampled);
if (downsampled->empty())
{
RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Downsampled cloud is empty!");
return nullptr;
}
// 3. 离群点去除
auto outlier_removed = applyStatisticalOutlierRemoval(downsampled);
if (outlier_removed->empty())
// 4. 离群点去除（使用预配置的StatisticalOutlierRemoval）
sor_.setInputCloud(downsampled);
sor_.filter(*processed_pcl_out);
if (processed_pcl_out->empty())
{
RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Outlier-removed cloud is empty!");
return nullptr;
@ -262,7 +343,7 @@ class LidarMerger : public rclcpp::Node
// 转换回ROS消息
auto processed_msg = cloud_pool_.acquire();
pcl::toROSMsg(*outlier_removed, *processed_msg);
pcl::toROSMsg(*processed_pcl_out, *processed_msg);
processed_msg->header = cloud_msg->header;
return processed_msg;
@ -286,6 +367,7 @@ class LidarMerger : public rclcpp::Node
{
cloud_pool_.release(cache.back()->cloud);
cloud_pool_.release(cache.back()->processed_cloud); // 释放处理后的点云
cache.back()->processed_pcl.reset(); // 释放PCL点云
cache.pop_back();
}
}
@ -356,11 +438,11 @@ class LidarMerger : public rclcpp::Node
merged_count_++;
// ========================= 栅格化 =========================
// 将合并后的点云转换为PCL格式用于栅格化
// 直接合并处理后的PCL点云，避免再次转换
pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pcl(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*merged, *merged_pcl);
*merged_pcl = *front_frame->processed_pcl + *rear_frame->processed_pcl;
// 生成栅格（无需重复处理，直接基于合并后的处理点云）
// 生成栅格（直接使用合并后的PCL点云）
auto grid = generateOccupancyGrid(merged_pcl);
if (!grid.empty())
{
@ -528,18 +610,17 @@ class LidarMerger : public rclcpp::Node
{
// 0:空 1:障碍物 2:车体
std::vector<std::vector<int>> grid(grid_size_, std::vector<int>(grid_size_, 0));
float resolution = grid_range_ / static_cast<float>(grid_size_);
static float resolution = grid_range_ / static_cast<float>(grid_size_); // 静态变量避免重复计算
static float inv_resolution = 1.0f / resolution;
// 标记车体区域
if (filter_car_)
{
// 计算车体在栅格中的位置
int car_min_x =
static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution);
int car_max_x =
static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution);
int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution);
int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution);
int car_min_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) * inv_resolution);
int car_max_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) * inv_resolution);
int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) * inv_resolution);
int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) * inv_resolution);
// 确保索引在有效范围内
car_min_x = std::max(0, std::min(car_min_x, grid_size_ - 1));
@ -561,10 +642,10 @@ class LidarMerger : public rclcpp::Node
for (const auto& point : cloud->points)
{
// X轴（前向）映射到行索引（从上到下为正X到负X）
int grid_x = static_cast<int>((grid_range_ / 2 - point.x) / resolution);
int grid_x = static_cast<int>((grid_range_ / 2 - point.x) * inv_resolution);
// Y轴（右向）映射到列索引（从左到右为负Y到正Y）
int grid_y = static_cast<int>((point.y + grid_range_ / 2) / resolution);
int grid_y = static_cast<int>((point.y + grid_range_ / 2) * inv_resolution);
if (grid_x >= 0 && grid_x < grid_size_ && grid_y >= 0 && grid_y < grid_size_)
{
@ -700,6 +781,7 @@ class LidarMerger : public rclcpp::Node
{
cloud_pool_.release((*front_it)->cloud);
cloud_pool_.release((*front_it)->processed_cloud);
(*front_it)->processed_pcl.reset();
front_clouds_.erase(front_it);
}
@ -709,6 +791,7 @@ class LidarMerger : public rclcpp::Node
{
cloud_pool_.release((*rear_it)->cloud);
cloud_pool_.release((*rear_it)->processed_cloud);
(*rear_it)->processed_pcl.reset();
rear_clouds_.erase(rear_it);
}
@ -734,6 +817,7 @@ class LidarMerger : public rclcpp::Node
}
cloud_pool_.release(front_clouds_.back()->cloud);
cloud_pool_.release(front_clouds_.back()->processed_cloud);
front_clouds_.back()->processed_pcl.reset();
front_clouds_.pop_back();
}
else
@ -754,6 +838,7 @@ class LidarMerger : public rclcpp::Node
}
cloud_pool_.release(rear_clouds_.back()->cloud);
cloud_pool_.release(rear_clouds_.back()->processed_cloud);
rear_clouds_.back()->processed_pcl.reset();
rear_clouds_.pop_back();
}
else
@ -809,6 +894,13 @@ class LidarMerger : public rclcpp::Node
double time_tolerance_, max_wait_time_;
bool enable_debug_;
// 点云处理过滤器（避免重复创建）
pcl::VoxelGrid<pcl::PointXYZ> vg_; // 体素网格过滤器
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor_; // 统计离群点去除器
pcl::CropBox<pcl::PointXYZ> car_crop_; // 车身区域过滤器
pcl::CropBox<pcl::PointXYZ> right_brush_crop_; // 右侧主刷过滤器
pcl::CropBox<pcl::PointXYZ> left_brush_crop_; // 左侧主刷过滤器
// 点云处理参数
float filter_min_x_, filter_max_x_;
float filter_min_y_, filter_max_y_;