PCL点云Kruskal最小生成树算法实现
#include
VTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2
using namespace pcl;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud
// 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
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; // This will store the resultant MST
// Sort all the edges in non-decreasing order of their weight
std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)
{
return a.weight < b.weight;
});
// Allocate memory for creating V subsets
Subset* subsets = new Subset[V];
for (int v = 0; v < V; ++v)
{
subsets[v].parent = v;
subsets[v].rank = 0;
}
int i = 0; // Index used to pick the next smallest edge
int e = 0; // Index used to pick the next edge to include in MST
// Number of edges to be taken is equal to 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 including this edge doesn't cause a cycle, include it in result and increment the index of result for next edge
if (x != y && next_edge.weight < 0.045)
{
result.push_back(next_edge);
Union(subsets, x, y);
++e;
}
}
// Visualize the resulting minimum spanning tree
pcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree");
viewer.setBackgroundColor(1, 1, 1);
// Add the original point cloud to the viewer
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 0, 0);
viewer.addPointCloud<pcl::PointXYZ>(cloud, single_color, "original_cloud");
// Add the edges of the MST to the viewer
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());
}
// Find the node with the maximum value in the y direction and mark it as green
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();
}
// Save the result of minimum spanning tree to PLY file
PointCloudT::Ptr mst_cloud(new PointCloudT);
for (const auto& edge : result)
{
const auto& src_point = cloud->points[edge.src];
const auto& tgt_point = cloud->points[edge.tgt];
mst_cloud->push_back(src_point);
mst_cloud->push_back(tgt_point);
}
pcl::io::savePLYFileBinary("D:\\DIANYUNWENJIANJIA\\newKRUSKAL_ply.ply", *mst_cloud);
}
};
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\OUSHIJULEI_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(10);
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
graph.KruskalMST(cloud);
return 0;
原文地址: https://www.cveoy.top/t/topic/pKRP 著作权归作者所有。请勿转载和采集!