PCL 点云最小生成树修剪与可视化 - 使用 Kruskal 算法 - 常规

#include//x3ciostream//x3e/n#include//x3cpcl//x2fpoint_types.h//x3e/n#include//x3cpcl//x2fio//x2fply_io.h//x3e/n#include//x3cpcl//x2fvisualization//x2fpcl_visualizer.h//x3e/n#include//x3cpcl//x2fcommon//x2fcentroid.h//x3e/n#include//x3cpcl//x2ffeatures//x2fnormal_3d.h//x3e/n#include//x3cpcl//x2fvisualization//x2fcloud_viewer.h//x3e/n#include//x3cpcl//x2fsegmentation//x2fextract_clusters.h//x3e/n#include//x3cvector//x3e/n#include//x3cunordered_map//x3e/n#include//x22vtkAutoInit.h//x22/n#include//x3cpcl//x2fvisualization//x2fpoint_cloud_color_handlers.h//x3e/nVTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2/n/nusing namespace pcl;/ntypedef pcl::PointXYZ PointT;/ntypedef pcl::PointCloud//x3cPointT//x3e PointCloudT;/n/n// 表示一条边的结构体/nstruct Edge/n{/n/tint src, tgt;/n/tfloat weight;/n};/n/n// 表示并查集的子集的结构体/nstruct Subset/n{/n/tint parent, rank;/n};/n/n// 表示一个连通的、无向的、带权重的图的类/nclass Graph/n{/npublic:/n/tint V, E;/n/tstd::vector//x3cEdge//x3e edges;/n/n/tGraph(int v, int e)/n/t{/n/t/tV = v;/n/t/tE = e;/n/t}/n/n/t// 添加一条边到图中/n/tvoid addEdge(int src, int tgt, float weight)/n/t{/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/n/t// 查找某个元素所在的集合/n/tint find(Subset subsets[], int i)/n/t{/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/n/t// 合并两个集合/n/tvoid Union(Subset subsets[], int x, int y)/n/t{/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/n/t// Kruskal算法找到最小生成树/n/tvoid KruskalMST(PointCloudT::Ptr cloud, std::vector//x3cEdge//x3e& result, float threshold)/n/t{/n/t/t// 将边按照权重从小到大排序/n/t/tstd::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)/n/t/t{/n/t/t/treturn a.weight //x3c b.weight;/n/t/t});/n/n/t/t// 为V个元素创建并查集的子集/n/t/tSubset* subsets = new Subset[V];/n/t/tfor (int v = 0; v //x3c V; ++v)/n/t/t{/n/t/t/tsubsets[v].parent = v;/n/t/t/tsubsets[v].rank = 0;/n/t/t}/n/n/t/tint i = 0; // 用于选择下一条边的索引/n/t/tint e = 0; // 用于选择下一条边加入最小生成树的索引/n/n/t/t// 需要选择V-1条边/n/t/twhile (e //x3c V - 1 && i //x3c E)/n/t/t{/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// 如果加入这条边不会形成环，并且权重大于阈值，则加入到结果中，并且增加已选择边的计数/n/t/t/tif (next_edge.weight //x3e= threshold && x != y)/n/t/t/t{/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// 可视化最小生成树的结果/n/t/tpcl::visualization::PCLVisualizer viewer(/'Minimum Spanning Tree/');/n/t/tviewer.setBackgroundColor(0, 0, 0);/n/n/t/t// 将原始点云添加到可视化窗口中/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// 将最小生成树的边添加到可视化窗口中/n/t/tfor (const auto& edge : result)/n/t/t{/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/tstd::stringstream ss;/n/t/t/tss //x3c//x3c /'edge_/' //x3c//x3c edge.src //x3c//x3c /'/' //x3c//x3c edge.tgt;/n/t/t/tviewer.addLine//x3cpcl::PointXYZ//x3e(src_point, tgt_point, ss.str());/n/t/t}/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{/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/t/twhile (!viewer.wasStopped())/n/t/t{/n/t/t/tviewer.spinOnce();/n/t/t}/n/t}/n};/n/n// 计算两个点之间的欧式距离/ndouble euclideanDistance(PointXYZ p1, PointXYZ p2)/n{/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}/n/nint main()/n{/n/t// 从PLY文件加载输入点云/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// 计算点云的质心/n/tEigen::Vector4f centroid;/n/tpcl::compute3DCentroid(*cloud, centroid);/n/n/t// 计算点云的法线/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// 创建一个有V个顶点和E个边的图/n/tint V = cloud->size();/n/tint E = V * (V - 1) / 2;/n/tGraph graph(V, E);/n/n/t// 基于点之间的欧式距离计算边的权重/n/tfor (int i = 0; i //x3c V - 1; ++i)/n/t{/n/t/tconst auto& src_point = cloud->points[i];/n/t/tfor (int j = i + 1; j //x3c V; ++j)/n/t/t{/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/t// 设置修剪的阈值/n/tfloat threshold = 0.00080;/n/t// 执行Kruskal算法找到最小生成树/n/tstd::vector//x3cEdge//x3e result;/n/tgraph.KruskalMST(cloud, result, threshold);/n// 创建一个新的点云对象保存修剪后的最小生成树结果/n/tpcl::PointCloud//x3cpcl::PointXYZ//x3e::Ptr new_cloud(new pcl::PointCloud//x3cpcl::PointXYZ//x3e);/n/tnew_cloud->width = cloud->width;/n/tnew_cloud->height = cloud->height;/n/tnew_cloud->points.resize(cloud->points.size());/n/n/t// 找到在最小生成树中出现三次的节点/n/tstd::unordered_map//x3cint, int//x3e node_count;/n/tfor (const auto& edge : result)/n/t{/n/t/tnode_count[edge.src]++;/n/t/tnode_count[edge.tgt]++;/n/t}/n/n/tstd::vector//x3cint//x3e threejiedian;/n/tfor (const auto& pair : node_count)/n/t{/n/t/tif (pair.second == 3)/n/t/t{/n/t/t/tthreejiedian.push_back(pair.first);/n/t/t}/n/t}/n/t// 如果与threejiedian相连的边的权重小于0.0008，则从最小生成树中移除该边/n/tstd::vector//x3cEdge//x3e pruned_result;/n/tfor (const auto& edge : result)/n/t{/n/t/tif (std::find(threejiedian.begin(), threejiedian.end(), edge.src) != threejiedian.end() ||/n/t/t/tstd::find(threejiedian.begin(), threejiedian.end(), edge.tgt) != threejiedian.end())/n/t/t{/n/t/t/tif (edge.weight //x3c= 0.008)/n/t/t/t{/n/t/t/t/tpruned_result.push_back(edge);/n/t/t/t}/n/t/t}/n/t/telse/n/t/t{/n/t/t/tpruned_result.push_back(edge);/n/t/t}/n/t}/n/t// 将修剪后的最小生成树的顶点添加到新的点云对象中/n/tfor (const auto& edge : pruned_result)/n/t{/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/t// 将与threejiedian相连的边的顶点设置为绿色/n/tpcl::PointCloud//x3cpcl::PointXYZRGB//x3e::Ptr colored_cloud(new pcl::PointCloud//x3cpcl::PointXYZRGB//x3e);/n/tcolored_cloud->points.resize(cloud->points.size());/n/tfor (const auto& edge : result)/n/t{/n/t/tif (std::find(threejiedian.begin(), threejiedian.end(), edge.src) != threejiedian.end() ||/n/t/t/tstd::find(threejiedian.begin(), threejiedian.end(), edge.tgt) != threejiedian.end())/n/t/t{/n/t/t/tcolored_cloud->points[edge.src].r = 0;/n/t/t/tcolored_cloud->points[edge.src].g = 255;/n/t/t/tcolored_cloud->points[edge.src].b = 0;/n/t/t/tcolored_cloud->points[edge.tgt].r = 0;/n/t/t/tcolored_cloud->points[edge.tgt].g = 255;/n/t/t/tcolored_cloud->points[edge.tgt].b = 0;/n/t/t}/n/t}/n/n/t// 将y值最大的threejiedian节点用绿色小球显示/n/tint max_y_index = -1;/n/tfloat max_y = std::numeric_limits//x3cfloat//x3e::min();/n/tfor (const auto& index : threejiedian)/n/t{/n/t/tconst auto& point = cloud->points[index];/n/t/tif (point.y //x3e max_y)/n/t/t{/n/t/t/tmax_y = point.y;/n/t/t/tmax_y_index = index;/n/t/t}/n/t}/n/tcolored_cloud->points[max_y_index].r = 0;/n/tcolored_cloud->points[max_y_index].g = 255;/n/tcolored_cloud->points[max_y_index].b = 0;/n/n/t// 将最小生成树中出现一次的节点设置为红色/n/tstd::unordered_map//x3cint, int//x3e node_count;/n/tfor (const auto& edge : result)/n/t{/n/t/tnode_count[edge.src]++;/n/t/tnode_count[edge.tgt]++;/n/t}/n/tfor (const auto& pair : node_count)/n/t{/n/t/tif (pair.second == 1)/n/t/t{/n/t/t/tcolored_cloud->points[pair.first].r = 255;/n/t/t/tcolored_cloud->points[pair.first].g = 0;/n/t/t/tcolored_cloud->points[pair.first].b = 0;/n/t/t}/n/t}/n/t// 创建一个可视化窗口/n/tpcl::visualization::PCLVisualizer viewer(/'Pruned Minimum Spanning Tree/');/n/tviewer.setBackgroundColor(0, 0, 0);/n/n/t// 将原始点云添加到可视化窗口中/n/tpcl::visualization::PointCloudColorHandlerCustom//x3cpcl::PointXYZ//x3e single_color(cloud, 255, 255, 255);/n/tviewer.addPointCloud//x3cpcl::PointXYZ//x3e(cloud, single_color, /'original_cloud/');/n/n/t// 将修剪后的最小生成树的边添加到可视化窗口中/n/tfor (const auto& edge : pruned_result)/n/t{/n/t/tconst auto& src_point = cloud->points[edge.src];/n/t/tconst auto& tgt_point = cloud->points[edge.tgt];/n/t/tstd::stringstream ss;/n/t/tss //x3c//x3c /'edge/' //x3c//x3c edge.src //x3c//x3c /'/' //x3c//x3c edge.tgt;/n/t/tviewer.addLine//x3cpcl::PointXYZ//x3e(src_point, tgt_point, ss.str());/n/t}/n/n/twhile (!viewer.wasStopped())/n/t{/n/t/tviewer.spinOnce();/n/t}/n/treturn 0;/n}'**以下是对代码进行的 SEO 优化：**1. 标题: 使用更具描述性的标题，例如 'PCL 点云最小生成树修剪与可视化 - 使用 Kruskal 算法'，并包含关键词 'PCL'、'点云' 和 '最小生成树'。2. 描述: 添加一个简短的描述，解释代码的功能，例如 '本代码使用 PCL 库实现点云最小生成树的修剪与可视化，并使用 Kruskal 算法找到最小生成树，并基于欧式距离计算边的权重。代码最后会可视化原始点云、最小生成树以及修剪后的最小生成树。'。3. 关键词: 添加相关的关键词，例如 'PCL'、'点云'、'最小生成树'、'Kruskal 算法'、'修剪' 和 '可视化'。4. 内容: 对代码进行了格式化，并添加了注释，使其更易于理解。**以下是对内容进行的字符转义:**1. 所有双引号都用 / 进行了转义。2. 所有反斜杠都用 / 进行了转义。3. 代码块被封装在 / 和 / 之间，并使用 /n 进行换行。**以下是代码中添加的修剪后的最小生成树可视化代码:**cpp// 创建一个可视化窗口pcl::visualization::PCLVisualizer viewer('Pruned Minimum Spanning Tree');viewer.setBackgroundColor(0, 0, 0);// 将原始点云添加到可视化窗口中pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ single_color(cloud, 255, 255, 255);viewer.addPointCloudpcl::PointXYZ(cloud, single_color, 'original_cloud');// 将修剪后的最小生成树的边添加到可视化窗口中for (const auto& edge : pruned_result){ const auto& src_point = cloud->points[edge.src]; const auto& tgt_point = cloud->points[edge.tgt]; std::stringstream ss; ss << 'edge' << edge.src << '_' << edge.tgt; viewer.addLinepcl::PointXYZ(src_point, tgt_point, ss.str());}while (!viewer.wasStopped()){ viewer.spinOnce()