cuda_sample:clock

本文介绍了一个使用CUDA实现的并行规约算法,该算法在GPU上执行,并利用共享内存进行优化。通过计算平均时钟周期,分析了不同块数和线程数对硬件利用率的影响。
部署运行你感兴趣的模型镜像

// System includes
#include <stdio.h>
#include <stdint.h>
#include <assert.h>

// CUDA runtime
#include <cuda_runtime.h>

// helper functions and utilities to work with CUDA
#include <helper_functions.h>
#include <helper_cuda.h>

// This kernel computes a standard parallel reduction and evaluates the
// time it takes to do that for each block. The timing results are stored
// in device memory.
__global__ static void timedReduction(const float *input, float *output, clock_t *timer)
{
    // __shared__ float shared[2 * blockDim.x];
    extern __shared__ float shared[];

    const int tid = threadIdx.x;
    const int bid = blockIdx.x;
    //每个块线程0号位置时记录一下,将每个块对应块号保存在timer列表中
    if (tid == 0) timer[bid] = clock();

    // 使用share memory时最优应该把global memory传输到share中,也就是L2内存
    shared[tid] = input[tid];
    shared[tid + blockDim.x] = input[tid + blockDim.x];

    // 规约求最小值
    for (int d = blockDim.x; d > 0; d /= 2)
    {
        __syncthreads();//这个同步出现在每次规约后,最后一次规约不需要同步

        if (tid < d)
        {
            float f0 = shared[tid];
            float f1 = shared[tid + d];

            if (f1 < f0)
            {
                shared[tid] = f1;
            }
        }
    }

    // 规约后tid == 0时share[0]就是结果
    if (tid == 0) output[bid] = shared[0];

    //同步所有线程
    __syncthreads();

    //这样计时函数就可以将每块块内执行时间保存在timer中,中间隔了一个gridDim.x,刚好可以存储截止时间
    if (tid == 0) timer[bid+gridDim.x] = clock();
}


#define NUM_BLOCKS    64
#define NUM_THREADS   256

// It's interesting to change the number of blocks and the number of threads to
// understand how to keep the hardware busy.
//
// Here are some numbers I get on my G80:
//    blocks - clocks
//    1 - 3096
//    8 - 3232
//    16 - 3364
//    32 - 4615
//    64 - 9981
//
// With less than 16 blocks some of the multiprocessors of the device are idle. With
// more than 16 you are using all the multiprocessors, but there's only one block per
// multiprocessor and that doesn't allow you to hide the latency of the memory. With
// more than 32 the speed scales linearly.

// Start the main CUDA Sample here
int main(int argc, char **argv)
{
    printf("CUDA Clock sample\n");

    // This will pick the best possible CUDA capable device
    int dev = findCudaDevice(argc, (const char **)argv);

    float *dinput = NULL;
    float *doutput = NULL;
    clock_t *dtimer = NULL;

    clock_t timer[NUM_BLOCKS * 2];
    float input[NUM_THREADS * 2];

    for (int i = 0; i < NUM_THREADS * 2; i++)
    {
        input[i] = (float)i;
    }

    checkCudaErrors(cudaMalloc((void **)&dinput, sizeof(float) * NUM_THREADS * 2));
    checkCudaErrors(cudaMalloc((void **)&doutput, sizeof(float) * NUM_BLOCKS));
    checkCudaErrors(cudaMalloc((void **)&dtimer, sizeof(clock_t) * NUM_BLOCKS * 2));

    checkCudaErrors(cudaMemcpy(dinput, input, sizeof(float) * NUM_THREADS * 2, cudaMemcpyHostToDevice));

    timedReduction<<<NUM_BLOCKS, NUM_THREADS, sizeof(float) * 2 *NUM_THREADS>>>(dinput, doutput, dtimer);

    checkCudaErrors(cudaMemcpy(timer, dtimer, sizeof(clock_t) * NUM_BLOCKS * 2, cudaMemcpyDeviceToHost));

    checkCudaErrors(cudaFree(dinput));
    checkCudaErrors(cudaFree(doutput));
    checkCudaErrors(cudaFree(dtimer));

    long double avgElapsedClocks = 0;

    for (int i = 0; i < NUM_BLOCKS; i++)
    {
        avgElapsedClocks += (long double) (timer[i + NUM_BLOCKS] - timer[i]);
    }

    avgElapsedClocks = avgElapsedClocks/NUM_BLOCKS;
    printf("Average clocks/block = %Lf\n", avgElapsedClocks);

    return EXIT_SUCCESS;
}

 

