基于PCL库的K-Means点云聚类C++代码及详解

基于PCL库的K-Means点云聚类代码解析

这篇博客文章提供了一个C++代码示例，演示如何利用点云库（PCL）实现K-Means聚类算法，对三维点云数据进行分组。

**代码：**cppvoid kMeansClustering(const pcl::PointCloudpcl::PointXYZ::Ptr& cloud, const double xj, const double yj, const double zj, const double r, std::vector<pcl::PointCloudpcl::PointXYZ::Ptr>& clusters){ // 创建Kd树并设置输入点云 pcl::KdTreeFLANNpcl::PointXYZ kdtree; kdtree.setInputCloud(cloud);

// 用于存储聚类结果和点访问状态    std::vector<pcl::PointIndices> clusterIndices;    std::vector<bool> visited(cloud->size(), false);

// 遍历点云中的每个点    for (int i = 0; i < cloud->size(); i++)    {        if (visited[i])            continue;

    pcl::PointXYZ searchPoint(cloud->points[i]);        std::vector<int> pointIdxRadiusSearch;        std::vector<float> pointRadiusSquaredDistance;

    // 使用Kd树进行半径搜索        if (kdtree.radiusSearch(searchPoint, r, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)        {            pcl::PointIndices cluster;            cluster.indices.push_back(i);            visited[i] = true;

        // 将满足条件的点添加到聚类簇            for (size_t j = 0; j < pointIdxRadiusSearch.size(); j++)            {                int index = pointIdxRadiusSearch[j];

            if (!visited[index])                {                    pcl::PointXYZ point = cloud->points[index];                    double distance = sqrt(pow(point.x - xj, 2) + pow(point.y - yj, 2) + pow(point.z - zj, 2));

                if (distance <= r)                    {                        cluster.indices.push_back(index);                        visited[index] = true;                     }                }            }

        clusterIndices.push_back(cluster);        }    }

// 将聚类结果存储到指针容器中    for (const auto& indices : clusterIndices)    {        pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);        for (const auto& index : indices.indices)        {            cluster->push_back(cloud->points[index]);        }        clusters.push_back(cluster);    }}

代码解释：

这段代码实现了一个基于K-Means的点云聚类算法。给定一个点云 cloud、中心点坐标 (xj, yj, zj) 以及聚类半径 r，该函数将点云中的点分组到不同的聚类簇中，并将结果存储在 clusters 中。

步骤：

创建Kd树： 首先，创建了一个Kd树对象 kdtree，并将输入点云 cloud 传递给它。Kd树是一种数据结构，可以高效地搜索k维空间中的最近邻居，用于加速半径搜索。2. 初始化： 创建了两个向量：clusterIndices 用于存储每个聚类簇的点索引，visited 用于标记点云中的点是否已经被访问过。3. 遍历点云： 遍历点云中的每个点。如果该点已经被访问过，则跳过该点。4. 半径搜索： 使用 kdtree.radiusSearch 函数搜索以当前点为中心，半径为 r 的所有点。搜索结果存储在 pointIdxRadiusSearch 和 pointRadiusSquaredDistance 中。5. 创建聚类簇： 对于每个未被访问的点，创建一个新的聚类簇 cluster，并将该点添加到该聚类簇中。6. 扩展聚类簇： 遍历半径搜索的结果，对于每个未被访问的点，计算其到中心点 (xj, yj, zj) 的距离。如果距离小于等于 r，则将该点添加到当前聚类簇中，并将其标记为已访问。7. 存储聚类结果： 将生成的聚类簇 cluster 添加到 clusterIndices 中。8. 生成输出： 最后，遍历 clusterIndices 中的所有聚类簇，将每个聚类簇的点存储到一个新的 pcl::PointCloud<pcl::PointXYZ> 对象中，并将该对象添加到输出向量 clusters 中。

总结：

这段代码展示了如何使用PCL库中的Kd树结构高效地实现K-Means点云聚类算法。您可以根据需要修改中心点坐标和聚类半径等参数，以适应不同的点云数据集和应用场景。