PCL点云路径生成及距离计算代码解析

这段代码的作用是根据给定的点云数据，生成路径yr1，并计算路径的总距离distances。

首先，代码定义了一个PointCloud指针yr1，并将clouds1[i]赋值给yr1。然后打印出路径的序号和所有骨架点的信息。

接下来，定义了两个引用变量point54和point56，分别指向jinqi和point54。并将point56加入到clouds1[i]中作为路径的起点。

然后，定义了一个distances变量，并初始化为0，用于记录路径的总距离。

接下来，通过两个嵌套循环分别遍历cloud100中的点和resulta中的边。

其中，第一个循环用于找到一个叶片内不同点的循环，确保不会遗漏边。

在第二个循环中，首先获取当前边的两个点p0和p00。

然后，通过判断point56和p0/p00是否相同，来确定当前边与point56的关系。

如果相同，则将p0/p00加入到yr1中，并更新distances的值，然后将point56更新为p0/p00。

接下来，通过判断point53是否为叶节点来确定是否需要删除当前边。如果point53是叶节点，说明point56是分支上的一个新节点，并已经加入到路径中，那么当前边代表的就是分支上第一段的关系。此时，将当前边从resulta中删除，以防止重复搜索。

最后，遍历完所有的点和边后，yr1中存储了完整的路径信息，distances记录了路径的总距离。

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);
				//删除后，最小生成树就没有这两点的对应关系，下次就不会搜索到
			}
		}
	}
}