最小生成树（克鲁斯卡尔）

此代码是点云中最小生成树的实现，并且输入两个顶点，输出其在最小生成树中的路径。

#include <iostream>
#include <fstream>
#include <vector>
#include <queue>
#include <algorithm>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
using namespace std;

//计算两点之间的距离用作初始边的权重
float p2pdis(pcl::PointXYZ a,pcl::PointXYZ b) {
	return sqrt((a.x-b.x)*(a.x - b.x)+ (a.y - b.y)*(a.y - b.y)+ (a.z - b.z)*(a.z - b.z));
}
//定义边的结构
typedef struct edge {
	int initial;
	int end;
	float weight;
	edge(int a,int b, float c) {
		initial = a;
		end = b;
		weight = c;
	}
};
//根据权重的大小对边排序
bool cmp(edge a, edge b) {
	return  a.weight < b.weight;
}

int main()
{
	int ki , kj;//输入的需要搜索路径的两个顶点索引

	/*-------------------------------------------构建最小生成树------------------------------------------*/
	//读取点云文件
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("means.pcd", *c