基于pcl库写一段以已知点p为起点的点云欧式聚类的c++代码

以下是基于PCL库写的以已知点p为起点的点云欧式聚类的C++代码示例：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>

int main()
{
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("input_cloud.pcd", *cloud);

    // 创建KD树对象，并设置输入点云
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud(cloud);

    // 设置欧式聚类算法的参数
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02); // 设置聚类的最大距离阈值为0.02m
    ec.setMinClusterSize(100);    // 设置聚类的最小点数为100个
    ec.setMaxClusterSize(25000);  // 设置聚类的最大点数为25000个
    ec.setSearchMethod(kdtree);
    ec.setInputCloud(cloud);

    // 定义一个点云索引向量，每个元素保存一个聚类的索引
    std::vector<pcl::PointIndices> cluster_indices;

    // 执行欧式聚类
    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 cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
        {
            cluster->points.push_back(cloud->points[*pit]);
        }
        cluster->width = cluster->points.size();
        cluster->height = 1;
        cluster->is_dense = true;

        // 输出聚类的点数和中心坐标
        std::cout << "Cluster size: " << cluster->points.size() << std::endl;
        pcl::PointXYZ centroid;
        pcl::computeCentroid(*cluster, centroid);
        std::cout << "Centroid coordinates: (" << centroid.x << ", " << centroid.y << ", " << centroid.z << ")" << std::endl;
    }

    return 0;
}

在这个示例代码中，假设输入点云数据保存在名为input_cloud.pcd的文件中。首先，我们读取点云数据并创建一个KD树对象，然后设置欧式聚类算法的参数。接下来，执行欧式聚类，并获取每个聚类的索引。最后，遍历每个聚类，输出聚类的点数和中心坐标。

请注意，这个示例代码中并没有直接指定起点p，因为欧式聚类算法会自动根据设置的距离阈值和点数阈值来确定聚类的起点。如果你想要以特定点p为起点进行聚类，可以在遍历聚类之前添加一个判断条件来筛选出包含点p的聚类。