PCL Minimum Spanning Tree (MST) Visualization using Kruskal's Algorithm

#include\x3ciostream\x3e#include\x3cpcl/point_types.h\x3e#include\x3cpcl/io/ply_io.h\x3e#include\x3cpcl/visualization/pcl_visualizer.h\x3e#include\x3cpcl/common/centroid.h\x3e#include\x3cpcl/features/normal_3d.h\x3e#include\x3cpcl/visualization/cloud_viewer.h\x3e#include\x3cpcl/segmentation/extract_clusters.h\x3e#include\x3cvector\x3e#include\x3cunordered_map\x3e#include\x22vtkAutoInit.h\x22VTK_MODULE_INIT(vtkRenderingOpenGL);using namespace pcl;typedef pcl::PointXYZ PointT;typedef pcl::PointCloud\x3cPointT\x3e PointCloudT;struct Edge{int src, tgt;float weight;};struct Subset{int parent, rank;};class Graph{public:int V, E;std::vector\x3cEdge\x3e edges;Graph(int v, int e){V = v;E = e;}void addEdge(int src, int tgt, float weight){Edge edge;edge.src = src;edge.tgt = tgt;edge.weight = weight;edges.push_back(edge);}int find(Subset subsets[], int i){if (subsets[i].parent != i)subsets[i].parent = find(subsets, subsets[i].parent);return subsets[i].parent;}void Union(Subset subsets[], int x, int y){int xroot = find(subsets, x);int yroot = find(subsets, y);if (subsets[xroot].rank \x3c subsets[yroot].rank)subsets[xroot].parent = yroot;else if (subsets[xroot].rank \x3e subsets[yroot].rank)subsets[yroot].parent = xroot;else{subsets[yroot].parent = xroot;subsets[xroot].rank++;}}void KruskalMST(PointCloudT::Ptr cloud){std::vector\x3cEdge\x3e result;std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b){return a.weight \x3c b.weight;});Subset* subsets = new Subset[V];for (int v = 0; v \x3c V; ++v){subsets[v].parent = v;subsets[v].rank = 0;}int i = 0;int e = 0;while (e \x3c V - 1 && i \x3c E){Edge next_edge = edges[i++];int x = find(subsets, next_edge.src);int y = find(subsets, next_edge.tgt);if (x != y && next_edge.weight \x3c 0.045){result.push_back(next_edge);Union(subsets, x, y);++e;}}pcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree");viewer.setBackgroundColor(1, 1, 1);pcl::visualization::PointCloudColorHandlerCustom\x3cpcl::PointXYZ\x3e single_color(cloud, 0, 0, 0);viewer.addPointCloud\x3cpcl::PointXYZ\x3e(cloud, single_color, "original_cloud");for (const auto& edge : result){const auto& src_point = cloud->points[edge.src];const auto& tgt_point = cloud->points[edge.tgt];std::stringstream ss;ss \x3c\x3c "edge_" \x3c\x3c edge.src \x3c\x3c "_" \x3c\x3c edge.tgt;viewer.addLine\x3cpcl::PointXYZ\x3e(src_point, tgt_point, ss.str());}pcl::PointXYZ point1 = cloud->points[55];pcl::PointXYZ point2 = cloud->points[56];viewer.addLine\x3cpcl::PointXYZ, pcl::PointXYZ\x3e(point1, point2, 0, 255, 0, "line");std::unordered_map\x3cint, int\x3e countMap;for (const auto& edge : result){countMap[edge.src]++;countMap[edge.tgt]++;}pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr jie(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);int begin = 0;for (const auto& pair : countMap){if (pair.second \x3e= 3){std::cout \x3c\x3c "该点坐标： " \x3c\x3c pair.first \x3c\x3c " 出现 " \x3c\x3c pair.second \x3c\x3c " 次" \x3c\x3c std::endl;std::cout \x3c\x3c cloud->points[pair.first] \x3c\x3c std::endl;pcl::PointXYZ point3 = cloud->points[pair.first];std::stringstream sa;sa \x3c\x3c begin;begin++;viewer.addSphere(pcl::PointXYZ(point3), 0.0023, 1, 0, 0, sa.str());jie->push_back(point3);}}pcl::PointXYZ maxPoint;pcl::PointXYZ minPoint;float maxY = -1;float minY = 1;for (size_t i = 0; i \x3c jie->size(); ++i){pcl::PointXYZ point = jie->at(i);if (point.y \x3e maxY){maxY = point.y;maxPoint = point;}if (point.y \x3c minY){minY = point.y;minPoint = point;}}std::cout \x3c\x3c "节点上的最高点为 (" \x3c\x3c maxPoint.x \x3c\x3c ", " \x3c\x3c maxPoint.y \x3c\x3c ", " \x3c\x3c maxPoint.z \x3c\x3c ")" \x3c\x3c std::endl;std::cout \x3c\x3c "节点上的最低点为 (" \x3c\x3c minPoint.x \x3c\x3c ", " \x3c\x3c minPoint.y \x3c\x3c ", " \x3c\x3c minPoint.z \x3c\x3c ")" \x3c\x3c std::endl;viewer.addSphere(pcl::PointXYZ(maxPoint), 0.0023, 0, 255, 0);while (!viewer.wasStopped()){viewer.spinOnce();}}};double euclideanDistance(PointXYZ p1, PointXYZ p2){double dx = p2.x - p1.x;double dy = p2.y - p1.y;double dz = p2.z - p1.z;return std::sqrt(dx*dx + dy * dy + dz * dz);}int main(){pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);pcl::io::loadPLYFile\x3cpcl::PointXYZ\x3e("E:\dianyun\guo\output8.ply", *cloud);Eigen::Vector4f centroid;pcl::compute3DCentroid(*cloud, centroid);pcl::NormalEstimation\x3cPointT, pcl::Normal\x3e ne;pcl::PointCloud\x3cpcl::Normal\x3e::Ptr cloud_normals(new pcl::PointCloud\x3cpcl::Normal\x3e);pcl::search::KdTree\x3cPointT\x3e::Ptr tree(new pcl::search::KdTree\x3cPointT\x3e);ne.setInputCloud(cloud);ne.setSearchMethod(tree);ne.setKSearch(20);ne.compute(*cloud_normals);int V = cloud->size();int E = V * (V - 1) / 2;Graph graph(V, E);for (int i = 0; i \x3c V - 1; ++i){const auto& src_point = cloud->points[i];for (int j = i + 1; j \x3c V; ++j){const auto& tgt_point = cloud->points[j];float distance = euclideanDistance(src_point, tgt_point);graph.addEdge(i, j, distance);}}graph.KruskalMST(cloud);return 0