PCL 点云最小生成树 (MST) 可视化 - Kruskal 算法

{"title":"#include "iostream"\n#include "pcl/point_types.h"\n#include "pcl/io/ply_io.h"\n#include "pcl/visualization/pcl_visualizer.h"\n#include "pcl/common/centroid.h"\n#include "pcl/features/normal_3d.h"\n#include "pcl/visualization/cloud_viewer.h"\n#include "pcl/segmentation/extract_clusters.h"\n#include "vector"\n#include "unordered_map"\n"#include "vtkAutoInit.h""\n\nVTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2\n\nusing namespace pcl;\ntypedef pcl::PointXYZ PointT;\ntypedef pcl::PointCloud PointCloudT;\n\n// Structure to represent an edge\nstruct Edge\n{\n int src, tgt;\n float weight;\n};\n\n// Structure to represent a subset for union-find\nstruct Subset\n{\n int parent, rank;\n};\n\n// Class to represent a connected, undirected and weighted graph\nclass Graph\n{\npublic:\n int V, E;\n std::vector edges;\n\n Graph(int v, int e)\n {\n V = v;\n E = e;\n }\n\n // Add an edge to the graph\n void addEdge(int src, int tgt, float weight)\n {\n Edge edge;\n edge.src = src;\n edge.tgt = tgt;\n edge.weight = weight;\n edges.push_back(edge);\n }\n\n // Find set of an element i\n int find(Subset subsets[], int i)\n {\n if (subsets[i].parent != i)\n subsets[i].parent = find(subsets, subsets[i].parent);\n return subsets[i].parent;\n }\n\n // Union of two sets x and y\n void Union(Subset subsets[], int x, int y)\n {\n int xroot = find(subsets, x);\n int yroot = find(subsets, y);\n if (subsets[xroot].rank < subsets[yroot].rank)\n subsets[xroot].parent = yroot;\n else if (subsets[xroot].rank > subsets[yroot].rank)\n subsets[yroot].parent = xroot;\n else {\n subsets[yroot].parent = xroot;\n subsets[xroot].rank++;\n }\n }\n\n // Kruskal's algorithm to find MST\n void KruskalMST(PointCloudT::Ptr cloud, std::vector& result)\n {\n // Sort all the edges in non-decreasing order of their weight\n std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)\n {\n return a.weight < b.weight;\n });\n\n // Allocate memory for creating V subsets\n Subset* subsets = new Subset[V];\n for (int v = 0; v < V; ++v)\n {\n subsets[v].parent = v;\n subsets[v].rank = 0;\n }\n\n int i = 0; // Index used to pick the next smallest edge\n int e = 0; // Index used to pick the next edge to include in MST\n\n // Number of edges to be taken is equal to V-1\n while (e < V - 1 && i < E)\n {\n Edge next_edge = edges[i++];\n\n int x = find(subsets, next_edge.src);\n int y = find(subsets, next_edge.tgt);\n\n // If including this edge doesn't cause a cycle, include it in result and increment the index of result for next edge\n if (x != y && next_edge.weight < 0.045)\n {\n result.push_back(next_edge);\n Union(subsets, x, y);\n ++e;\n }\n }\n\n // Visualize the resulting minimum spanning tree\n pcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree");\n viewer.setBackgroundColor(0, 0, 0);\n\n // Add the original point cloud to the viewer\n pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ single_color(cloud, 255, 255, 255);\n viewer.addPointCloudpcl::PointXYZ(cloud, single_color, "original_cloud");\n\n // Connect all edges in the minimum spanning tree\n for (const auto& edge : result)\n {\n const auto& src_point = cloud->points[edge.src];\n const auto& tgt_point = cloud->points[edge.tgt];\n viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(src_point, tgt_point, 0, 255, 0, "line");\n }\n\n // Count the number of points in the minimum spanning tree\n int numPoints = 0;\n for (const auto& edge : result)\n {\n const auto& src_point = cloud->points[edge.src];\n const auto& tgt_point = cloud->points[edge.tgt];\n numPoints += 2;\n }\n\n std::cout << "Number of points in the minimum spanning tree: " << numPoints << std::endl;\n\n while (!viewer.wasStopped())\n {\n viewer.spinOnce();\n }\n\n }\n};\n\ndouble euclideanDistance(PointXYZ p1, PointXYZ p2)\n{\n double dx = p2.x - p1.x;\n double dy = p2.y - p1.y;\n double dz = p2.z - p1.z;\n return std::sqrt(dx * dx + dy * dy + dz * dz);\n}\n\nint main()\n{\n // Load the input point cloud from PLY file\n pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ);\n pcl::io::loadPLYFilepcl::PointXYZ("D:\DIANYUNWENJIANJIA\newOUSHIJULEI_ply.ply", *cloud);\n\n // Compute the centroid of the point cloud\n Eigen::Vector4f centroid;\n pcl::compute3DCentroid(*cloud, centroid);\n\n // Compute the normals of the point cloud\n pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;\n pcl::PointCloudpcl::Normal::Ptr cloud_normals(new pcl::PointCloudpcl::Normal);\n pcl::search::KdTreepcl::PointXYZ::Ptr tree(new pcl::search::KdTreepcl::PointXYZ);\n ne.setInputCloud(cloud);\n ne.setSearchMethod(tree);\n ne.setKSearch(40);\n ne.compute(*cloud_normals);\n\n // Create a graph with V vertices and E edges\n int V = cloud->size();\n int E = V * (V - 1) / 2;\n Graph graph(V, E);\n\n // Calculate the edge weights based on Euclidean distance between points\n for (int i = 0; i < V - 1; ++i)\n {\n const auto& src_point = cloud->points[i];\n for (int j = i + 1; j < V; ++j)\n {\n const auto& tgt_point = cloud->points[j];\n float distance = euclideanDistance(src_point, tgt_point);\n graph.addEdge(i, j, distance);\n }\n }\n\n // Perform Kruskal's algorithm to find the minimum spanning tree\n std::vector result;\n graph.KruskalMST(cloud, result);\n\n // Create a new point cloud object to save the result of the minimum spanning tree\n pcl::PointCloudpcl::PointXYZ::Ptr new_cloud(new pcl::PointCloudpcl::PointXYZ);\n new_cloud->width = cloud->width;\n new_cloud->height = cloud->height;\n new_cloud->points.resize(cloud->points.size());\n\n // Add the vertices of the minimum spanning tree to the new point cloud object\n for (const auto& edge : result)\n {\n const auto& src_point = cloud->points[edge.src];\n const auto& tgt_point = cloud->points[edge.tgt];\n new_cloud->points[edge.src] = src_point;\n new_cloud->points[edge.tgt] = tgt_point;\n }\n\n // Save the new point cloud as a PLY file\n pcl::io::savePLYFile("D:\DIANYUNWENJIANJIA\newKRUSKAL_ply.ply", *new_cloud, true);\n return 0;\n}