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-01-27 09:49:42 +08:00
parent 45a853b86c
commit 20b6002099
1 changed files with 110 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -22,10 +22,11 @@
using sensor_msgs::msg::PointCloud2;
using std::placeholders::_1;
// 点云帧结构体
// 点云帧结构体（新增processed_cloud存储处理后的点云）
struct CloudFrame
{
std::shared_ptr<PointCloud2> cloud;
std::shared_ptr<PointCloud2> cloud; // 原始点云
std::shared_ptr<PointCloud2> 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>(
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>(
CloudFrame{transformed_cloud, processed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
// 4. 添加到缓存并立即尝试合并
{
std::lock_guard<std::mutex> lock(cache_mutex_);
addToCache(cloud_frame);
@ -218,6 +227,47 @@ class LidarMerger : public rclcpp::Node
printStats();
}
/* ---------- 单雷达点云处理（独立处理前后雷达点云） ---------- */
std::shared_ptr<PointCloud2> processSinglePointCloud(const std::shared_ptr<PointCloud2>& cloud_msg)
{
if (!cloud_msg || cloud_msg->data.empty()) return nullptr;
// 将ROS点云消息转换为PCL点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pcl(new pcl::PointCloud<pcl::PointXYZ>);
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<CloudFrame> 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<pcl::PointXYZ>::Ptr merged_pcl(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*merged, *merged_pcl);
// ========================= 点云 =========================
auto processed_pcl = processPointCloud(merged_pcl);
if (!processed_pcl) return;
// 将处理后的PCL点云转回ROS消息
auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>();
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<double>(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<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
/* ---------- 合并处理后的点云 ---------- */
std::shared_ptr<PointCloud2> 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<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ> crop;
@ -453,8 +468,6 @@ class LidarMerger : public rclcpp::Node
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
crop.filter(*output);
// return output;
// 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65)
pcl::CropBox<pcl::PointXYZ> crop1;
crop1.setInputCloud(output);
@ -630,6 +643,7 @@ class LidarMerger : public rclcpp::Node
// 发布消息
grid_pub_->publish(msg);
}
/* ---------- 优化的匹配算法 ---------- */
std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> 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<PointCloud2> 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<PointCloud2> transformCloud(const PointCloud2& in)
{