【点云处理技术之PCL】点云分割算法1——平面模型分割、圆柱模型分割和欧式聚类提取(含欧式聚类原理)

最新推荐文章于 2024-07-24 22:51:50 发布

非晚非晚

最新推荐文章于 2024-07-24 22:51:50 发布

阅读量4.1k

点赞数 3

分类专栏： PCL 文章标签：算法聚类平面分割圆柱模型分割欧式聚类

本文链接：https://blog.csdn.net/QLeelq/article/details/122311773

版权

PCL 专栏收录该内容

18 篇文章 57 订阅

订阅专栏

文章目录

1. 平面分割
2. 圆柱分割
3. 欧式聚类分割

1. 平面分割

下列中，先随机创建了z=1.0的随机点，然后改变其中3个点的z值。最后，使用SACMODEL_PLANE平面模型对它进行拟合。

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // Fill in the cloud data，15个点
    cloud->width = 15;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);

    // Generate the data
    for (auto &point : *cloud)//同一个z
    {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1.0;
    }

    // Set a few outliers，设置离群点，设置z就可以
    (*cloud)[0].z = 2.0;
    (*cloud)[3].z = -2.0;
    (*cloud)[6].z = 4.0;

    std::cerr << "Point cloud data: " << cloud->size() << " points" << std::endl;
    for (const auto &point : *cloud)
        std::cerr << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients(true);
    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);//平面模型
    seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型，平面类型
    seg.setDistanceThreshold(0.01);表示点到估计模型的距离最大值

    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.size() == 0)
    {
        PCL_ERROR("Could not estimate a planar model for the given dataset.");
        return (-1);
    }

    //平面模型类型，系数
    std::cerr << "Model coefficients: " << coefficients->values[0] << " "
              << coefficients->values[1] << " "
              << coefficients->values[2] << " "
              << coefficients->values[3] << std::endl;

    std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
  for (size_t i = 0; i < inliers->indices.size (); ++i)
    std::cerr << inliers->indices[i] << "    " << cloud->points[inliers->indices[i]].x << " "
                                               << cloud->points[inliers->indices[i]].y << " "
                                               << cloud->points[inliers->indices[i]].z << std::endl;

    return (0);
}

输出：

Point cloud data: 15 points
    0.352222 -0.151883 2
    -0.106395 -0.397406 1
    -0.473106 0.292602 1
    -0.731898 0.667105 -2
    0.441304 -0.734766 1
    0.854581 -0.0361733 1
    -0.4607 -0.277468 4
    -0.916762 0.183749 1
    0.968809 0.512055 1
    -0.998983 -0.463871 1
    0.691785 0.716053 1
    0.525135 -0.523004 1
    0.439387 0.56706 1
    0.905417 -0.579787 1
    0.898706 -0.504929 1
Model coefficients: 0 0 1 -1
Model inliers: 12
1    -0.106395 -0.397406 1
2    -0.473106 0.292602 1
4    0.441304 -0.734766 1
5    0.854581 -0.0361733 1
7    -0.916762 0.183749 1
8    0.968809 0.512055 1
9    -0.998983 -0.463871 1
10    0.691785 0.716053 1
11    0.525135 -0.523004 1
12    0.439387 0.56706 1
13    0.905417 -0.579787 1
14    0.898706 -0.504929 1

2. 圆柱分割

下例先使用平面分割出平面，使用的是SACMODEL_NORMAL_PLANE，模型约束平面的法向方向，针对复杂的平面，可以更准确分割出平面点云，与SACMODEL_PLANE不同。最后针对剩余的点云进行圆柱分割。

下列中保存了两份分割点云：平面点云和圆柱点云。

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

typedef pcl::PointXYZ PointT;

