#include iostream#include pclpoint_typesh#include pclioply_ioh#include pclvisualizationpcl_visualizerh#include pclcommoncentroidh#include pclfeaturesnormal_3dh#include pclvisualizationcloud_viewerh#in
The code you provided is missing some necessary header files and definitions, so it cannot be compiled and run directly. However, I can modify the code to remove the comments and output the desired result. Here is the modified code:
#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>
#include "vtkAutoInit.h"
VTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2
using namespace pcl;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> 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<Edge> 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; // 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());
}
// 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");
// 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();
}
}
}
原文地址: http://www.cveoy.top/t/topic/h1GJ 著作权归作者所有。请勿转载和采集!