• 日期: 2027-01-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> 'vtkAutoInit.h' #include <pcl/visualization/point_cloud_color_handlers.h> VTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL2

using namespace pcl; typedef pcl::PointXYZ PointT; typedef pcl::PointCloud PointCloudT;

// 表示一条边的结构体 struct Edge { int src, tgt; float weight; };

// 表示并查集的子集的结构体 struct Subset { int parent, rank; };

// 表示一个连通的、无向的、带权重的图的类 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(Subset subsets[], int i)
{
    if (subsets[i].parent != i)
        subsets[i].parent = find(subsets, subsets[i].parent);
    return subsets[i].parent;
}

// 合并两个集合
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算法找到最小生成树
void KruskalMST(PointCloudT::Ptr cloud, std::vector<Edge>& result, float threshold)
{
    // 将边按照权重从小到大排序
    std::sort(edges.begin(), edges.end(), [](const Edge& a, const Edge& b)
    {
        return a.weight < b.weight;
    });

    // 为V个元素创建并查集的子集
    Subset* subsets = new Subset[V];
    for (int v = 0; v < V; ++v)
    {
        subsets[v].parent = v;
        subsets[v].rank = 0;
    }

    int i = 0;  // 用于选择下一条边的索引
    int e = 0;  // 用于选择下一条边加入最小生成树的索引

    // 需要选择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 (next_edge.weight >= threshold && x != y)
        {
            result.push_back(next_edge);
            Union(subsets, x, y);
            ++e;
        }
    }

    // 可视化最小生成树的结果
    pcl::visualization::PCLVisualizer viewer('Minimum Spanning Tree');
    viewer.setBackgroundColor(0, 0, 0);

    // 将原始点云添加到可视化窗口中
    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());
    }

    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() { // 从PLY文件加载输入点云 pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ); pcl::io::loadPLYFilepcl::PointXYZ('D:\DIANYUNWENJIANJIA\newOUSHIJULEI_ply.ply', *cloud);

// 计算点云的质心
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);

// 计算点云的法线
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);

// 创建一个有V个顶点和E个边的图
int V = cloud->size();
int E = V * (V - 1) / 2;
Graph graph(V, E);

// 基于点之间的欧式距离计算边的权重
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);
    }
}
// 设置修剪的阈值
float threshold = 0.00080;
// 执行Kruskal算法找到最小生成树
std::vector<Edge> result;
graph.KruskalMST(cloud, result, threshold);
// 创建一个新的点云对象保存修剪后的最小生成树结果
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());

// 找到在最小生成树中出现三次的节点
std::unordered_map<int, int> node_count;
for (const auto& edge : result)
{
    node_count[edge.src]++;
    node_count[edge.tgt]++;
}

std::vector<int> threejiedian;
for (const auto& pair : node_count)
{
    if (pair.second == 3)
    {
        threejiedian.push_back(pair.first);
    }
}
// 如果与threejiedian相连的边的权重小于0.0008，则从最小生成树中移除该边
std::vector<Edge> pruned_result;
for (const auto& edge : result)
{
    if (std::find(threejiedian.begin(), threejiedian.end(), edge.src) != threejiedian.end() ||
        std::find(threejiedian.begin(), threejiedian.end(), edge.tgt) != threejiedian.end())
    {
        if (edge.weight <= 0.008)
        {
            pruned_result.push_back(edge);
        }
    }
    else
    {
        pruned_result.push_back(edge);
    }
}
// 将修剪后的最小生成树的顶点添加到新的点云对象中
for (const auto& edge : pruned_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;
}
// 将与threejiedian相连的边的顶点设置为绿色
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
colored_cloud->points.resize(cloud->points.size());
for (const auto& edge : result)
{
    if (std::find(threejiedian.begin(), threejiedian.end(), edge.src) != threejiedian.end() ||
        std::find(threejiedian.begin(), threejiedian.end(), edge.tgt) != threejiedian.end())
    {
        colored_cloud->points[edge.src].r = 0;
        colored_cloud->points[edge.src].g = 255;
        colored_cloud->points[edge.src].b = 0;
        colored_cloud->points[edge.tgt].r = 0;
        colored_cloud->points[edge.tgt].g = 255;
        colored_cloud->points[edge.tgt].b = 0;
    }
}

