欧式聚类提取-------PCL

news2024/12/23 20:43:03

欧式聚类

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> PclTool::euclideanClustering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clustered_clouds;

    // 下采样
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*cloud_filtered);

       // 创建平面模型分割的对象并设置参数
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);  // 分割模型
    seg.setMethodType(pcl::SAC_RANSAC);     // 随机参数估计方法
    seg.setMaxIterations(100);              // 最大的迭代的次数
    seg.setDistanceThreshold(0.02);         // 设置阀值
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

    int i = 0, nr_points = (int)cloud_filtered->points.size();
    while (cloud_filtered->points.size() > 0.3 * nr_points)  // 滤波停止条件
    {
        // Segment the largest planar component from the remaining cloud
        seg.setInputCloud(cloud_filtered);  // 输入
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0)
        {
            std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(inliers);
        extract.setNegative(false);

        // Get the points associated with the planar surface
        extract.filter(*cloud_plane);  // [平面
        std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

        //  // 移去平面局内点，提取剩余点云
        extract.setNegative(true);
        extract.filter(*cloud_f);
        *cloud_filtered = *cloud_f;
    }


    // 创建KdTree对象用于欧式聚类的搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_filtered);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;  // 欧式聚类对象
    ec.setClusterTolerance(0.02);                       // 设置聚类容差为2cm
    ec.setMinClusterSize(100);                          // 设置一个聚类的最小点数为100
    ec.setMaxClusterSize(25000);                        // 设置一个聚类的最大点数为25000
    ec.setSearchMethod(tree);                           // 设置搜索方法
    ec.setInputCloud(cloud_filtered);
    ec.extract(cluster_indices);  // 从点云中提取聚类

    // 迭代聚类索引并创建每个聚类的点云
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
            cloud_cluster->points.push_back(cloud_filtered->points[*pit]);

        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        clustered_clouds.push_back(cloud_cluster);
    }

    return clustered_clouds;
}