PCL库中路径长度计算及路径点生成代码详解

这段代码的作用是计算路径的长度以及生成路径点。

首先，yr1是一个指向pcl::PointCloudpcl::PointXYZRGB类型的指针，用来存储路径点。clouds1是一个pcl::PointCloudpcl::PointXYZRGB类型的向量，yr1指向其中的一个元素。

然后，将jinqi赋值给point54和point56，这里使用引用的方式避免修改掉jinqi的值。然后将point56添加到clouds1[i]中。

接下来，通过遍历cloud100中的点，找到与point56相邻的点，并将其添加到yr1中。同时，计算路径的长度，使用euclideanDistance函数计算point56和相邻点之间的欧式距离，并将其累加到distances中。最后，将相邻点赋值给point56。

在遍历resulta中的边时，如果找到了与point56相邻的点，将其添加到yr1中，并更新point56的值。同时，在遍历jie中的点时，如果找到了与上一个点相同的点，将其在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];
    pcl::PointXYZRGB p00 = cloud100->points[edge.tgt];
    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;
    }
    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)
      {
        it = resulta.erase(it)；
      }
    }
  }
}

这段代码使用了PCL库中的点云数据结构，并结合欧式距离计算，实现了路径长度计算和路径点生成。代码结构清晰，逻辑易懂，对于理解PCL库的使用和点云数据处理具有一定的参考价值。