提升雷达数据处理性能

src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp

@@ -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. 条件过滤（含车身/主刷过滤）
+        // 1. 条件过滤（空间过滤）
-        auto filtered = applyConditionalFiltering(cloud_pcl);
+        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. 降采样
+        // 2. 车身和主刷过滤（使用预配置的CropBox）
-        auto downsampled = applyVoxelGridFiltering(filtered);
+        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. 离群点去除
+        // 4. 离群点去除（使用预配置的StatisticalOutlierRemoval）
-        auto outlier_removed = applyStatisticalOutlierRemoval(downsampled);
+        sor_.setInputCloud(downsampled);
-        if (outlier_removed->empty())
+        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 =
+            int car_min_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) * inv_resolution);
-                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)) * inv_resolution);
-            int car_max_x =
+            int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) * inv_resolution);
-                static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution);
+            int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) * inv_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);
 
             // 确保索引在有效范围内
             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_;