PCL点云库路径搜索算法代码解析

这段代码使用PCL点云库实现路径搜索功能。

首先，创建了一个名为yr1的PointCloud指针，类型为pcl::PointCloudpcl::PointXYZRGB，并将该指针指向clouds1[i]。然后，输出路径的编号。

接下来，初始化了两个PointXYZRGB类型的引用变量point54和point56，分别指向jinqi。这样做是为了防止修改jinqi，同时将搜索的起点统一设置为茎秆的起点。然后，将point56加入到clouds1[i]中。

接着，初始化了一个变量distances，用于记录路径的距离。然后，通过两层循环，找到一个叶片内不同点的循环，确保不会遗漏边。

在内层循环中，遍历所有生成树的边，获取边的两个点p0和p00。如果point56与p0相同，则将p00加入到yr1中，并更新distances和point56的值。如果point56与p00相同，则将p0加入到yr1中，并更新distances和point56的值。

然后，判断是否遇到叶节点，如果是，则将该边从resulta中删除，以防止下次搜索时重复遇到。

最后，外层循环结束后，路径的构建完成。

代码片段：

pcl::PointCloud<pcl::PointXYZRGB>::Ptr yr1 = clouds1[i];
cout << '路径' << i + 1 << '所有骨架点：' << endl;
pcl::PointXYZRGB& point54 = jinqi;//防止修改掉茎起点
pcl::PointXYZRGB& point56 = point54;//将搜索的起点统一设置为茎秆的起点
clouds1[i]->push_back(point56);//起点加入路径
double distances = 0;//设置路径的距离
for (int i = 0; i < cloud100->size(); i++)//找一个叶片内不同点的循环，确保不会遗漏边
{
	for (auto it = resulta.begin(); it != resulta.end();)//遍历所有生成树的边
	{
		pcl::PointXYZRGB& point53 = point56;//53表示上一个点的数据
		const auto& edge = *it;
		pcl::PointXYZRGB p0 = cloud100->points[edge.src];//在点云cloud里面找到该索引对应的点值
		pcl::PointXYZRGB p00 = cloud100->points[edge.tgt];
		//判断叶尖点与找到边两点是否相同，若相同则将该边加入yr1点云指针，将该边加入容器yr1
		if ((point56.x == p0.x) && (point56.y == p0.y) && (point56.z == p0.z))
		{
			yr1->push_back(p00);//yr1表示储存的路径
			distances = distances + euclideanDistance(point56, p00);
			point56 = p00;
		}
		else if ((point56.x == p00.x) && (point56.y == p00.y) && (point56.z == p00.z))
		{
			yr1->push_back(p0);
			distances = distances + euclideanDistance(point56, p0);
			point56 = p0;
		}
		//判断寻找的终点，当遇到叶节点时，此时点56为在分支上的新节点且加入了路径
		for (const auto& point9 : jie->points)
		{
			float x1 = point9.x;
			float y1 = point9.y;
			float z1 = point9.z;
			if (x1 == point53.x && y1 == point53.y && z1 == point53.z)
			{
			//如果点53是叶节点，那么56就是分支上的一点，而且已经加入到了路径。此时的边代表的就是分支上第一段的关系
				it = resulta.erase(it);
				//删除后，最小生成树就没有这两点的对应关系，下次就不会搜索到
			}
		}
	}
}