int main(int argc, char **argv)
{
    // All the objects needed
    pcl::PCDReader reader;//pcd读取
    pcl::PassThrough<PointT> pass;//直通滤波器
    pcl::NormalEstimation<PointT, pcl::Normal> ne;//法线估计对象
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;//分割对象
    pcl::PCDWriter writer;//pcd写
    pcl::ExtractIndices<PointT> extract;//点提取
    pcl::ExtractIndices<pcl::Normal> extract_normals;//点提取
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());

    // Datasets
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices);

    // Read in the cloud data
    reader.read("../../pcd/table_scene_mug_stereo_textured.pcd", *cloud);
    std::cerr << "PointCloud has: " << cloud->size() << " data points." << std::endl;

    // Build a passthrough filter to remove spurious NaNs
    //过滤掉z（0，1.5）的点
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0, 1.5);
    pass.filter(*cloud_filtered);
    std::cerr << "PointCloud after filtering has: " << cloud_filtered->size() << " data points." << std::endl;

    // Estimate point normals，法线估计，要基于法线分割
    ne.setSearchMethod(tree);
    ne.setInputCloud(cloud_filtered);
    ne.setKSearch(50);
    ne.compute(*cloud_normals);

    // Create the segmentation object for the planar model and set all the parameters
    seg.setOptimizeCoefficients(true);
    //模型约束平面的法向方向，针对复杂的平面，可以更准确分割出平面点云，与SACMODEL_PLANE不同
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight(0.1);
    seg.setMethodType(pcl::SAC_RANSAC);//RANSAC方法
    seg.setMaxIterations(100);//最大迭代次数
    seg.setDistanceThreshold(0.03);//与模型的最大距离
    seg.setInputCloud(cloud_filtered);
    seg.setInputNormals(cloud_normals);
    // Obtain the plane inliers and coefficients
    seg.segment(*inliers_plane, *coefficients_plane);
    std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

    // Extract the planar inliers from the input cloud
    //提取平面点云
    extract.setInputCloud(cloud_filtered);
    extract.setIndices(inliers_plane);
    extract.setNegative(false);

    // Write the planar inliers to disk
    //将平面点云写入pcd文件
    pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
    extract.filter(*cloud_plane);
    std::cerr << "PointCloud representing the planar component: " << cloud_plane->size() << " data points." << std::endl;
    writer.write("../../pcd/table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);

    // Remove the planar inliers, extract the rest，提取剩余部分
    extract.setNegative(true);
    extract.filter(*cloud_filtered2);
    extract_normals.setNegative(true);
    extract_normals.setInputCloud(cloud_normals);
    extract_normals.setIndices(inliers_plane);
    extract_normals.filter(*cloud_normals2);

    // Create the segmentation object for cylinder segmentation and set all the parameters
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);//设置圆柱模型
    seg.setMethodType(pcl::SAC_RANSAC);//RANSAC方法
    seg.setNormalDistanceWeight(0.1);//设置表面法线权重系数
    seg.setMaxIterations(10000);//最大迭代次数
    seg.setDistanceThreshold(0.05);//最大阈值
    seg.setRadiusLimits(0, 0.1);//设置估计出的圆柱模型的半径的范围
    seg.setInputCloud(cloud_filtered2);
    seg.setInputNormals(cloud_normals2);

    // Obtain the cylinder inliers and coefficients
    seg.segment(*inliers_cylinder, *coefficients_cylinder);
    std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

    // Write the cylinder inliers to disk
    extract.setInputCloud(cloud_filtered2);
    extract.setIndices(inliers_cylinder);
    extract.setNegative(false);
    pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
    extract.filter(*cloud_cylinder);
    if (cloud_cylinder->points.empty())
        std::cerr << "Can't find the cylindrical component." << std::endl;
    else
    {
        std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->size() << " data points." << std::endl;
        writer.write("../../pcd/table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
    }
    return (0);
}

原始点云为：
在这里插入图片描述

分割出来的圆柱为：
在这里插入图片描述
分割出来的平面为：

3. 欧式聚类分割

欧式聚类原理：

找到空间中某点 $p_0$ ，有kdTree找到离它最近的n个点，判断这n个点到 $p_0$ 的距离。将距离小于阈值r的点 $p_1,p_2,p_3,p_4....$ 放在类Q里。
对Q中的剩余点重复以上步骤，并将满足条件的点放入Q。
当Q中再也不能有新的点加入时，则完成搜索。

下面的欧式聚类分割代码逻辑为：

首先对原始点云做下采样，体素大小为0.01.
然后采样点云做平面滤波，使用的是SAC_RANSAC平面滤波，直到总量小于原始的0.3倍之后才不做平面滤波
对平面滤波剩余的点云进行欧式距离分割。

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

