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\nVTK_MODULE_INIT(vtkRenderingOpenGL);\x20//\x20VTK\x20was\x20built\x20with\x20vtkRenderingOpenGL2\n\nusing\x20namespace\x20pcl;
typedef\x20pcl::PointXYZ\x20PointT;
typedef\x20pcl::PointCloud\x3cPointT\x3e\x20PointCloudT;
\n//\x20Structure\x20to\x20represent\x20an\x20edge
struct\x20Edge
{
\x20\x20int\x20src, tgt;
\x20\x20float\x20weight;
};
\n//\x20Structure\x20to\x20represent\x20a\x20subset\x20for\x20union-find
struct\x20Subset
{
\x20\x20int\x20parent, rank;
};
\n//\x20Class\x20to\x20represent\x20a\x20connected, undirected\x20and\x20weighted\x20graph
class\x20Graph
{
public:
\x20\x20int\x20V, E;
\x20\x20std::vector\x3cEdge\x3e\x20edges;
\n\x20\x20Graph(int\x20v, int\x20e)
\x20\x20{
\x20\x20\x20\x20V\x20=\x20v;
\x20\x20\x20\x20E\x20=\x20e;
\x20\x20}
\n\x20\x20//\x20Add\x20an\x20edge\x20to\x20the\x20graph
\x20\x20void\x20addEdge(int\x20src, int\x20tgt, float\x20weight)
\x20\x20{
\x20\x20\x20\x20Edge\x20edge;
\x20\x20\x20\x20edge.src\x20=\x20src;
\x20\x20\x20\x20edge.tgt\x20=\x20tgt;
\x20\x20\x20\x20edge.weight\x20=\x20weight;
\x20\x20\x20\x20edges.push_back(edge);
\x20\x20}
\n\x20\x20//\x20Find\x20set\x20of\x20an\x20element\x20i
\x20\x20int\x20find(Subset\x20subsets[], int\x20i)
\x20\x20{
\x20\x20\x20\x20if\x20(subsets[i].parent\x20!=\x20i)
\x20\x20\x20\x20\x20\x20subsets[i].parent\x20=\x20find(subsets, subsets[i].parent);
\x20\x20\x20\x20return\x20subsets[i].parent;
\x20\x20}
\n\x20\x20//\x20Union\x20of\x20two\x20sets\x20x\x20and\x20y
\x20\x20void\x20Union(Subset\x20subsets[], int\x20x, int\x20y)
\x20\x20{
\x20\x20\x20\x20int\x20xroot\x20=\x20find(subsets, x);
\x20\x20\x20\x20int\x20yroot\x20=\x20find(subsets, y);
\x20\x20\x20\x20if\x20(subsets[xroot].rank\x20\x3c\x20subsets[yroot].rank)
\x20\x20\x20\x20\x20\x20subsets[xroot].parent\x20=\x20yroot;
\x20\x20\x20\x20else\x20if\x20(subsets[xroot].rank\x20\x3e\x20subsets[yroot].rank)
\x20\x20\x20\x20\x20\x20subsets[yroot].parent\x20=\x20xroot;
\x20\x20\x20\x20else\x20{
\x20\x20\x20\x20\x20\x20subsets[yroot].parent\x20=\x20xroot;
\x20\x20\x20\x20\x20\x20subsets[xroot].rank++;
\x20\x20\x20\x20}
\x20\x20}
\n\x20\x20//\x20Kruskal's\x20algorithm\x20to\x20find\x20MST
\x20\x20void\x20KruskalMST(PointCloudT::Ptr\x20cloud)
\x20\x20{
\x20\x20\x20\x20std::vector\x3cEdge\x3e\x20result; // This will store the resultant MST
\n\x20\x20\x20\x20// Sort all the edges in non-decreasing order of their weight
\x20\x20\x20\x20std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20return\x20a.weight\x20\x3c\x20b.weight;
\x20\x20\x20\x20});
\n\x20\x20\x20\x20// Allocate memory for creating V subsets
\x20\x20\x20\x20Subset*\x20subsets\x20=\x20new\x20Subset[V];
\x20\x20\x20\x20for\x20(int\x20v\x20=\x200; v\x20\x3c\x20V; ++v)
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20subsets[v].parent\x20=\x20v;
\x20\x20\x20\x20\x20\x20subsets[v].rank\x20=\x200;
\x20\x20\x20\x20}
\n\x20\x20\x20\x20int\x20i\x20=\x200; // Index used to pick the next smallest edge
\x20\x20\x20\x20int\x20e\x20=\x200; // Index used to pick the next edge to include in MST
\n\x20\x20\x20\x20// Number of edges to be taken is equal to V-1
\x20\x20\x20\x20while\x20(e\x20\x3c\x20V\x20-\x201\x20&&\x20i\x20\x3c\x20E)
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20Edge\x20next_edge\x20=\x20edges[i++];
\n\x20\x20\x20\x20\x20\x20int\x20x\x20=\x20find(subsets, next_edge.src);
\x20\x20\x20\x20\x20\x20int\x20y\x20=\x20find(subsets, next_edge.tgt);
\n\x20\x20\x20\x20\x20\x20// If including this edge doesn't cause a cycle, include it in result and increment the index of result for next edge
\x20\x20\x20\x20\x20\x20if\x20(x\x20!=\x20y\x20&&\x20next_edge.weight\x20\x3c\x200.045)
\x20\x20\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20\x20\x20result.push_back(next_edge);
\x20\x20\x20\x20\x20\x20\x20\x20Union(subsets, x, y);
\x20\x20\x20\x20\x20\x20\x20\x20++e;
\x20\x20\x20\x20\x20\x20}
\x20\x20\x20\x20}
\n\x20\x20\x20\x20// Visualize the resulting minimum spanning tree
\x20\x20\x20\x20pcl::visualization::PCLVisualizer\x20viewer("Minimum Spanning Tree");
\x20\x20\x20\x20viewer.setBackgroundColor(0, 0, 0);
\n\x20\x20\x20\x20// Add the original point cloud to the viewer
\x20\x20\x20\x20pcl::visualization::PointCloudColorHandlerCustom\x3cpcl::PointXYZ\x3e\x20single_color(cloud, 255, 255, 255);
\x20\x20\x20\x20viewer.addPointCloud\x3cpcl::PointXYZ\x3e(cloud, single_color, "original_cloud");
\n\x20\x20\x20\x20// Count the number of points in the minimum spanning tree
\x20\x20\x20\x20int\x20numPoints\x20=\x200;
\x20\x20\x20\x20for\x20(const\x20auto&\x20edge\x20:\x20result)
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20const\x20auto&\x20src_point\x20=\x20cloud->points[edge.src];
\x20\x20\x20\x20\x20\x20const\x20auto&\x20tgt_point\x20=\x20cloud->points[edge.tgt];
\x20\x20\x20\x20\x20\x20numPoints\x20+=\x202;
\x20\x20\x20\x20}
\n\x20\x20\x20\x20std::cout\x20\x3c\x3c\x20"Number\x20of\x20points\x20in\x20the\x20minimum\x20spanning\x20tree:\x20"\x20\x3c\x3c\x20numPoints\x20\x3c\x3c\x20std::endl;
\n\x20\x20\x20\x20// Add point indices to viewer and set the callback function for mouse events
\x20\x20\x20\x20for\x20(int\x20i\x20=\x200; i\x20\x3c\x20cloud->size(); ++i)
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20std::string\x20point_id\x20=\x20"point_"\x20+\x20std::to_string(i);
\x20\x20\x20\x20\x20\x20viewer.addText3D(std::to_string(i), cloud->points[i], 0.1, 1.0, 1.0, 1.0, point_id);
\x20\x20\x20\x20}
\n\x20\x20\x20\x20struct\x20PointIndexCallback
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20void\x20operator()(const\x20pcl::visualization::PointPickingEvent&\x20event, void*\x20viewer_void)
\x20\x20\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20\x20\x20int\x20point_index\x20=\x20event.getPointIndex();
\x20\x20\x20\x20\x20\x20\x20\x20if\x20(point_index\x20!=\x20-1)
\x20\x20\x20\x20\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20std::cout\x20\x3c\x3c\x20"Clicked\x20point\x20index:\x20"\x20\x3c\x3c\x20point_index\x20\x3c\x3c\x20std::endl;
\x20\x20\x20\x20\x20\x20\x20\x20}
\x20\x20\x20\x20\x20\x20}
\x20\x20\x20\x20};
\n\x20\x20\x20\x20PointIndexCallback\x20callback;
\x20\x20\x20\x20viewer.registerPointPickingCallback(callback, (void*)&viewer);
\n\x20\x20\x20\x20while\x20(!viewer.wasStopped())
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20viewer.spinOnce();
\x20\x20\x20\x20}
\n\x20\x20}
};
double euclideanDistance(PointXYZ p1, PointXYZ p2)
{
\x20\x20double dx = p2.x - p1.x;
\x20\x20double dy = p2.y - p1.y;
\x20\x20double dz = p2.z - p1.z;
\x20\x20return std::sqrt(dx*dx + dy * dy + dz * dz);
}
\nint main()
{
\x20\x20// Load the input point cloud from PLY file
\x20\x20pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);
\x20\x20pcl::io::loadPLYFile\x3cpcl::PointXYZ\x3e("D:\DIANYUNWENJIANJIA\newOUSHIJULEI_ply.ply", *cloud);
\n\x20\x20// Compute the centroid of the point cloud
\x20\x20Eigen::Vector4f centroid;
\x20\x20pcl::compute3DCentroid(*cloud, centroid);
\n\x20\x20// Compute the normals of the point cloud
\x20\x20pcl::NormalEstimation\x3cpcl::PointXYZ, pcl::Normal\x3e ne;
\x20\x20pcl::PointCloud\x3cpcl::Normal\x3e::Ptr cloud_normals(new pcl::PointCloud\x3cpcl::Normal\x3e);
\x20\x20pcl::search::KdTree\x3cpcl::PointXYZ\x3e::Ptr tree(new pcl::search::KdTree\x3cpcl::PointXYZ\x3e);
\x20\x20ne.setInputCloud(cloud);
\x20\x20ne.setSearchMethod(tree);
\x20\x20ne.setKSearch(40);
\x20\x20ne.compute(*cloud_normals);
\n\x20\x20// Create a graph with V vertices and E edges
\x20\x20int V = cloud->size();
\x20\x20int E = V * (V - 1) / 2;
\x20\x20Graph graph(V, E);
\n\x20\x20// Calculate the edge weights based on Euclidean distance between points
\x20\x20for\x20(int i = 0; i \x3c V - 1; ++i)
\x20\x20{
\x20\x20\x20\x20const auto& src_point = cloud->points[i];
\x20\x20\x20\x20for\x20(int j = i + 1; j \x3c V; ++j)
\x20\x20\x20\x20{
\x20\x20\x20\x20\x20\x20const auto& tgt_point = cloud->points[j];
\x20\x20\x20\x20\x20\x20float distance = euclideanDistance(src_point, tgt_point);
\x20\x20\x20\x20\x20\x20graph.addEdge(i, j, distance);
\x20\x20\x20\x20}
\x20\x20}
\n\x20\x20// Perform Kruskal's algorithm to find the minimum spanning tree
\x20\x20graph.KruskalMST(cloud);
\n\x20\x20// Create a new point cloud object to save the result of the minimum spanning tree
\x20\x20pcl::PointCloud\x3cpcl::PointXYZ\x3e::Ptr new_cloud(new pcl::PointCloud\x3cpcl::PointXYZ\x3e);
\x20\x20std::vector\x3cEdge\x3e result;
\x20\x20new_cloud->width = cloud->width;
\x20\x20new_cloud->height = cloud->height;
\x20\x20new_cloud->points.resize(cloud->points.size());
\n\x20\x20// Add the vertices of the minimum spanning tree to the new point cloud object
\x20\x20for\x20(const auto& edge : result)
\x20\x20{
\x20\x20\x20\x20const auto& src_point = cloud->points[edge.src];
\x20\x20\x20\x20const auto& tgt_point = cloud->points[edge.tgt];
\x20\x20\x20\x20new_cloud->points[edge.src] = src_point;
\x20\x20\x20\x20new_cloud->points[edge.tgt] = tgt_point;
\x20\x20}
\n\x20\x20// Save the new point cloud as a PLY file
\x20\x20pcl::io::savePLYFile("D:\DIANYUNWENJIANJIA\newKRUSKAL_ply.ply", *new_cloud, true);
\x20\x20return 0;
}\n
原文地址: https://www.cveoy.top/t/topic/pNdi 著作权归作者所有。请勿转载和采集!