PCL点云最小生成树修剪与可视化

#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#include\x3cpcl/visualization/point_cloud_color_handlers.h\x3e\nVTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2\n\nusing 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++;}} // Kruskal算法找到最小生成树void KruskalMST(PointCloudT::Ptr cloud, std::vector\x3cEdge\x3e& result, float threshold){ // 将边按照权重从小到大排序std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b){return a.weight \x3c b.weight;}); // 为V个元素创建并查集的子集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; // 用于选择下一条边加入最小生成树的索引 // 需要选择V-1条边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 (next_edge.weight \x3e= threshold && x != y){result.push_back(next_edge);Union(subsets, x, y);++e;}} // 可视化最小生成树的结果pcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree");viewer.setBackgroundColor(0, 0, 0); // 将原始点云添加到可视化窗口中pcl::visualization::PointCloudColorHandlerCustom\x3cpcl::PointXYZ\x3e single_color(cloud, 255, 255, 255);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());} // Count the number of points in the minimum spanning treeint numPoints = 0;for (const auto& edge : result){const auto& src_point = cloud->points[edge.src];const auto& tgt_point = cloud->points[edge.tgt];numPoints += 2;} std::cout \x3c\x3c "Number of points in the minimum spanning tree: " \x3c\x3c numPoints \x3c\x3c std::endl; // 将修剪后的最小生成树的顶点添加到新的点云对象中PointCloudT::Ptr new_cloud(new PointCloudT);for (const auto& edge : result){const auto& src_point = cloud->points[edge.src];const auto& tgt_point = cloud->points[edge.tgt];new_cloud->push_back(src_point);new_cloud->push_back(tgt_point);} // 将最小生成树中出现三次的节点threejiedian的颜色设置为绿色std::unordered_map\x3cint, int\x3e count_map;for (const auto& edge : result){count_map[edge.src]++;count_map[edge.tgt]++;} for (const auto& count_pair : count_map){if (count_pair.second \x3e= 3){new_cloud->points[count_pair.first].r = 0;new_cloud->points[count_pair.first].g = 255;new_cloud->points[count_pair.first].b = 0;}} // 将最小生成树中出现一次的节点颜色设置为红色for (const auto& count_pair : count_map){if (count_pair.second == 1){new_cloud->points[count_pair.first].r = 255;new_cloud->points[count_pair.first].g = 0;new_cloud->points[count_pair.first].b = 0;}} // 执行新的可视化pcl::visualization::PCLVisualizer new_viewer("Pruned Minimum Spanning Tree");new_viewer.setBackgroundColor(0, 0, 0);pcl::visualization::PointCloudColorHandlerRGBField\x3cpcl::PointXYZ\x3e rgb(new_cloud);new_viewer.addPointCloud\x3cpcl::PointXYZ\x3e(new_cloud, rgb, "pruned_cloud");while (!new_viewer.wasStopped()){new_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(){ // 从PLY文件加载输入点云pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);pcl::io::loadPLYFile\x3cpcl::PointXYZ\x3e("D:/DIANYUNWENJIANJIA/newOUSHIJULEI_ply.ply", *cloud); // 计算点云的质心Eigen::Vector4f centroid;pcl::compute3DCentroid(*cloud, centroid); // 计算点云的法线pcl::NormalEstimation\x3cpcl::PointXYZ, pcl::Normal\x3e ne;pcl::PointCloud\x3cpcl::Normal\x3e::Ptr cloud_normals(new pcl::PointCloud\x3cpcl::Normal\x3e);pcl::search::KdTree\x3cpcl::PointXYZ\x3e::Ptr tree(new pcl::search::KdTree\x3cpcl::PointXYZ\x3e);ne.setInputCloud(cloud);ne.setSearchMethod(tree);ne.setKSearch(40);ne.compute(*cloud_normals); // 创建一个有V个顶点和E个边的图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);}} // 设置修剪的阈值float threshold = 0.00080; // 执行Kruskal算法找到最小生成树std::vector\x3cEdge\x3e result;graph.KruskalMST(cloud, result, threshold);return 0