点云最小生成树可视化：鼠标点击显示节点索引

#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#include\x22vtkAutoInit.h\x22\n\nVTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2\n\nusing namespace pcl;typedef pcl::PointXYZ PointT;typedef pcl::PointCloud\x3cPointT\x3e PointCloudT;// Structure to represent an edge\nstruct Edge{\n\tint src, tgt;\n\tfloat weight;\n};// Structure to represent a subset for union-find\nstruct Subset{\n\tint parent, rank;\n};// Class to represent a connected, undirected and weighted graph\nclass Graph{\npublic:\n\tint V, E;\n\tstd::vector\x3cEdge\x3e edges;\n\tGraph(int v, int e){\n\t\tV = v;\n\t\tE = e;\n\t}\n\t// Add an edge to the graph\n\tvoid addEdge(int src, int tgt, float weight){\n\t\tEdge edge;\n\t\tedge.src = src;\n\t\tedge.tgt = tgt;\n\t\tedge.weight = weight;\n\t\tedges.push_back(edge);\n\t}\n\t// Find set of an element i\n\tint find(Subset subsets[], int i){\n\t\tif (subsets[i].parent != i)\n\t\t\tsubsets[i].parent = find(subsets, subsets[i].parent);\n\t\treturn subsets[i].parent;\n\t}\n\t// Union of two sets x and y\n\tvoid Union(Subset subsets[], int x, int y){\n\t\tint xroot = find(subsets, x);\n\t\tint yroot = find(subsets, y);\n\t\tif (subsets[xroot].rank \x3c subsets[yroot].rank)\n\t\t\tsubsets[xroot].parent = yroot;\n\t\telse if (subsets[xroot].rank \x3e subsets[yroot].rank)\n\t\t\tsubsets[yroot].parent = xroot;\n\t\telse {\n\t\t\tsubsets[yroot].parent = xroot;\n\t\t\tsubsets[xroot].rank++;\n\t\t}\n\t}\n\t// Kruskal's algorithm to find MST\n\tvoid KruskalMST(PointCloudT::Ptr cloud){\n\t\tstd::vector\x3cEdge\x3e result; // This will store the resultant MST\n\n\t\t// Sort all the edges in non-decreasing order of their weight\n\t\tstd::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b){\n\t\t\treturn a.weight \x3c b.weight;\n\t\t});\n\n\t\t// Allocate memory for creating V subsets\n\t\tSubset* subsets = new Subset[V];\n\t\tfor (int v = 0; v \x3c V; ++v){\n\t\t\tsubsets[v].parent = v;\n\t\t\tsubsets[v].rank = 0;\n\t\t}\n\n\t\tint i = 0; // Index used to pick the next smallest edge\n\t\tint e = 0; // Index used to pick the next edge to include in MST\n\n\t\t// Number of edges to be taken is equal to V-1\n\t\twhile (e \x3c V - 1 && i \x3c E){\n\t\t\tEdge next_edge = edges[i++];\n\n\t\t\tint x = find(subsets, next_edge.src);\n\t\t\tint y = find(subsets, next_edge.tgt);\n\n\t\t\t// If including this edge doesn't cause a cycle, include it in result and increment the index of result for next edge\n\t\t\tif (x != y && next_edge.weight \x3c 0.045){\n\t\t\t\tresult.push_back(next_edge);\n\t\t\t\tUnion(subsets, x, y);\n\t\t\t\t++e;\n\t\t\t}\n\t\t}\n\n\t\t// Visualize the resulting minimum spanning tree\n\t\tpcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree");\n\t\tviewer.setBackgroundColor(0, 0, 0);\n\n\t\t// Add the original point cloud to the viewer\n\t\tpcl::visualization::PointCloudColorHandlerCustom\x3cpcl::PointXYZ\x3e single_color(cloud, 255, 255, 255);\n\t\tviewer.addPointCloud\x3cpcl::PointXYZ\x3e(cloud, single_color, "original_cloud");\n\n\t\t// Count the number of points in the minimum spanning tree\n\t\tint numPoints = 0;\n\t\tfor (const auto& edge : result){\n\t\t\tconst auto& src_point = cloud->points[edge.src];\n\t\t\tconst auto& tgt_point = cloud->points[edge.tgt];\n\t\t\tnumPoints += 2;\n\t\t}\n\n\t\tstd::cout \x3c\x3c "Number of points in the minimum spanning tree: " \x3c\x3c numPoints \x3c\x3c std::endl;\n\n\t\t// Callback function for mouse events\n\t\tstruct callback_args {\n\t\t\tPointCloudT::Ptr cloud;\n\t\t\tpcl::visualization::PCLVisualizer::Ptr viewer;\n\t\t};\n\n\t\tcallback_args cb_args;\n\t\tcb_args.cloud = cloud;\n\t\tcb_args.viewer = pcl::visualization::PCLVisualizer::Ptr(&viewer);\n\n\t\tviewer.registerPointPickingCallback([](const pcl::visualization::PointPickingEvent& event, void* args){\n\t\t\tcallback_args* data = static_cast\x3ccallback_args*\x3e(args);\n\t\t\tif (event.getPointIndex() != -1){\n\t\t\t\tint index = event.getPointIndex();\n\t\t\t\tstd::cout \x3c\x3c "Selected point index: " \x3c\x3c index \x3c\x3c std::endl;\n\t\t\t}\n\t\t}, (void*)&cb_args);\n\n\t\twhile (!viewer.wasStopped()){\n\t\t\tviewer.spinOnce();\n\t\t}\n\n\t}\n};\ndouble euclideanDistance(PointXYZ p1, PointXYZ p2){\n\tdouble dx = p2.x - p1.x;\n\tdouble dy = p2.y - p1.y;\n\tdouble dz = p2.z - p1.z;\n\treturn std::sqrt(dx*dx + dy * dy + dz * dz);\n}\nint main(){\n\t// Load the input point cloud from PLY file\n\tpcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);\n\tpcl::io::loadPLYFile\x3cpcl::PointXYZ\x3e("D:\DIANYUNWENJIANJIA\newOUSHIJULEI_ply.ply", *cloud);\n\n\t// Compute the centroid of the point cloud\n\tEigen::Vector4f centroid;\n\tpcl::compute3DCentroid(*cloud, centroid);\n\n\t// Compute the normals of the point cloud\n\tpcl::NormalEstimation\x3cpcl::PointXYZ, pcl::Normal\x3e ne;\n\tpcl::PointCloud\x3cpcl::Normal\x3e::Ptr cloud_normals(new pcl::PointCloud\x3cpcl::Normal\x3e);\n\tpcl::search::KdTree\x3cpcl::PointXYZ\x3e::Ptr tree(new pcl::search::KdTree\x3cpcl::PointXYZ\x3e);\n\tne.setInputCloud(cloud);\n\tne.setSearchMethod(tree);\n\tne.setKSearch(40);\n\tne.compute(*cloud_normals);\n\n\t// Create a graph with V vertices and E edges\n\tint V = cloud->size();\n\tint E = V * (V - 1) / 2;\n\tGraph graph(V, E);\n\n\t// Calculate the edge weights based on Euclidean distance between points\n\tfor (int i = 0; i \x3c V - 1; ++i){\n\t\tconst auto& src_point = cloud->points[i];\n\t\tfor (int j = i + 1; j \x3c V; ++j){\n\t\t\tconst auto& tgt_point = cloud->points[j];\n\t\t\tfloat distance = euclideanDistance(src_point, tgt_point);\n\t\t\tgraph.addEdge(i, j, distance);\n\t\t}\n\t}\n\n\t// Perform Kruskal's algorithm to find the minimum spanning tree\n\tgraph.KruskalMST(cloud);\n\n\t// 创建一个新的点云对象来保存最小生成树的结果\n\tpcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr new_cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);\n\tstd::vector\x3cEdge\x3e result;\n\tnew_cloud->width = cloud->width;\n\tnew_cloud->height = cloud->height;\n\tnew_cloud->points.resize(cloud->points.size());\n\n\t// 将最小生成树的顶点添加到新的点云对象\n\tfor (const auto& edge : result){\n\t\tconst auto& src_point = cloud->points[edge.src];\n\t\tconst auto& tgt_point = cloud->points[edge.tgt];\n\t\tnew_cloud->points[edge.src] = src_point;\n\t\tnew_cloud->points[edge.tgt] = tgt_point;\n\t}\n\n\t// 将新的点云保存为PLY文件\n\tpcl::io::savePLYFile("D:\DIANYUNWENJIANJIA\newKRUSKAL_ply.ply", *new_cloud, true);\n\n\treturn 0;\n