• 日期: 2027-04-02
  • 标签: 常规

点云最小生成树可视化与保存 - 基于Kruskal算法

本文将介绍如何使用Kruskal算法计算点云最小生成树，并将其可视化并保存为PLY文件。代码使用PCL库实现，包含点云加载、质心计算、法向量计算、图构建、最小生成树计算、可视化和保存等步骤。

1. 代码示例：

#include <iostream>
#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 <vector>
#include <unordered_map>
"vtkAutoInit.h"

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

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

// 结构体表示边
struct Edge
{
	int src, tgt;
	float weight;
};

// 结构体表示并查集中的子集
struct Subset
{
	int parent, rank;
};

// 表示一个连接的、无向的、有权的图的类
class Graph
{
public:
	int V, E;
	std::vector<Edge> 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);
	}

	// 查找元素i的集合
	int find(Subset subsets[], int i)
	{
		if (subsets[i].parent != i)
			subsets[i].parent = find(subsets, subsets[i].parent);
		return subsets[i].parent;
	}

	// 合并两个集合x和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算法找到最小生成树
	void KruskalMST(PointCloudT::Ptr cloud)
	{
		std::vector<Edge> result;  // 存储最小生成树的边

		// 将所有边按权重非递减的顺序排序
		std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)
		{
			return a.weight < b.weight;
		});

		// 为创建V个子集分配内存
		Subset* subsets = new Subset[V];
		for (int v = 0; v < V; ++v)
		{
			subsets[v].parent = v;
			subsets[v].rank = 0;
		}

		int i = 0;  // 用于选择下一个最小边的索引
		int e = 0;  // 用于选择下一条要包含在MST中的边的索引

		// 要选择的边的数量等于V-1
		while (e < V - 1 && i < 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 < 0.045)
			{
				result.push_back(next_edge);
				Union(subsets, x, y);
				++e;
			}
		}

		// 可视化结果的最小生成树
		pcl::visualization::PCLVisualizer viewer("最小生成树");
		viewer.setBackgroundColor(1, 1, 1);

		// 将原始点云添加到视图器中
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 0, 0);
		viewer.addPointCloud<pcl::PointXYZ>(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 << "edge_" << edge.src << "_" << edge.tgt;
			viewer.addLine<pcl::PointXYZ>(src_point, tgt_point, ss.str());
		}

		/* Connect two disconnected edges
		pcl::PointXYZ point1 = cloud->points[55];
		pcl::PointXYZ point2 = cloud->points[56];
		viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(point1, point2, 0, 255, 0, "line");
		*/
		// 找到y方向上值最大的节点，并将其标记为绿色
		std::unordered_map<int, int> countMap;
		for (const auto& edge : result)
		{
			countMap[edge.src]++;
			countMap[edge.tgt]++;
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr jie(new pcl::PointCloud<pcl::PointXYZ>);
		int begin = 0;
		for (const auto& pair : countMap)
		{
			if (pair.second >= 3)
			{
				std::cout << "该点坐标： " << pair.first << " 出现 " << pair.second << " 次" << std::endl;
				std::cout << cloud->points[pair.first] << std::endl;
				pcl::PointXYZ point3 = cloud->points[pair.first];

				std::stringstream sa;
				sa << 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 < jie->size(); ++i)
		{
			pcl::PointXYZ point = jie->at(i);
			if (point.y > maxY)
			{
				maxY = point.y;
				maxPoint = point;
			}
			if (point.y < minY)
			{
				minY = point.y;
				minPoint = point;
			}
		}

		std::cout << "节点上的最高点为 (" << maxPoint.x << ", " << maxPoint.y << ", " << maxPoint.z << ")" << std::endl;
		std::cout << "节点上的最低点为 (" << minPoint.x << ", " << minPoint.y << ", " << minPoint.z << ")" << 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()
{
	// 从PLY文件中加载输入点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPLYFile<pcl::PointXYZ>("D:\\DIANYUNWENJIANJIA\\OUSHIJULEI_ply.ply", *cloud);

	// 计算点云的质心
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);

	// 计算点云的法向量
	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(10);
	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 < 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);
		}
	}

	// 执行Kruskal算法找到最小生成树
	graph.KruskalMST(cloud);

	// 创建一个新的点云对象来保存最小生成树的结果
	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;
}

2. 代码解读：

  • 点云加载和预处理：

  • 使用pcl::io::loadPLYFile()函数加载点云数据。

  • 使用pcl::compute3DCentroid()函数计算点云的质心。

  • 使用pcl::NormalEstimation类计算点云的法向量，并使用KdTree进行近邻搜索。

  • 图构建：

  • 创建一个Graph类，该类包含顶点数量V和边数量E。

  • 使用addEdge()函数将点之间的距离作为权重添加到图中。

  • 最小生成树计算：

  • 使用KruskalMST()函数，该函数使用Kruskal算法计算最小生成树。

  • 算法将边按权重从小到大排序，并使用并查集数据结构来避免环路。

  • 可视化：

  • 使用pcl::visualization::PCLVisualizer类创建可视化窗口。

  • 添加原始点云和最小生成树的边。

  • 找到y方向上值最大的节点，并将其标记为绿色。

  • 保存结果：

  • 创建一个新的点云对象new_cloud，并将最小生成树的顶点添加到该对象中。

  • 使用pcl::io::savePLYFile()函数将new_cloud保存为PLY文件。

3. 总结：

本文介绍了如何使用Kruskal算法计算点云最小生成树，并将其可视化并保存为PLY文件。该代码可以帮助您分析点云数据并提取其关键结构。

4. 应用场景：

  • **点云分割：**最小生成树可以帮助识别点云中的关键连接点，从而分割点云。
  • **特征提取：**最小生成树可以帮助提取点云中的特征，例如边缘和角点。
  • **点云重建：**最小生成树可以帮助重建点云，例如从部分点云中重建完整点云。

原文地址: https://www.cveoy.top/t/topic/pKTc 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录