int main(int argc, char **argv)
{
    // Read in the cloud data
    pcl::PCDReader reader;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
    reader.read("../../pcd/table_scene_lms400.pcd", *cloud);
    std::cout << "PointCloud before filtering has: " << cloud->size() << " data points." << std::endl; //*

    // Create the filtering object: downsample the dataset using a leaf size of 1cm
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.01f, 0.01f, 0.01f);
    vg.filter(*cloud_filtered);
    std::cout << "PointCloud after filtering has: " << cloud_filtered->size() << " data points." << std::endl; //*

    // Create the segmentation object for the planar model and set all the parameters
    //平面分割
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PCDWriter writer;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE); //分割模型
    seg.setMethodType(pcl::SAC_RANSAC);    //参数估计方法
    seg.setMaxIterations(100);             //最大迭代次数
    seg.setDistanceThreshold(0.02);        //设置阈值

    int i = 0, nr_points = (int)cloud_filtered->size();
    //先分割平面，循环分割
    //删除平面之后，剩余点云不能大于总数的0.3倍，否则继续分割
    while (cloud_filtered->size() > 0.3 * nr_points)
    {
        // Segment the largest planar component from the remaining cloud
        seg.setInputCloud(cloud_filtered);
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0)
        {
            std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        // Extract the planar inliers from the input cloud
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(inliers);
        extract.setNegative(false);

        // Get the points associated with the planar surface
        extract.filter(*cloud_plane);
        std::cout << "PointCloud representing the planar component: " << cloud_plane->size() << " data points." << std::endl;

        // Remove the planar inliers, extract the rest
        //要分割好几个平面，移除平面点
        extract.setNegative(true);
        extract.filter(*cloud_f);
        *cloud_filtered = *cloud_f;
    }

    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_filtered);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; //欧式距离对象
    ec.setClusterTolerance(0.02);                      // 2cm，设置近邻搜索的搜索半径
    ec.setMinClusterSize(100);                         //设置一个聚类需要的最少的点数目为100
    ec.setMaxClusterSize(25000);                       //设置一个聚类需要的最大点数目为25000
    ec.setSearchMethod(tree);                          //设置点云的搜索机制
    ec.setInputCloud(cloud_filtered);
    //cluster_indices为欧式分割后的聚类索引
    ec.extract(cluster_indices);//从点云中提取聚类，并将点云索引保存在cluster_indices中

    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {
        //提取每个聚类的点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
            cloud_cluster->push_back((*cloud_filtered)[*pit]); //*

        cloud_cluster->width = cloud_cluster->size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size() << " data points." << std::endl;
        std::stringstream ss;
        ss << "cloud_cluster_" << j << ".pcd";
        writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
        j++;
    }

    return (0);
}

输出如下，下面对数据进行简单的分析：

原始点为：460400个，下采样之后为：41049
对下采样数据进行了两次平面分割，分割输出的点云分别为：20536和20536
欧式聚类一共分割出5个目标，点云数分别为：4857、1386、321、291和123

PointCloud before filtering has: 460400 data points.
PointCloud after filtering has: 41049 data points.
PointCloud representing the planar component: 20536 data points.
PointCloud representing the planar component: 20536 data points.
PointCloud representing the Cluster: 4857 data points.
PointCloud representing the Cluster: 1386 data points.
PointCloud representing the Cluster: 321 data points.
PointCloud representing the Cluster: 291 data points.
PointCloud representing the Cluster: 123 data points.

原始pcd点云如下：
在这里插入图片描述
欧式聚类分割之后的5个目标点云如下：

第一个聚类目标点云
第二个聚类目标点云
第三个聚类目标点云
第四个聚类目标点云
第五个聚类目标点云

非晚非晚

关注

3
点赞
踩
36

收藏

觉得还不错? 一键收藏
打赏
6
评论
【点云处理技术之PCL】点云分割算法1——平面模型分割、圆柱模型分割和欧式聚类提取(含欧式聚类原理)

1. 平面分割2. 圆柱分割3. 欧式聚类分割下面的欧式聚类分割代码逻辑为：首先对原始点云做下采样，体素大小为0.01.然后采样点云做平面滤波，使用的是SAC_RANSAC平面滤波，直到总量小于原始的0.3倍之后才不做平面滤波，对平面滤波剩余的点云进行欧式距离分割。
复制链接

扫一扫