PCL 点云库中查找最近点并提取聚类代码详解

这段代码的功能是从一个点云中找到与给定点最近的点，并提取出该点的聚类。

首先，定义了一个名为 'yq' 的指向 pcl::PointCloudpcl::PointXYZ 类型的智能指针。然后，创建了一个名为 'centroid1' 的 pcl::PointXYZ 类型的对象，并将其 x、y、z 坐标设置为 clouds[i]->points[j] 的对应坐标。

接下来，创建了一个名为 'kdtree' 的 pcl::KdTreeFLANNpcl::PointXYZ 类型的对象，并将其输入点云设置为 'cloud3'。然后，定义了两个 vector，'pointIdxNKNSearch' 用于存储最近点的索引，'pointNKNSquaredDistance' 用于存储最近点与给定点的距离的平方。调用 'kdtree' 的 'nearestKSearch' 方法，找到最近的点的索引，并将结果存储在 'pointIdxNKNSearch' 和 'pointNKNSquaredDistance' 中。

然后，创建了一个 pcl::ExtractIndicespcl::PointXYZ 类型的对象 'extract'，并将其输入点云设置为 'cloud3'。接着，创建了一个名为 'inliers' 的 pcl::PointIndices::Ptr 类型的智能指针，并将最近点的索引添加到 'inliers' 中。

然后，调用 'extract' 的 'setIndices' 方法，将 'inliers' 设置为提取的索引。接着，将 'extract' 的 'setNegative' 设置为 false，表示只提取 'inliers' 中的点，并调用 'filter' 方法，将提取的结果存储在 'yq' 中。

最后，通过循环遍历 'yq' 中的每个点，并打印出其坐标。

pcl::PointCloud<pcl::PointXYZ>::Ptr yq = yenn[j];
	pcl::PointXYZ centroid1;
	centroid1.x = clouds[i]->points[j].x;
	centroid1.y = clouds[i]->points[j].y;
	centroid1.z = clouds[i]->points[j].z;

	//pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
	//kMeansClustering(clouds[i], xj, yj, zj, r, yenn);

		// 创建KD树对象
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud3);
	std::vector<int> pointIdxNKNSearch(1);
	std::vector<float> pointNKNSquaredDistance(0.008);
	kdtree.nearestKSearch(centroid1, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
	// 获取最近点的索引
	int nearestPointIndex = pointIdxNKNSearch[0];
	// 创建提取索引对象
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud3);
	// 创建点云索引对象
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	inliers->indices.push_back(nearestPointIndex);

	// 提取最近点的聚类
	extract.setIndices(inliers);
	extract.setNegative(false);
	extract.filter(*yq);

	for (size_t i = 0; i < yq->size(); ++i)
	{
		std::cout << "Point " << i + 1 << " coordinates" << "("
			<< yq->points[i].x << ", "
			<< yq->points[i].y << ", "
			<< yq->points[i].z << ")" << std::endl;
	}