点云聚类：K-means 算法实现

该函数使用 K-means 算法对给定的点云进行聚类分析，并将聚类结果存储在指针容器中。

参数：

• 'cloud': 输入的点云数据，类型为 pcl::PointCloudpcl::PointXYZ::Ptr。
• 'xj', 'yj', 'zj': 聚类中心的坐标。
• 'r': 聚类的半径。
• 'clusters': 存储聚类结果的指针容器，类型为 std::vector<pcl::PointCloudpcl::PointXYZ::Ptr>。

具体实现步骤：

1. 创建一个 Kd 树对象，并将输入的点云数据设置为 Kd 树的输入。
2. 创建一个存储聚类索引的向量 'clusterIndices'，以及一个存储点是否被访问过的标记向量 'visited'。
3. 遍历点云中的每个点，如果该点已被访问过，则跳过。
4. 创建一个搜索点 'searchPoint'，将当前点设置为搜索点。
5. 使用 Kd 树的 radiusSearch 函数，在半径 'r' 内搜索与搜索点距离小于等于 'r' 的邻近点，并将搜索结果存储在 'pointIdxRadiusSearch' 和 'pointRadiusSquaredDistance' 中。
6. 创建一个聚类对象 'cluster'，将当前点的索引添加到聚类索引中，并将当前点标记为已访问。
7. 遍历搜索结果中的每个邻近点，如果邻近点未被访问且与聚类中心的距离小于等于 'r'，则将邻近点的索引添加到聚类索引中，并将邻近点标记为已访问。
8. 将聚类对象添加到 'clusterIndices' 中。
9. 遍历 'clusterIndices' 中的每个聚类索引，创建一个新的聚类对象 'cluster'，并将聚类索引中的点添加到聚类对象中。
10. 将聚类对象添加到 'clusters' 中。
11. 函数结束。

void kMeansClustering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const double xj, const double yj, const double zj, const double r, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clusters)
{
	pcl::KdTreeFLANN<pcl::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;

		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);

	}
}