PCL Point Cloud Minimum Spanning Tree (MST) with Curvature Filtering

#include #include #include #include <pcl/point_cloud.h> #include <pcl/io/ply_io.h> #include <pcl/features/normal_3d.h> #include <pcl/common/centroid.h>

struct Edge { int src, tgt; float weight; };

class Graph { public: int V, E; std::vector 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);
}

int find(std::vector<int>& parent, int i) {
    if (parent[i] == -1)
        return i;
    return find(parent, parent[i]);
}

void Union(std::vector<int>& parent, int x, int y) {
    int xset = find(parent, x);
    int yset = find(parent, y);
    parent[xset] = yset;
}

void KruskalMST(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<Edge>& result) {
    std::vector<Edge> sorted_edges = edges;
    std::sort(sorted_edges.begin(), sorted_edges.end(), [](const Edge& a, const Edge& b) {
        return a.weight < b.weight;
    });

    std::vector<int> parent(V, -1);

    int count = 0;
    for (int i = 0; i < E; ++i) {
        int src_parent = find(parent, sorted_edges[i].src);
        int tgt_parent = find(parent, sorted_edges[i].tgt);

        if (src_parent != tgt_parent) {
            result.push_back(sorted_edges[i]);
            Union(parent, src_parent, tgt_parent);
            count++;
        }
        if (count == V - 1)
            break;
    }
}

};

float euclideanDistance(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2) { float dx = p1.x - p2.x; float dy = p1.y - p2.y; float dz = p1.z - p2.z; return std::sqrt(dxdx + dydy + dz*dz); }

int main() { std::vector edges; pcl::PointCloudpcl::PointXYZ::Ptr singlejiedian(new pcl::PointCloudpcl::PointXYZ); // 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);

// Calculate the edge weights based on Euclidean distance between points
for (int i = 0; i < cloud->size() - 1; ++i) {
    const auto& src_point = cloud->points[i];
    for (int j = i + 1; j < cloud->size(); ++j) {
        const auto& tgt_point = cloud->points[j];
        float distance = euclideanDistance(src_point, tgt_point);
        edges.emplace_back(Edge{i, j, distance});
    }
}

// Create a graph with V vertices and E edges
int V = cloud->size();
int E = V * (V - 1) / 2;
Graph graph(V, E);
for (const auto& edge : edges) {
    graph.addEdge(edge.src, edge.tgt, edge.weight);
}

// Perform minimum spanning tree (MST) algorithm
std::vector<Edge> result;
graph.KruskalMST(cloud, result);

// Perform curvature filtering to select leaf and stem points
// ...

// Use the selected points to create a new point cloud and perform MST
// ...

return 0;

}