• 日期: 2027-09-15
  • 标签: 常规

#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> #include "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)
{
    // 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 (next_edge.weight < 0.043)
        {
            if (x != y)
            {
                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(0, 0, 0);

    // Add the original point cloud to the viewer
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 255, 255);
    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[14];
    pcl::PointXYZ point2 = cloud->points[34];
    pcl::PointXYZ point3 = cloud->points[68];
    pcl::PointXYZ point4 = cloud->points[62];
     viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(point1, point2, 0, 255, 0, "line1");
    viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(point3, point4, 0, 255, 0, "line2");

    // Count the number of points in the minimum spanning tree
    int numPoints = 0;
    for (const auto& edge : result)
    {
        const auto& src_point = cloud->points[edge.src];
        const auto& tgt_point = cloud->points[edge.tgt];
        numPoints += 2;
    }

    std::cout << "Number of points in the minimum spanning tree: " << numPoints << std::endl;

    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() { // 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);

// Create a new point cloud object to save the result of the minimum spanning tree
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());

// Add the vertices of the minimum spanning tree to the new point cloud object
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;
}

// Save the new point cloud as a PLY file
pcl::io::savePLYFile("D:\\DIANYUNWENJIANJIA\\newKRUSKAL_ply.ply", *new_cloud, true);

// 统计每个点的出现次数
std::unordered_map<pcl::PointXYZ, int> pointFrequency;
for (const auto& edge : result)
{
    pointFrequency[cloud->points[edge.src]]++;
    pointFrequency[cloud->points[edge.tgt]]++;
}

// 创建一个新的点云对象，用于存储高亮显示的点
pcl::PointCloud<pcl::PointXYZ>::Ptr highlightedCloud(new pcl::PointCloud<pcl::PointXYZ>);

// 找出频率大于等于 3 次的点，并将颜色设置为红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> redColor(highlightedCloud, 255, 0, 0);
for (const auto& pair : pointFrequency)
{
    if (pair.second >= 3)
    {
        highlightedCloud->push_back(pair.first);
    }
}

// 找出 Y 值最高的点，并将颜色设置为绿色
pcl::PointXYZ highestYPoint;
float highestY = -std::numeric_limits<float>::max();
for (const auto& pair : pointFrequency)
{
    if (pair.first.y > highestY)
    {
        highestY = pair.first.y;
        highestYPoint = pair.first;
    }
}

// 创建一个新的点云对象，用于存储 Y 值最高的点
pcl::PointCloud<pcl::PointXYZ>::Ptr highestYCloud(new pcl::PointCloud<pcl::PointXYZ>);
highestYCloud->push_back(highestYPoint);

// 创建一个新的视图器
pcl::visualization::PCLVisualizer viewer("Minimum Spanning Tree with Highlighted Points");
viewer.setBackgroundColor(0, 0, 0);

// 添加原始点云
viewer.addPointCloud<pcl::PointXYZ>(cloud, "original_cloud");

// 添加频率大于等于 3 次的点，并设置颜色为红色
viewer.addPointCloud<pcl::PointXYZ>(highlightedCloud, redColor, "highlighted_points");

// 添加 Y 值最高的点，并设置颜色为绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> greenColor(highestYCloud, 0, 255, 0);
viewer.addPointCloud<pcl::PointXYZ>(highestYCloud, greenColor, "highest_y_point");

// 添加最小生成树的边
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());
}

// 运行视图器
while (!viewer.wasStopped())
{
    viewer.spinOnce();
}

return 0;

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

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