PCL—点云数据分割

一.算法原理

欧式聚类是一种基于欧氏距离度量的聚类算法，过程如下：

1.首先选取种子点，利用kd-tree对种子点进行半径r邻域搜索，若邻域内存在点，则与种子点归为同一聚类簇Q；
2.在聚类簇Q中选取新的种子点，继续执行步骤1），若Q中点数不再增加，则Q聚类结束；
3.设置聚类点数阈值区间[Num_min, Num_max]，若聚类簇Q中点数在阈值区间内，则保存聚类结果；
4.在剩余点云中选取新的种子点，继续执行以上步骤，直到遍历完成点云中所有点。

流程图如下：

二.代码实现

#include <iostream>
#include <boost/thread.hpp>
#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>
#include <pcl/visualization/pcl_visualizer.h>

int color_bar[][3] =
{
    
    {
     255,0,0},
    {
     0,255,0 },
    {
     0,0,255 },
    {
     0,255,255 },
    {
     255,255,0 },
    {
     100,100,100 },
    {
     255,0,255 }
};
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("D:\\.....\\03.pcd", *cloud);
    std::cout << "PointCloud before filtering has: " << cloud->points.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);//设置体素栅格叶大小，向量参数leaf_ size 是体素栅格叶大小参数，每个元素分别表示体素在XYZ方向上的尺寸
    vg.filter(*cloud_filtered);
    // std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*
    std::cout << "PointCloud after filtering has: " << cloud_filtered->points.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->points.size();
      while (cloud_filtered->points.size() > 0.9 * 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);

          // Write the planar inliers to disk
          extract.filter(*cloud_plane);
          std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

          // 移去平面局内点，提取剩余点云
          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.018); //设置近邻搜索的搜索半径为2cm
    ec.setMinClusterSize(6000);    //设置一个聚类需要的最少点数目为100
    ec.setMaxClusterSize(15000);  //设置一个聚类需要的最大点数目为25000
    ec.setSearchMethod(tree);     //设置点云的搜索机制
    ec.setInputCloud(cloud_filtered); //设置原始点云 
    ec.extract(cluster_indices);  //从点云中提取聚类

    // 可视化部分
    pcl::visualization::PCLVisualizer viewer("segmention");
    // 我们将要使用的颜色
    float bckgr_gray_level = 0.0;  // 黑色
    float txt_gray_lvl = 1.0 - bckgr_gray_level;
    int num = cluster_indices.size();

    //迭代访问点云索引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>);
        //创建新的点云数据集cloud_cluster,将所有当前聚类写入到点云数据集中
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
             cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //* 
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        //输出每一簇的点云数量
        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
        std::stringstream ss;
        ss << "cloud_cluster_" << j << ".pcd";
        writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
        
        
        
        //赋予显示点云的颜色
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h(cloud,
            color_bar[j][0],
            color_bar[j][1],
            color_bar[j][2]);
        viewer.addPointCloud(cloud_cluster, cloud_in_color_h, std::to_string(j));

        pcl::io::savePCDFile("D:\\.....\\033.pcd", *cloud_cluster);

        j++;
    }
    //等待直到可视化窗口关闭。
    while (!viewer.wasStopped())
    {
    
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    return (0);
}

三.结果展示

本实验用于将一个小车点云数据中的轮胎分离出来
原始点云

结果点云