From 20b60020992fdc6b14e943b04a78768f865f1a84 Mon Sep 17 00:00:00 2001 From: lyq Date: Tue, 27 Jan 2026 09:49:42 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E7=82=B9=E4=BA=91=E5=A4=84?= =?UTF-8?q?=E7=90=86=E6=B5=81=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/merge_two_lidars.cpp | 235 ++++++++---------- 1 file changed, 110 insertions(+), 125 deletions(-) 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 84fba5a..a958d23 100644 --- a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -22,10 +22,11 @@ using sensor_msgs::msg::PointCloud2; using std::placeholders::_1; -// 点云帧结构体 +// 点云帧结构体（新增processed_cloud存储处理后的点云） struct CloudFrame { - std::shared_ptr cloud; + std::shared_ptr cloud; // 原始点云 + std::shared_ptr processed_cloud; // 处理后的点云 rclcpp::Time stamp; rclcpp::Time received_time; // 添加接收时间用于延时分析 std::string source; @@ -193,7 +194,7 @@ class LidarMerger : public rclcpp::Node /* ---------- 同步处理点云 ---------- */ void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time) { - // 立即进行坐标变换 + // 1. 立即进行坐标变换（原始点云） auto transformed_cloud = transformCloud(*msg); if (!transformed_cloud) { @@ -201,11 +202,19 @@ class LidarMerger : public rclcpp::Node return; } - // 创建CloudFrame - auto cloud_frame = std::make_shared( - CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source}); + // 2. 对变换后的原始点云进行处理（过滤、降采样、离群点去除） + auto processed_cloud = processSinglePointCloud(transformed_cloud); + if (!processed_cloud) + { + RCLCPP_WARN(get_logger(), "Failed to process %s cloud", source.c_str()); + return; + } - // 添加到缓存并立即尝试合并 + // 3. 创建CloudFrame（存储原始+处理后的点云） + auto cloud_frame = std::make_shared( + CloudFrame{transformed_cloud, processed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source}); + + // 4. 添加到缓存并立即尝试合并 { std::lock_guard lock(cache_mutex_); addToCache(cloud_frame); @@ -218,6 +227,47 @@ class LidarMerger : public rclcpp::Node printStats(); } + /* ---------- 单雷达点云处理（独立处理前后雷达点云） ---------- */ + std::shared_ptr processSinglePointCloud(const std::shared_ptr& cloud_msg) + { + if (!cloud_msg || cloud_msg->data.empty()) return nullptr; + + // 将ROS点云消息转换为PCL点云 + pcl::PointCloud::Ptr cloud_pcl(new pcl::PointCloud); + pcl::fromROSMsg(*cloud_msg, *cloud_pcl); + + // 1. 条件过滤（含车身/主刷过滤） + auto filtered = applyConditionalFiltering(cloud_pcl); + if (filtered->empty()) + { + RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Filtered cloud is empty!"); + return nullptr; + } + + // 2. 降采样 + auto downsampled = applyVoxelGridFiltering(filtered); + if (downsampled->empty()) + { + RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Downsampled cloud is empty!"); + return nullptr; + } + + // 3. 离群点去除 + auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); + if (outlier_removed->empty()) + { + RCLCPP_WARN(get_logger(), "[processSinglePointCloud] Outlier-removed cloud is empty!"); + return nullptr; + } + + // 转换回ROS消息 + auto processed_msg = cloud_pool_.acquire(); + pcl::toROSMsg(*outlier_removed, *processed_msg); + processed_msg->header = cloud_msg->header; + + return processed_msg; + } + /* ---------- 添加到缓存 ---------- */ void addToCache(std::shared_ptr frame) { @@ -235,11 +285,12 @@ class LidarMerger : public rclcpp::Node while (cache.size() > cache_size_) { cloud_pool_.release(cache.back()->cloud); + cloud_pool_.release(cache.back()->processed_cloud); // 释放处理后的点云 cache.pop_back(); } } - /* ---------- 尝试合并 - 触发式 ---------- */ + /* ---------- 尝试合并 - 触发式（合并处理后的点云） ---------- */ void tryMerge() { if (front_clouds_.empty() || rear_clouds_.empty()) return; @@ -287,9 +338,9 @@ class LidarMerger : public rclcpp::Node return; } - // 合并点云 + // 合并处理后的点云（核心修改：不再合并原始点云） auto merge_start = std::chrono::high_resolution_clock::now(); - auto merged = mergeClouds(*front_frame, *rear_frame); + auto merged = mergeProcessedClouds(*front_frame, *rear_frame); auto merge_end = std::chrono::high_resolution_clock::now(); if (!merged) return; @@ -301,115 +352,80 @@ class LidarMerger : public rclcpp::Node merged->header.frame_id = target_frame_; // 发布合并后的点云 - // pub_->publish(*merged); + pub_->publish(*merged); merged_count_++; - // 将ROS点云消息转换为PCL点云 + // ========================= 栅格化 ========================= + // 将合并后的点云转换为PCL格式用于栅格化 pcl::PointCloud::Ptr merged_pcl(new pcl::PointCloud); pcl::fromROSMsg(*merged, *merged_pcl); - // ========================= 点云 ========================= - auto processed_pcl = processPointCloud(merged_pcl); - if (!processed_pcl) return; - - // 将处理后的PCL点云转回ROS消息 - auto processed = std::make_shared(); - pcl::toROSMsg(*processed_pcl, *processed); - processed->header = merged->header; - - // 发布处理后的点云 - pub_->publish(*processed); - // ========================= 点云 ========================= - // ========================= 栅格 ========================= - auto grid = processPointCloud_grid(merged_pcl); - if (grid.empty()) return; - - // 可视化栅格 - if (enable_print_) visualizeGridInTerminal(grid); - - // 发布栅格到ROS话题 - publishGrid(grid); - // ========================= 栅格 ========================= + // 生成栅格（无需重复处理，直接基于合并后的处理点云） + auto grid = generateOccupancyGrid(merged_pcl); + if (!grid.empty()) + { + // 可视化栅格 + if (enable_print_) visualizeGridInTerminal(grid); + // 发布栅格 + publishGrid(grid); + } // 延时分析（降低频率） if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 { - auto total_delay = (now_time - processed->header.stamp).seconds(); + auto total_delay = (now_time - merged->header.stamp).seconds(); auto process_time = std::chrono::duration(std::chrono::high_resolution_clock::now() - merge_start).count(); RCLCPP_INFO( get_logger(), - "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", - merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step, - total_delay, process_time * 1000, adaptive_threshold); + "Merged #%d: merged_points=%zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", + merged_count_, merged->data.size() / merged->point_step, total_delay, process_time * 1000, + adaptive_threshold); } // 清理已使用的点云 removeUsedClouds(front_frame, rear_frame); } - /* ---------- 点云处理流程 ---------- */ - pcl::PointCloud::Ptr processPointCloud(const pcl::PointCloud::Ptr& cloud) + /* ---------- 合并处理后的点云 ---------- */ + std::shared_ptr mergeProcessedClouds(const CloudFrame& front, const CloudFrame& rear) { - if (!cloud || cloud->empty()) return nullptr; - - // 1. 条件过滤 - auto filtered = applyConditionalFiltering(cloud); - if (filtered->empty()) + if (!front.processed_cloud || !rear.processed_cloud) { - RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!"); - return cloud; // 返回原始云而不是nullptr + RCLCPP_ERROR(get_logger(), "Processed cloud is null!"); + return nullptr; } - // 2. 降采样 - auto downsampled = applyVoxelGridFiltering(filtered); - if (downsampled->empty()) + size_t front_points = front.processed_cloud->data.size() / front.processed_cloud->point_step; + size_t rear_points = rear.processed_cloud->data.size() / rear.processed_cloud->point_step; + + if (front.processed_cloud->fields != rear.processed_cloud->fields || + front.processed_cloud->point_step != rear.processed_cloud->point_step || + front.processed_cloud->is_bigendian != rear.processed_cloud->is_bigendian) { - RCLCPP_WARN(get_logger(), "[processPointCloud] Downsampled cloud is empty!"); - return filtered; + RCLCPP_ERROR(get_logger(), "Processed PointCloud2 formats do not match!"); + return nullptr; } - // 3. 离群点去除 - auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); - if (outlier_removed->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud] Outlier-removed cloud is empty!"); - return downsampled; - } + auto merged = cloud_pool_.acquire(); - return outlier_removed; - } - std::vector> processPointCloud_grid(const pcl::PointCloud::Ptr& cloud) - { - if (!cloud || cloud->empty()) return {}; // 返回空栅格 + // 复制元数据 + merged->header = front.processed_cloud->header; + merged->fields = front.processed_cloud->fields; + merged->is_bigendian = front.processed_cloud->is_bigendian; + merged->point_step = front.processed_cloud->point_step; + merged->height = 1; + merged->width = front_points + rear_points; - // 1. 条件过滤 - auto filtered = applyConditionalFiltering(cloud); - if (filtered->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Filtered cloud is empty!"); - return {}; - } + // 高效合并处理后的点云数据 + merged->data.clear(); + merged->data.reserve(front.processed_cloud->data.size() + rear.processed_cloud->data.size()); + merged->data.insert(merged->data.end(), front.processed_cloud->data.begin(), front.processed_cloud->data.end()); + merged->data.insert(merged->data.end(), rear.processed_cloud->data.begin(), rear.processed_cloud->data.end()); + merged->row_step = merged->data.size(); - // 2. 降采样 - auto downsampled = applyVoxelGridFiltering(filtered); - if (downsampled->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Downsampled cloud is empty!"); - return {}; - } - - // 3. 离群点去除 - auto outlier_removed = applyStatisticalOutlierRemoval(downsampled); - if (outlier_removed->empty()) - { - RCLCPP_WARN(get_logger(), "[processPointCloud_grid] Outlier-removed cloud is empty!"); - return {}; - } - - // 4. 栅格化处理并返回栅格结果 - return generateOccupancyGrid(outlier_removed); + return merged; } /* ---------- 条件过滤 ---------- */ @@ -433,7 +449,6 @@ class LidarMerger : public rclcpp::Node // 不启用车身过滤，直接返回 if (!filter_car_) return filtered; - // RCLCPP_INFO(get_logger(), "启用车身过滤"); // 使用CropBox移除车身区域 pcl::CropBox crop; @@ -453,8 +468,6 @@ class LidarMerger : public rclcpp::Node pcl::PointCloud::Ptr output(new pcl::PointCloud); crop.filter(*output); - // return output; - // 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65) pcl::CropBox crop1; crop1.setInputCloud(output); @@ -630,6 +643,7 @@ class LidarMerger : public rclcpp::Node // 发布消息 grid_pub_->publish(msg); } + /* ---------- 优化的匹配算法 ---------- */ std::pair, std::shared_ptr> findBestMatchOptimized() { @@ -685,6 +699,7 @@ class LidarMerger : public rclcpp::Node if (front_it != front_clouds_.end()) { cloud_pool_.release((*front_it)->cloud); + cloud_pool_.release((*front_it)->processed_cloud); front_clouds_.erase(front_it); } @@ -693,6 +708,7 @@ class LidarMerger : public rclcpp::Node if (rear_it != rear_clouds_.end()) { cloud_pool_.release((*rear_it)->cloud); + cloud_pool_.release((*rear_it)->processed_cloud); rear_clouds_.erase(rear_it); } @@ -717,6 +733,7 @@ class LidarMerger : public rclcpp::Node RCLCPP_DEBUG(get_logger(), "Cleaning up old front cloud, age=%.3fs", age); } cloud_pool_.release(front_clouds_.back()->cloud); + cloud_pool_.release(front_clouds_.back()->processed_cloud); front_clouds_.pop_back(); } else @@ -736,6 +753,7 @@ class LidarMerger : public rclcpp::Node RCLCPP_DEBUG(get_logger(), "Cleaning up old rear cloud, age=%.3fs", age); } cloud_pool_.release(rear_clouds_.back()->cloud); + cloud_pool_.release(rear_clouds_.back()->processed_cloud); rear_clouds_.pop_back(); } else @@ -745,39 +763,6 @@ class LidarMerger : public rclcpp::Node } } - /* ---------- 合并点云 ---------- */ - std::shared_ptr mergeClouds(const CloudFrame& front, const CloudFrame& rear) - { - size_t front_points = front.cloud->data.size() / front.cloud->point_step; - size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step; - - if (front.cloud->fields != rear.cloud->fields || front.cloud->point_step != rear.cloud->point_step || - front.cloud->is_bigendian != rear.cloud->is_bigendian) - { - RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!"); - return nullptr; - } - - auto merged = cloud_pool_.acquire(); - - // 复制元数据 - merged->header = front.cloud->header; - merged->fields = front.cloud->fields; - merged->is_bigendian = front.cloud->is_bigendian; - merged->point_step = front.cloud->point_step; - merged->height = 1; - merged->width = front_points + rear_points; - - // 高效合并数据 - merged->data.clear(); - merged->data.reserve(front.cloud->data.size() + rear.cloud->data.size()); - merged->data.insert(merged->data.end(), front.cloud->data.begin(), front.cloud->data.end()); - merged->data.insert(merged->data.end(), rear.cloud->data.begin(), rear.cloud->data.end()); - merged->row_step = merged->data.size(); - - return merged; - } - /* ---------- 坐标变换 - 简化版 ---------- */ std::shared_ptr transformCloud(const PointCloud2& in) {