diff --git a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp index a958d23..6e16b3e 100644 --- a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -27,6 +27,7 @@ struct CloudFrame { std::shared_ptr cloud; // 原始点云 std::shared_ptr processed_cloud; // 处理后的点云 + pcl::PointCloud::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("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移 car_lidar_offset_y_ = declare_parameter("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>(); + 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{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 processSinglePointCloud(const std::shared_ptr& cloud_msg) + std::shared_ptr processSinglePointCloud(const std::shared_ptr& cloud_msg, + pcl::PointCloud::Ptr& processed_pcl_out) { if (!cloud_msg || cloud_msg->data.empty()) return nullptr; @@ -236,25 +276,66 @@ class LidarMerger : public rclcpp::Node pcl::PointCloud::Ptr cloud_pcl(new pcl::PointCloud); pcl::fromROSMsg(*cloud_msg, *cloud_pcl); - // 1. 条件过滤（含车身/主刷过滤） - auto filtered = applyConditionalFiltering(cloud_pcl); + // 1. 条件过滤（空间过滤） + pcl::PointCloud::Ptr filtered(new pcl::PointCloud); + 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::Ptr temp(new pcl::PointCloud); + 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::Ptr downsampled(new pcl::PointCloud); + 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::Ptr merged_pcl(new pcl::PointCloud); - 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> grid(grid_size_, std::vector(grid_size_, 0)); - float resolution = grid_range_ / static_cast(grid_size_); + static float resolution = grid_range_ / static_cast(grid_size_); // 静态变量避免重复计算 + static float inv_resolution = 1.0f / resolution; // 标记车体区域 if (filter_car_) { // 计算车体在栅格中的位置 - int car_min_x = - static_cast((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); - int car_max_x = - static_cast((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); - int car_min_y = static_cast((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution); - int car_max_y = static_cast((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution); + int car_min_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) * inv_resolution); + int car_max_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) * inv_resolution); + int car_min_y = static_cast((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) * inv_resolution); + int car_max_y = static_cast((-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((grid_range_ / 2 - point.x) / resolution); + int grid_x = static_cast((grid_range_ / 2 - point.x) * inv_resolution); // Y轴（右向）映射到列索引（从左到右为负Y到正Y） - int grid_y = static_cast((point.y + grid_range_ / 2) / resolution); + int grid_y = static_cast((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 vg_; // 体素网格过滤器 + pcl::StatisticalOutlierRemoval sor_; // 统计离群点去除器 + pcl::CropBox car_crop_; // 车身区域过滤器 + pcl::CropBox right_brush_crop_; // 右侧主刷过滤器 + pcl::CropBox left_brush_crop_; // 左侧主刷过滤器 + // 点云处理参数 float filter_min_x_, filter_max_x_; float filter_min_y_, filter_max_y_;