您可能感兴趣的与本文相关的镜像

PyTorch 2.5

PyTorch 2.5

PyTorch
Cuda

PyTorch 是一个开源的 Python 机器学习库,基于 Torch 库,底层由 C++ 实现,应用于人工智能领域,如计算机视觉和自然语言处理

2025-08-17 17:07:21.467] [trace] [5949] perception_worker.cpp:837: process_frame fusion_non_ground_by_rgb_mask_and_det done obstacle_pcd_idxs.size 1602 [2025-08-17 17:07:21.468] [trace] [5949] perception_worker.cpp:857: cloud_obstacle size 1602 [2025-08-17 17:07:21.534] [trace] [5949] perception_worker.cpp:924: After cluster filtering, cloud_obstacle size 1245 [2025-08-17 17:07:21.534] [trace] [5949] perception_worker.cpp:929: *************PCL filter Time to run:0.066653777 seconds.*************在auto start_filter = std::chrono::high_resolution_clock::now(); if (!cloud_obstacle->empty() && cloud_obstacle->size() > 200) { pcl::PointCloud<PCL_POINT_TYPE>::Ptr cloud_processed(new pcl::PointCloud<PCL_POINT_TYPE>); // *cloud_processed = *cloud_obstacle; // if (cloud_obstacle->size() < 2000) { // *cloud_processed = *cloud_obstacle; // } else { // // 降采样处理 // pcl::VoxelGrid<PCL_POINT_TYPE> voxel_filter; // voxel_filter.setInputCloud(cloud_obstacle); // voxel_filter.setLeafSize(0.01f, 0.01f, 0.01f); // 设置体素大小 // voxel_filter.filter(*cloud_processed); // } pcl::VoxelGrid<PCL_POINT_TYPE> voxel_filter; voxel_filter.setInputCloud(cloud_obstacle); voxel_filter.setLeafSize(0.01f, 0.01f, 0.01f); // 设置体素大小 voxel_filter.filter(*cloud_processed); // 欧式聚类 pcl::search::KdTree<PCL_POINT_TYPE>::Ptr tree(new pcl::search::KdTree<PCL_POINT_TYPE>); tree->setInputCloud(cloud_processed); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PCL_POINT_TYPE> ec; ec.setClusterTolerance(0.03); // 聚类容忍度(单位:米) ec.setMinClusterSize(50); // 最小簇大小 ec.setMaxClusterSize(25000); // 最大簇大小 ec.setSearchMethod(tree); ec.setInputCloud(cloud_processed); ec.extract(cluster_indices); // 提取有效簇 std::vector<pcl::PointCloud<PCL_POINT_TYPE>::Ptr> valid_clusters; for (const auto& indices : cluster_indices) { if (indices.indices.size() >= 50) { pcl::PointCloud<PCL_POINT_TYPE>::Ptr cluster(new pcl::PointCloud<PCL_POINT_TYPE>(*cloud_processed, indices.indices)); valid_clusters.push_back(cluster); } } // 合并有效簇 pcl::PointCloud<PCL_POINT_TYPE>::Ptr merged(new pcl::PointCloud<PCL_POINT_TYPE>); for (auto& c : valid_clusters) *merged += *c; if (merged->size() > 0) { cloud_obstacle = merged; LOG_TRACE("After cluster filtering, cloud_obstacle size {}", cloud_obstacle->size()); } } auto end_filter = std::chrono::high_resolution_clock::now(); std::chrono::duration<double> diff_end_filter= end_filter - start_filter; LOG_TRACE("*************PCL filter Time to run:{} seconds.*************",diff_end_filter.count());这里的耗时较大有办法优化吗
08-19
#include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/point_cloud2.hpp> #include <visualization_msgs/msg/marker_array.hpp> #include <pcl_conversions/pcl_conversions.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/common/common.h> #include <pcl/search/kdtree.h> #include <pcl/filters/filter.h> #include <pcl/filters/voxel_grid.h> #include <pcl/filters/passthrough.h> #include <chrono> #include <iostream> #include <vector> #include <cmath> #include <iomanip> #include <sstream> #include <memory> #include <queue> #include <unordered_set> // 边界框结构体 struct Box { float x_min; float y_min; float z_min; float x_max; float y_max; float z_max; }; // 锥桶信息结构体 struct Cone { pcl::PointXYZ center; float height; float width; float confidence; int id; std::string type; }; // 自定义点云有效性检查函数 template<typename PointT> bool isPointFinite(const PointT& point) { return std::isfinite(point.x) && std::isfinite(point.y) && std::isfinite(point.z); } // 点云处理模板类 template<typename PointT> class ProcessPointClouds { public: // 自定义欧式距离聚类算法 std::vector<typename pcl::PointCloud<PointT>::Ptr> EuclideanClustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize) { auto startTime = std::chrono::steady_clock::now(); std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters; std::cout << "=== 欧式距离聚类开始 ===" << std::endl; std::cout << "输入点云数量: " << cloud->size() << std::endl; std::cout << "聚类参数: 距离容忍度=" << clusterTolerance << ", 最小尺寸=" << minSize << ", 最大尺寸=" << maxSize << std::endl; // 检查点云是否为空 if (cloud->empty()) { std::cout << "点云为空,无法进行聚类" << std::endl; return clusters; } // 创建KdTree用于加速近邻搜索 typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); try { auto filtered_cloud = RemoveNaNPoints(cloud); tree->setInputCloud(filtered_cloud); std::cout << "KdTree构建成功" << std::endl; } catch (const std::exception& e) { std::cout << "KdTree构建失败: " << e.what() << std::endl; return clusters; } // 标记已访问的点 std::vector<bool> processed(cloud->points.size(), false); int total_clusters_found = 0; int skipped_clusters = 0; // 对每个点进行聚类 for (size_t i = 0; i < cloud->points.size(); ++i) { // 跳过已处理或无效的点 if (processed[i] || !isPointFinite(cloud->points[i])) { continue; } // 创建新的聚类 std::vector<int> cluster_indices; std::queue<int> points_queue; // 将当前点加入队列 points_queue.push(i); processed[i] = true; // 区域生长聚类 while (!points_queue.empty()) { // 获取队列中的点 int current_index = points_queue.front(); points_queue.pop(); cluster_indices.push_back(current_index); // 搜索近邻点 std::vector<int> neighbor_indices; std::vector<float> neighbor_distances; if (tree->radiusSearch(cloud->points[current_index], clusterTolerance, neighbor_indices, neighbor_distances) > 0) { for (const auto& neighbor_index : neighbor_indices) { if (!processed[neighbor_index] && isPointFinite(cloud->points[neighbor_index])) { processed[neighbor_index] = true; points_queue.push(neighbor_index); } } } } // 检查聚类尺寸是否在范围内 size_t cluster_size = cluster_indices.size(); total_clusters_found++; if (cluster_size >= static_cast<size_t>(minSize) && cluster_size <= static_cast<size_t>(maxSize)) { // 创建聚类点云 typename pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>); for (const auto& index : cluster_indices) { cluster_cloud->points.push_back(cloud->points[index]); } cluster_cloud->width = cluster_cloud->points.size(); cluster_cloud->height = 1; cluster_cloud->is_dense = true; clusters.push_back(cluster_cloud); std::cout << "聚类 " << clusters.size() << ": " << cluster_cloud->size() << " 个点" << std::endl; } else { skipped_clusters++; if (cluster_size < static_cast<size_t>(minSize)) { std::cout << "跳过小聚类: " << cluster_size << " 个点 (小于 " << minSize << ")" << std::endl; } else { std::cout << "跳过大聚类: " << cluster_size << " 个点 (大于 " << maxSize << ")" << std::endl; } } } auto endTime = std::chrono::steady_clock::now(); auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime); std::cout << "欧式距离聚类完成,耗时 " << elapsedTime.count() << " 毫秒,找到 " << clusters.size() << " 个有效聚类 (" << skipped_clusters << " 个被跳过,总共 " << total_clusters_found << " 个聚类)" << std::endl; return clusters; } // 计算两个点之间的欧式距离 float EuclideanDistance(const PointT& point1, const PointT& point2) { float dx = point1.x - point2.x; float dy = point1.y - point2.y; float dz = point1.z - point2.z; return std::sqrt(dx*dx + dy*dy + dz*dz); } // 计算边界框 Box BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster) { PointT minPoint, maxPoint; pcl::getMinMax3D(*cluster, minPoint, maxPoint); Box box; box.x_min = minPoint.x; box.y_min = minPoint.y; box.z_min = minPoint.z; box.x_max = maxPoint.x; box.y_max = maxPoint.y; box.z_max = maxPoint.z; return box; } // 过滤无效点 typename pcl::PointCloud<PointT>::Ptr RemoveNaNPoints(typename pcl::PointCloud<PointT>::Ptr cloud) { typename pcl::PointCloud<PointT>::Ptr filtered_cloud(new pcl::PointCloud<PointT>); std::vector<int> indices; pcl::removeNaNFromPointCloud(*cloud, *filtered_cloud, indices); std::cout << "过滤掉 " << (cloud->size() - filtered_cloud->size()) << " 个无效点,剩余 " << filtered_cloud->size() << " 个有效点" << std::endl; return filtered_cloud; } // 降采样点云 typename pcl::PointCloud<PointT>::Ptr DownsampleCloud(typename pcl::PointCloud<PointT>::Ptr cloud, float leaf_size) { typename pcl::PointCloud<PointT>::Ptr downsampled_cloud(new pcl::PointCloud<PointT>); pcl::VoxelGrid<PointT> voxel_grid; voxel_grid.setInputCloud(cloud); voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size); voxel_grid.filter(*downsampled_cloud); std::cout << "降采样后点云: " << cloud->size() << " -> " << downsampled_cloud->size() << " 个点" << std::endl; return downsampled_cloud; } // 直通滤波 typename pcl::PointCloud<PointT>::Ptr PassThroughFilter(typename pcl::PointCloud<PointT>::Ptr cloud, const std::string& field, float min_limit, float max_limit) { typename pcl::PointCloud<PointT>::Ptr filtered_cloud(new pcl::PointCloud<PointT>); pcl::PassThrough<PointT> pass; pass.setInputCloud(cloud); pass.setFilterFieldName(field); pass.setFilterLimits(min_limit, max_limit); pass.filter(*filtered_cloud); std::cout << "直通滤波(" << field << "): " << cloud->size() << " -> " << filtered_cloud->size() << " 个点, 范围[" << min_limit << ", " << max_limit << "]" << std::endl; return filtered_cloud; } // 锥桶识别函数 std::vector<Cone> DetectCones(typename pcl::PointCloud<PointT>::Ptr cloud, float min_height = 0.05, float max_height = 0.5, float min_width = 0.05, float max_width = 0.25, float height_to_width_ratio_min = 0.8, float height_to_width_ratio_max = 5.0) { std::vector<Cone> cones; std::cout << "=== 锥桶识别开始 ===" << std::endl; std::cout << "锥桶识别参数: 高度[" << min_height << ", " << max_height << "], 宽度[" << min_width << ", " << max_width << "], 高宽比[" << height_to_width_ratio_min << ", " << height_to_width_ratio_max << "]" << std::endl; // 使用欧式距离聚类 auto clusters = EuclideanClustering(cloud, 0.15, 10, 1000); int cone_id = 0; int potential_cones = 0; for (size_t i = 0; i < clusters.size(); ++i) { auto cluster = clusters[i]; // 计算边界框 Box box = BoundingBox(cluster); float height = box.z_max - box.z_min; float width_x = box.x_max - box.x_min; float width_y = box.y_max - box.y_min; float width = std::max(width_x, width_y); // 计算高度与宽度的比例 float height_to_width_ratio = (width > 0) ? height / width : 0; // 计算中心点 pcl::PointXYZ center; center.x = (box.x_min + box.x_max) / 2.0; center.y = (box.y_min + box.y_max) / 2.0; center.z = (box.z_min + box.z_max) / 2.0; // 计算置信度 float confidence = 0.0; // 检查尺寸是否符合锥桶特征 bool height_ok = (height >= min_height && height <= max_height); bool width_ok = (width >= min_width && width <= max_width); bool ratio_ok = (height_to_width_ratio >= height_to_width_ratio_min && height_to_width_ratio <= height_to_width_ratio_max); std::cout << "聚类 " << i << " 分析: " << std::endl; std::cout << " - 位置: (" << std::fixed << std::setprecision(2) << center.x << ", " << center.y << ", " << center.z << ")" << std::endl; std::cout << " - 尺寸: 高度=" << height << "m, 宽度=" << width << "m, 高宽比=" << height_to_width_ratio << std::endl; std::cout << " - 检查结果: 高度" << (height_ok ? "✓" : "✗") << " 宽度" << (width_ok ? "✓" : "✗") << " 高宽比" << (ratio_ok ? "✓" : "✗") << std::endl; if (height_ok && width_ok && ratio_ok) { potential_cones++; // 计算置信度 confidence = 0.7f; // 基础置信度 if (height_ok) confidence += 0.1f; if (width_ok) confidence += 0.1f; if (ratio_ok) confidence += 0.1f; // 确保置信度不超过1.0 confidence = std::min(confidence, 1.0f); Cone cone; cone.center = center; cone.height = height; cone.width = width; cone.confidence = confidence; cone.id = cone_id++; cone.type = "unknown"; cones.push_back(cone); std::cout << ">>> 识别为锥桶 #" << cone.id << " 置信度: " << confidence << std::endl; } else { std::cout << ">>> 未识别为锥桶" << std::endl; } } std::cout << "锥桶识别完成,共检测到 " << cones.size() << " 个锥桶 (" << potential_cones << " 个潜在锥桶)" << std::endl; return cones; } }; // 主要的ROS2节点类 class EuclideanClusteringNode : public rclcpp::Node { public: EuclideanClusteringNode() : Node("euclidean_clustering_node") { // 声明参数 - 使用更宽松的默认值 this->declare_parameter("cluster_tolerance", 0.09); this->declare_parameter("min_cluster_size", 10); this->declare_parameter("max_cluster_size", 200); this->declare_parameter("downsample_leaf_size", 0.03f); this->declare_parameter("enable_downsampling", false); // 锥桶识别参数 - 放宽条件 this->declare_parameter("cone_min_height", 0.0); this->declare_parameter("cone_max_height", 0.8); this->declare_parameter("cone_min_width", 0.0); this->declare_parameter("cone_max_width", 6.0); this->declare_parameter("cone_min_ratio", 0.0); this->declare_parameter("cone_max_ratio", 6.0); this->declare_parameter("cone_confidence_threshold", 0.3); // 直通滤波参数 this->declare_parameter("enable_passthrough", false); this->declare_parameter("passthrough_x_min", -10.0); this->declare_parameter("passthrough_x_max", 10.0); this->declare_parameter("passthrough_y_min", -10.0); this->declare_parameter("passthrough_y_max", 10.0); this->declare_parameter("passthrough_z_min", -0.5); this->declare_parameter("passthrough_z_max", 2.0); // 创建订阅者 subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>( "/filtered_pointcloud", 10, std::bind(&EuclideanClusteringNode::pointcloud_callback, this, std::placeholders::_1)); // 创建发布者 cluster_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("clustering_results/clusters", 10); cone_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("clustering_results/cones", 10); marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("clustering_results/markers", 10); marker_id_counter_ = 0; RCLCPP_INFO(this->get_logger(), "欧式距离聚类节点已初始化"); } private: void debug_pointcloud_stats(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const std::string& name) { if (cloud->empty()) { std::cout << name << ": 空点云" << std::endl; return; } pcl::PointXYZ min_pt, max_pt; pcl::getMinMax3D(*cloud, min_pt, max_pt); std::cout << name << "统计:" << std::endl; std::cout << " - 点数: " << cloud->size() << std::endl; std::cout << " - 范围 X: " << min_pt.x << " 到 " << max_pt.x << std::endl; std::cout << " - 范围 Y: " << min_pt.y << " 到 " << max_pt.y << std::endl; std::cout << " - 范围 Z: " << min_pt.z << " 到 " << max_pt.z << std::endl; // 统计有效点 int valid_points = 0; for (const auto& point : cloud->points) { if (isPointFinite(point)) valid_points++; } std::cout << " - 有效点: " << valid_points << "/" << cloud->size() << std::endl; } void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { auto start_time = std::chrono::steady_clock::now(); std::cout << "\n\n=== 收到新点云 ===" << std::endl; std::cout << "时间戳: " << msg->header.stamp.sec << "." << msg->header.stamp.nanosec << std::endl; std::cout << "坐标系: " << msg->header.frame_id << std::endl; // 转换点云格式 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg, *cloud); debug_pointcloud_stats(cloud, "原始点云"); if (cloud->empty()) { RCLCPP_WARN(this->get_logger(), "收到空点云"); return; } // 获取参数 float cluster_tolerance = this->get_parameter("cluster_tolerance").as_double(); int min_cluster_size = this->get_parameter("min_cluster_size").as_int(); int max_cluster_size = this->get_parameter("max_cluster_size").as_int(); float downsample_leaf_size = this->get_parameter("downsample_leaf_size").as_double(); bool enable_downsampling = this->get_parameter("enable_downsampling").as_bool(); // 获取锥桶识别参数 float cone_min_height = this->get_parameter("cone_min_height").as_double(); float cone_max_height = this->get_parameter("cone_max_height").as_double(); float cone_min_width = this->get_parameter("cone_min_width").as_double(); float cone_max_width = this->get_parameter("cone_max_width").as_double(); float cone_min_ratio = this->get_parameter("cone_min_ratio").as_double(); float cone_max_ratio = this->get_parameter("cone_max_ratio").as_double(); float cone_confidence_threshold = this->get_parameter("cone_confidence_threshold").as_double(); // 获取直通滤波参数 bool enable_passthrough = this->get_parameter("enable_passthrough").as_bool(); float passthrough_x_min = this->get_parameter("passthrough_x_min").as_double(); float passthrough_x_max = this->get_parameter("passthrough_x_max").as_double(); float passthrough_y_min = this->get_parameter("passthrough_y_min").as_double(); float passthrough_y_max = this->get_parameter("passthrough_y_max").as_double(); float passthrough_z_min = this->get_parameter("passthrough_z_min").as_double(); float passthrough_z_max = this->get_parameter("passthrough_z_max").as_double(); // 打印实际使用的参数 std::cout << "使用参数:" << std::endl; std::cout << " - 聚类: 容差=" << cluster_tolerance << ", 最小=" << min_cluster_size << ", 最大=" << max_cluster_size << std::endl; std::cout << " - 锥桶: 高度[" << cone_min_height << ", " << cone_max_height << "], 宽度[" << cone_min_width << ", " << cone_max_width << "], 高宽比[" << cone_min_ratio << ", " << cone_max_ratio << "]" << std::endl; // 创建点云处理器 ProcessPointClouds<pcl::PointXYZ> processor; // 1. 预处理:移除无效点 auto filtered_cloud = processor.RemoveNaNPoints(cloud); debug_pointcloud_stats(filtered_cloud, "过滤后点云"); if (filtered_cloud->empty()) { RCLCPP_WARN(this->get_logger(), "过滤后点云为空"); return; } // 2. 可选:直通滤波(限制感兴趣区域) pcl::PointCloud<pcl::PointXYZ>::Ptr processed_cloud = filtered_cloud; if (enable_passthrough) { std::cout << "应用直通滤波..." << std::endl; // X方向滤波 processed_cloud = processor.PassThroughFilter(processed_cloud, "x", passthrough_x_min, passthrough_x_max); debug_pointcloud_stats(processed_cloud, "X方向滤波后"); // Y方向滤波 processed_cloud = processor.PassThroughFilter(processed_cloud, "y", passthrough_y_min, passthrough_y_max); debug_pointcloud_stats(processed_cloud, "Y方向滤波后"); // Z方向滤波 processed_cloud = processor.PassThroughFilter(processed_cloud, "z", passthrough_z_min, passthrough_z_max); debug_pointcloud_stats(processed_cloud, "Z方向滤波后"); } // 3. 可选:降采样点云 if (enable_downsampling) { processed_cloud = processor.DownsampleCloud(processed_cloud, downsample_leaf_size); debug_pointcloud_stats(processed_cloud, "降采样后点云"); } if (processed_cloud->empty()) { RCLCPP_WARN(this->get_logger(), "处理后的点云为空"); return; } // 4. 欧式距离聚类 auto clusters = processor.EuclideanClustering(processed_cloud, cluster_tolerance, min_cluster_size, max_cluster_size); std::cout << "聚类结果: " << clusters.size() << " 个聚类" << std::endl; for (size_t i = 0; i < clusters.size(); ++i) { Box box = processor.BoundingBox(clusters[i]); std::cout << " 聚类 " << i << ": " << clusters[i]->size() << " 个点, " << "位置(" << std::fixed << std::setprecision(2) << (box.x_min + box.x_max)/2 << ", " << (box.y_min + box.y_max)/2 << ", " << (box.z_min + box.z_max)/2 << ")" << std::endl; } // 5. 锥桶识别 auto cones = processor.DetectCones(processed_cloud, cone_min_height, cone_max_height, cone_min_width, cone_max_width, cone_min_ratio, cone_max_ratio); // 6. 创建可视化结果 visualization_msgs::msg::MarkerArray all_markers; pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clusters_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cone_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); // 预定义颜色 - 紫色用于聚类点云 std::vector<uint8_t> purple_color = {128, 0, 128}; // 紫色 std::vector<uint8_t> green_color = {0, 255, 0}; // 绿色用于标记 // 处理聚类结果 for (size_t i = 0; i < clusters.size(); ++i) { bool is_cone = false; Cone matched_cone; if (!clusters[i]->empty()) { Box cluster_box = processor.BoundingBox(clusters[i]); pcl::PointXYZ cluster_center; cluster_center.x = (cluster_box.x_min + cluster_box.x_max) / 2.0; cluster_center.y = (cluster_box.y_min + cluster_box.y_max) / 2.0; cluster_center.z = (cluster_box.z_min + cluster_box.z_max) / 2.0; // 匹配锥桶 for (const auto& cone : cones) { float dist = processor.EuclideanDistance(cluster_center, cone.center); if (dist < 0.2 && cone.confidence >= cone_confidence_threshold) { is_cone = true; matched_cone = cone; break; } } } // 分配颜色 - cluster_pub_使用紫色,cone_pub_使用橙色 std::vector<uint8_t> cluster_color = purple_color; // 聚类点云使用紫色 std::vector<uint8_t> cone_color = {255, 165, 0}; // 锥桶点云使用橙色 // 创建彩色点云 for (const auto& point : clusters[i]->points) { if (isPointFinite(point)) { // 为cluster_pub_创建紫色点云 pcl::PointXYZRGB colored_point; colored_point.x = point.x; colored_point.y = point.y; colored_point.z = point.z; colored_point.r = cluster_color[0]; colored_point.g = cluster_color[1]; colored_point.b = cluster_color[2]; colored_clusters_cloud->points.push_back(colored_point); // 如果是锥桶,为cone_pub_创建橙色点云 if (is_cone) { pcl::PointXYZRGB cone_colored_point; cone_colored_point.x = point.x; cone_colored_point.y = point.y; cone_colored_point.z = point.z; cone_colored_point.r = cone_color[0]; cone_colored_point.g = cone_color[1]; cone_colored_point.b = cone_color[2]; cone_cloud->points.push_back(cone_colored_point); } } } // 创建边界框标记 - 使用绿色 Box box = processor.BoundingBox(clusters[i]); visualization_msgs::msg::Marker bbox_marker; bbox_marker.header = msg->header; bbox_marker.ns = "bounding_boxes"; bbox_marker.id = marker_id_counter_++; bbox_marker.type = visualization_msgs::msg::Marker::CUBE; bbox_marker.action = visualization_msgs::msg::Marker::ADD; bbox_marker.pose.position.x = (box.x_min + box.x_max) / 2.0; bbox_marker.pose.position.y = (box.y_min + box.y_max) / 2.0; bbox_marker.pose.position.z = (box.z_min + box.z_max) / 2.0; bbox_marker.pose.orientation.w = 1.0; bbox_marker.scale.x = box.x_max - box.x_min; bbox_marker.scale.y = box.y_max - box.y_min; bbox_marker.scale.z = box.z_max - box.z_min; // 标记使用绿色 bbox_marker.color.r = green_color[0] / 255.0; bbox_marker.color.g = green_color[1] / 255.0; bbox_marker.color.b = green_color[2] / 255.0; bbox_marker.color.a = 0.3; bbox_marker.lifetime = rclcpp::Duration::from_seconds(0.5); all_markers.markers.push_back(bbox_marker); } // 创建锥桶文本标记 - 使用绿色 for (size_t i = 0; i < cones.size(); ++i) { const auto& cone = cones[i]; if (cone.confidence >= cone_confidence_threshold) { visualization_msgs::msg::Marker text_marker; text_marker.header = msg->header; text_marker.ns = "cone_text"; text_marker.id = marker_id_counter_++; text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; text_marker.action = visualization_msgs::msg::Marker::ADD; text_marker.pose.position.x = cone.center.x; text_marker.pose.position.y = cone.center.y; text_marker.pose.position.z = cone.center.z + cone.height + 0.15; text_marker.scale.z = 0.15; // 文本标记使用绿色 text_marker.color.r = green_color[0] / 255.0; text_marker.color.g = green_color[1] / 255.0; text_marker.color.b = green_color[2] / 255.0; text_marker.color.a = 1.0; std::stringstream coord_stream; coord_stream << std::fixed << std::setprecision(2); coord_stream << "x: " << cone.center.x << "\n"; coord_stream << "y: " << cone.center.y << "\n"; coord_stream << "z: " << cone.center.z; text_marker.text = "Cone " + std::to_string(cone.id) + " (" + std::to_string(static_cast<int>(cone.confidence * 100)) + "%)\n" + coord_stream.str(); text_marker.lifetime = rclcpp::Duration::from_seconds(0.5); all_markers.markers.push_back(text_marker); } } // 发布所有结果 publish_colored_pointcloud(colored_clusters_cloud, msg->header, cluster_pub_); publish_colored_pointcloud(cone_cloud, msg->header, cone_pub_); marker_pub_->publish(all_markers); auto end_time = std::chrono::steady_clock::now(); auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time); RCLCPP_INFO(this->get_logger(), "处理完成: %lu 个聚类, %lu 个锥桶, 耗时 %ld ms", clusters.size(), cones.size(), elapsed_time.count()); // 重置标记ID计数器 if (marker_id_counter_ > 10000) { marker_id_counter_ = 0; } } void publish_pointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const std_msgs::msg::Header& header, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher) { if (cloud->empty()) return; sensor_msgs::msg::PointCloud2 output; pcl::toROSMsg(*cloud, output); output.header = header; output.header.frame_id = header.frame_id.empty() ? "rslidar" : header.frame_id; output.header.stamp = this->now(); publisher->publish(output); } void publish_colored_pointcloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, const std_msgs::msg::Header& header, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher) { if (cloud->empty()) return; sensor_msgs::msg::PointCloud2 output; pcl::toROSMsg(*cloud, output); output.header = header; output.header.frame_id = header.frame_id.empty() ? "rslidar" : header.frame_id; output.header.stamp = this->now(); publisher->publish(output); } // 订阅者和发布者 rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_pub_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cone_pub_; rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_; int marker_id_counter_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared<EuclideanClusteringNode>(); RCLCPP_INFO(node->get_logger(), "启动欧式距离聚类节点"); rclcpp::spin(node); rclcpp::shutdown(); return 0; } 这个代码存在那些问题
最新发布
11-13
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值