// 将y值最大的threejiedian节点用绿色小球显示
int max_y_index = -1;
float max_y = std::numeric_limits<float>::min();
for (const auto& index : threejiedian)
{
    const auto& point = cloud->points[index];
    if (point.y > max_y)
    {
        max_y = point.y;
        max_y_index = index;
    }
}
colored_cloud->points[max_y_index].r = 0;
colored_cloud->points[max_y_index].g = 255;
colored_cloud->points[max_y_index].b = 0;

// 将最小生成树中出现一次的节点设置为红色
std::unordered_map<int, int> node_count;
for (const auto& edge : result)
{
    node_count[edge.src]++;
    node_count[edge.tgt]++;
}
for (const auto& pair : node_count)
{
    if (pair.second == 1)
    {
        colored_cloud->points[pair.first].r = 255;
        colored_cloud->points[pair.first].g = 0;
        colored_cloud->points[pair.first].b = 0;
    }
}
// 创建一个新的可视化窗口
pcl::visualization::PCLVisualizer viewer2('Pruned Minimum Spanning Tree');
viewer2.setBackgroundColor(0, 0, 0);

// 将修剪后的最小生成树添加到可视化窗口中
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud);
viewer2.addPointCloud<pcl::PointXYZRGB>(colored_cloud, rgb, 'pruned_cloud');
viewer2.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, 'pruned_cloud');

// 将修剪后的最小生成树的边添加到可视化窗口中
for (const auto& edge : pruned_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;
    viewer2.addLine<pcl::PointXYZ>(src_point, tgt_point, ss.str());
}
while (!viewer2.wasStopped())
{
    viewer2.spinOnce();
}
return 0;

} '}

代码修改说明:

  1. 添加新的可视化窗口: 在代码中添加了 pcl::visualization::PCLVisualizer viewer2('Pruned Minimum Spanning Tree') 来创建一个新的可视化窗口。2. 将修剪后的点云添加到新的可视化窗口: 使用 viewer2.addPointCloud<pcl::PointXYZRGB>(colored_cloud, rgb, 'pruned_cloud'); 将修剪后的点云添加到新的可视化窗口。3. 设置点云大小: 使用 viewer2.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, 'pruned_cloud'); 将点云的大小设置为 3。4. 将修剪后的最小生成树的边添加到新的可视化窗口: 使用 viewer2.addLine<pcl::PointXYZ>(src_point, tgt_point, ss.str()); 将修剪后的最小生成树的边添加到新的可视化窗口。5. 循环显示新的可视化窗口: 使用 while (!viewer2.wasStopped()) { viewer2.spinOnce(); } 循环显示新的可视化窗口。

可视化结果:

新的可视化窗口将显示修剪后的最小生成树，其中：

  • 与 threejiedian 相连的边的顶点将以绿色显示。- y 值最大的 threejiedian 节点将以绿色小球显示。- 最小生成树中出现一次的节点将以红色显示。

替换内容:

为了实现上述可视化功能，需要在代码中添加如下内容:cpp // 创建一个新的可视化窗口 pcl::visualization::PCLVisualizer viewer2('Pruned Minimum Spanning Tree'); viewer2.setBackgroundColor(0, 0, 0);

// 将修剪后的最小生成树添加到可视化窗口中    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(colored_cloud);    viewer2.addPointCloud<pcl::PointXYZRGB>(colored_cloud, rgb, 'pruned_cloud');    viewer2.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, 'pruned_cloud');

// 将修剪后的最小生成树的边添加到可视化窗口中    for (const auto& edge : pruned_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;        viewer2.addLine<pcl::PointXYZ>(src_point, tgt_point, ss.str());    }    while (!viewer2.wasStopped())    {        viewer2.spinOnce();
基于Kruskal算法的点云最小生成树修剪与可视化

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

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