C++ 使用 Kruskal 算法提取点云最小生成树 (MST)

#include #include <pcl/point_types.h> #include <pcl/io/ply_io.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/common/centroid.h> #include <pcl/features/normal_3d.h> #include <pcl/visualization/cloud_viewer.h> #include <pcl/segmentation/extract_clusters.h> #include #include <unordered_map> "vtkAutoInit.h"

VTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2

using namespace pcl; typedef pcl::PointXYZ PointT; typedef pcl::PointCloud PointCloudT;

// Structure to represent an edge struct Edge { int src, tgt; float weight; };

// Structure to represent a subset for union-find struct Subset { int parent, rank; };

// Class to represent a connected, undirected and weighted graph class Graph { public: int V, E; std::vector edges;

Graph(int v, int e)
{
	V = v;
	E = e;
}

// Add an edge to the graph
void addEdge(int src, int tgt, float weight)
{
	Edge edge;
	edge.src = src;
	edge.tgt = tgt;
	edge.weight = weight;
	edges.push_back(edge);
}

// Find set of an element i
int find(Subset subsets[], int i)
{
	if (subsets[i].parent != i)
		subsets[i].parent = find(subsets, subsets[i].parent);
	return subsets[i].parent;
}

// Union of two sets x and y
void Union(Subset subsets[], int x, int y)
{
	int xroot = find(subsets, x);
	int yroot = find(subsets, y);
	if (subsets[xroot].rank < subsets[yroot].rank)
		subsets[xroot].parent = yroot;
	else if (subsets[xroot].rank > subsets[yroot].rank)
		subsets[yroot].parent = xroot;
	else {
		subsets[yroot].parent = xroot;
		subsets[xroot].rank++;
	}
}

// Kruskal's algorithm to find MST
void KruskalMST(PointCloudT::Ptr cloud, std::vector<Edge>& result)
{
	// ...
	// Number of edges to be taken is equal to V-1
	while (e < V - 1 && i < E)
	{
		// ...
	}
}

}; 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() { // Load the input point cloud from PLY file pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ); pcl::io::loadPLYFilepcl::PointXYZ("D:\DIANYUNWENJIANJIA\newOUSHIJULEI_ply.ply", *cloud);

// Compute the centroid of the point cloud
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);

// Compute the normals of the point cloud
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
ne.setInputCloud(cloud);
ne.setSearchMethod(tree);
ne.setKSearch(40);
ne.compute(*cloud_normals);

// Create a graph with V vertices and E edges
int V = cloud->size();
int E = V * (V - 1) / 2;
Graph graph(V, E);

// Calculate the edge weights based on Euclidean distance between points
for (int i = 0; i < V - 1; ++i)
{
	const auto& src_point = cloud->points[i];
	for (int j = i + 1; j < V; ++j)
	{
		const auto& tgt_point = cloud->points[j];
		float distance = euclideanDistance(src_point, tgt_point);
		graph.addEdge(i, j, distance);
	}
}

// Perform Kruskal's algorithm to find the minimum spanning tree
std::vector<Edge> result;
graph.KruskalMST(cloud, result);
// 创建一个新的点云对象来保存最小生成树的结果
pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZ>);
new_cloud->width = cloud->width;
new_cloud->height = cloud->height;
new_cloud->points.resize(cloud->points.size());

// 将最小生成树的顶点添加到新的点云对象
for (const auto& edge : result)
{
	const auto& src_point = cloud->points[edge.src];
	const auto& tgt_point = cloud->points[edge.tgt];
	new_cloud->points[edge.src] = src_point;
	new_cloud->points[edge.tgt] = tgt_point;
}

// 将新的点云保存为PLY文件
pcl::io::savePLYFile("D:\\DIANYUNWENJIANJIA\\newKRUSKAL_ply.ply", *new_cloud, true);
return 0;

}