PCL点云最小生成树算法实现及可视化 - 基于Kruskal算法

#include\x3ciostream\x3e\n#include\x3cpcl/point_types.h\x3e\n#include\x3cpcl/io/ply_io.h\x3e\n#include\x3cpcl/visualization/pcl_visualizer.h\x3e\n#include\x3cpcl/common/centroid.h\x3e\n#include\x3cpcl/features/normal_3d.h\x3e\n#include\x3cpcl/visualization/cloud_viewer.h\x3e\n#include\x3cpcl/segmentation/extract_clusters.h\x3e\n#include\x3cvector\x3e\n#include\x3cunordered_map\x3e\n"vtkAutoInit.h"\n#include\x3cpcl/visualization/point_cloud_color_handlers.h\x3e\nVTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2\nusing namespace pcl;\ntypedef pcl::PointXYZ PointT;\ntypedef pcl::PointCloud\x3cPointT\x3e PointCloudT;\n// 表示一条边的结构体\nstruct Edge\n{\n\t int src, tgt;\n\t float weight;\n};\n// 表示并查集的子集的结构体\nstruct Subset\n{\n\t int parent, rank;\n};\n// 表示一个连通的、无向的、带权重的图的类\nclass Graph\n{\npublic:\n\t int V, E;\n\t std::vector\x3cEdge\x3e edges;\n\t Graph(int v, int e)\n\t {\n\t\t V = v;\n\t\t E = e;\n\t }\n\t // 添加一条边到图中\n\t void addEdge(int src, int tgt, float weight)\n\t {\n\t\t Edge edge;\n\t\t edge.src = src;\n\t\t edge.tgt = tgt;\n\t\t edge.weight = weight;\n\t\t edges.push_back(edge);\n\t }\n\t // 查找某个元素所在的集合\n\t int find(Subset subsets[], int i)\n\t {\n\t\t if (subsets[i].parent != i)\n\t\t\t subsets[i].parent = find(subsets, subsets[i].parent);\n\t\t return subsets[i].parent;\n\t }\n\t // 合并两个集合\n\t void Union(Subset subsets[], int x, int y)\n\t {\n\t\t int xroot = find(subsets, x);\n\t\t int yroot = find(subsets, y);\n\t\t if (subsets[xroot].rank \x3c subsets[yroot].rank)\n\t\t\t subsets[xroot].parent = yroot;\n\t\t else if (subsets[xroot].rank \x3e subsets[yroot].rank)\n\t\t\t subsets[yroot].parent = xroot;\n\t\t else {\n\t\t\t subsets[yroot].parent = xroot;\n\t\t\t subsets[xroot].rank++;\n\t\t }\n\t }\n\t // Kruskal算法找到最小生成树\n\t void KruskalMST(PointCloudT::Ptr cloud, std::vector\x3cEdge\x3e& result, float threshold)\n\t {\n\t\t // 将边按照权重从小到大排序\n\t\t std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)\n\t\t {\n\t\t\t return a.weight \x3c b.weight;\n\t\t });\n\t\t // 为V个元素创建并查集的子集\n\t\t Subset* subsets = new Subset[V];\n\t\t for (int v = 0; v \x3c V; ++v)\n\t\t {\n\t\t\t subsets[v].parent = v;\n\t\t\t subsets[v].rank = 0;\n\t\t }\n\t\t int i = 0; // 用于选择下一条边的索引\n\t\t int e = 0; // 用于选择下一条边加入最小生成树的索引\n\t\t // 需要选择V-1条边\n\t\t while (e \x3c V - 1 && i \x3c E)\n\t\t {\n\t\t\t Edge next_edge = edges[i++];\n\t\t\t int x = find(subsets, next_edge.src);\n\t\t\t int y = find(subsets, next_edge.tgt);\n\t\t\t // 如果加入这条边不会形成环，并且权重大于阈值，则加入到结果中，并且增加已选择边的计数\n\t\t\t if (next_edge.weight \x3e= threshold && x != y)\n\t\t\t {\n\t\t\t\t result.push_back(next_edge);\n\t\t\t\t Union(subsets, x, y);\n\t\t\t\t ++e;\n\t\t\t }\n\t\t }\n\t }\n};\n// 计算两个点之间的欧式距离\ndouble euclideanDistance(PointXYZ p1, PointXYZ p2)\n{\n\t double dx = p2.x - p1.x;\n\t double dy = p2.y - p1.y;\n\t double dz = p2.z - p1.z;\n\t return std::sqrt(dx * dx + dy * dy + dz * dz);\n}\nint main()\n{\n\t // 从PLY文件加载输入点云\n\t pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);\n\t pcl::io::loadPLYFile\x3cpcl::PointXYZ\x3e("D:\DIANYUNWENJIANJIA\newOUSHIJULEI_ply.ply", *cloud);\n\n\t // 计算点云的质心\n\t Eigen::Vector4f centroid;\n\t pcl::compute3DCentroid(*cloud, centroid);\n\t // 计算点云的法线\n\t pcl::NormalEstimation\x3cpcl::PointXYZ, pcl::Normal\x3e ne;\n\t pcl::PointCloud\x3cpcl::Normal\x3e::Ptr cloud_normals(new pcl::PointCloud\x3cpcl::Normal\x3e);\n\t pcl::search::KdTree\x3cpcl::PointXYZ\x3e::Ptr tree(new pcl::search::KdTree\x3cpcl::PointXYZ\x3e);\n\t ne.setInputCloud(cloud);\n\t ne.setSearchMethod(tree);\n\t ne.setKSearch(40);\n\t ne.compute(*cloud_normals);\n\t // 创建一个有V个顶点和E个边的图\n\t int V = cloud->size();\n\t int E = V * (V - 1) / 2;\n\t Graph graph(V, E);\n\t // 基于点之间的欧式距离计算边的权重\n\t for (int i = 0; i \x3c V - 1; ++i)\n\t {\n\t\t const auto& src_point = cloud->points[i];\n\t\t for (int j = i + 1; j \x3c V; ++j)\n\t\t {\n\t\t\t const auto& tgt_point = cloud->points[j];\n\t\t\t float distance = euclideanDistance(src_point, tgt_point);\n\t\t\t graph.addEdge(i, j, distance);\n\t\t }\n\t }\n\t // 设置修剪的阈值\n\t float threshold = 0.00080;\n\t // 执行Kruskal算法找到最小生成树\n\t std::vector\x3cEdge\x3e result;\n\t graph.KruskalMST(cloud, result, threshold);\n// 创建一个新的点云对象保存修剪后的最小生成树结果\n\t pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr new_cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);\n\t new_cloud->width = cloud->width;\n\t new_cloud->height = cloud->height;\n\t new_cloud->points.resize(cloud->points.size());\n\t // 找到在最小生成树中出现三次的节点\n\t std::unordered_map\x3cint, int\x3e node_count;\n\t for (const auto& edge : result)\n\t {\n\t\t node_count[edge.src]++;\n\t\t node_count[edge.tgt]++;\n\t }\n\t std::vector\x3cint\x3e threejiedian;\n\t for (const auto& pair : node_count)\n\t {\n\t\t if (pair.second == 3)\n\t\t {\n\t\t\t threejiedian.push_back(pair.first);\n\t\t }\n\t }\n\t // 如果与threejiedian相连的边的权重小于0.0008，则从最小生成树中移除该边\n\t std::vector\x3cEdge\x3e pruned_result;\n\t for (const auto& edge : result)\n\t {\n\t\t if (std::find(threejiedian.begin(), threejiedian.end(), edge.src) != threejiedian.end() ||\n\t\t\t std::find(threejiedian.begin(), threejiedian.end(), edge.tgt) != threejiedian.end())\n\t\t {\n\t\t\t if (edge.weight \x3c= 0.008)\n\t\t\t {\n\t\t\t\t pruned_result.push_back(edge);\n\t\t\t }\n\t\t }\n\t\t else\n\t\t {\n\t\t\t pruned_result.push_back(edge);\n\t\t }\n\t }\n\t // 将修剪后的最小生成树的顶点添加到新的点云对象中\n\t for (const auto& edge : pruned_result)\n\t {\n\t\t const auto& src_point = cloud->points[edge.src];\n\t\t const auto& tgt_point = cloud->points[edge.tgt];\n\t\t new_cloud->points[edge.src] = src_point;\n\t\t new_cloud->points[edge.tgt] = tgt_point;\n\t }\n\t // 将与threejiedian相连的边的顶点设置为绿色\n\t pcl::PointCloud\x3cpcl::PointXYZRGB\x3e::Ptr colored_cloud(new pcl::PointCloud\x3cpcl::PointXYZRGB\x3e);\n\t colored_cloud->points.resize(cloud->points.size());\n\t for (size_t i = 0; i \x3c cloud->points.size(); ++i)\n\t {\n\t\t colored_cloud->points[i].x = cloud->points[i].x;\n\t\t colored_cloud->points[i].y = cloud->points[i].y;\n\t\t colored_cloud->points[i].z = cloud->points[i].z;\n\n\t\t colored_cloud->points[i].r = 255;\n\t\t colored_cloud->points[i].g = 0;\n\t\t colored_cloud->points[i].b = 0;\n\t }\n\t for (const auto& edge : result)\n\t {\n\t\t if (std::find(threejiedian.begin(), threejiedian.end(), edge.src) != threejiedian.end() ||\n\t\t\t std::find(threejiedian.begin(), threejiedian.end(), edge.tgt) != threejiedian.end())\n\t\t {\n\t\t\t colored_cloud->points[edge.src].r = 0;\n\t\t\t colored_cloud->points[edge.src].g = 255;\n\t\t\t colored_cloud->points[edge.src].b = 0;\n\t\t\t colored_cloud->points[edge.tgt].r = 0;\n\t\t\t colored_cloud->points[edge.tgt].g = 255;\n\t\t\t colored_cloud->points[edge.tgt].b = 0;\n\t\t }\n\t }\n\t // 找到y值最大的threejiedian节点\n\t float max_y = std::numeric_limits\x3cfloat\x3e::min();\n\t int max_y_index = -1;\n\t for (const auto& index : threejiedian)\n\t {\n\t\t const auto& point = cloud->points[index];\n\t\t if (point.y \x3e max_y)\n\t\t {\n\t\t\t max_y = point.y;\n\t\t\t max_y_index = index;\n\t\t }\n\t }\n\t // 将y值最大的threejiedian节点用绿色小球显示\n\t pcl::visualization::PointCloudColorHandlerRGBField\x3cpcl::PointXYZRGB\x3e colored_cloud_color_handler(colored_cloud);\n\t pcl::visualization::PointCloudColorHandlerCustom\x3cpcl::PointXYZ\x3e red_color(cloud, 255, 0, 0);\n\t pcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree");\n\t viewer.setBackgroundColor(0, 0, 0);\n\t viewer.addPointCloud\x3cpcl::PointXYZRGB\x3e(colored_cloud, colored_cloud_color_handler, "colored_cloud");\n\t viewer.addPointCloud\x3cpcl::PointXYZ\x3e(cloud, red_color, "original_cloud");\n\t viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "colored_cloud");\n\t viewer.spin();\n\t // 在y值最大的threejiedian节点上添加一个绿色小球\n\t pcl::ModelCoefficients sphere_coeff;\n\t sphere_coeff.values.resize(4);\n\t sphere_coeff.values[0] = cloud->points[max_y_index].x;\n\t sphere_coeff.values[1] = cloud->points[max_y_index].y;\n\t sphere_coeff.values[2] = cloud->points[max_y_index].z;\n\t sphere_coeff.values[3] = 0.00001; // 设置球的半径为0.01\n\t viewer.addSphere(sphere_coeff, "max_y_sphere");\n\t viewer.spin();\n\n\t // 将新的点云保存为PLY文件\n\t pcl::io::savePLYFile("D:\DIANYUNWENJIANJIA\newKRUSKAL_ply.ply", *new_cloud, true);\n\t return 0;\n