提取点云

/// <summary>
/// VoxelGrid滤波下采样
/// </summary>
/// <param name="cloud">需要滤波的点云</param>
/// <param name="lx">三维体素栅格的x</param>
/// <param name="ly">三维体素栅格的y</param>
/// <param name="lz">三维体素栅格的z</param>
/// <returns></returns>
pcl::PCLPointCloud2::Ptr PclTool::voxelGridFilter(pcl::PCLPointCloud2::Ptr cloud, float lx, float ly, float lz)
{
    pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());

    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  // 创建滤波对象
    sor.setInputCloud(cloud);                 // 设置需要过滤的点云给滤波对象
    sor.setLeafSize(lx, ly, lz);              // 设置滤波时创建的体素体积 单位：m
    sor.filter(*cloud_filtered);              // 执行滤波处理，存储输出

    return cloud_filtered;

}

pcl::PCLPointCloud2::Ptr PclTool::voxelGridFilter(pcl::PCLPointCloud2::Ptr cloud, float lx, float ly, float lz)
{
    pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());

    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  // 创建滤波对象
    sor.setInputCloud(cloud);                 // 设置需要过滤的点云给滤波对象
    sor.setLeafSize(lx, ly, lz);              // 设置滤波时创建的体素体积 单位：m
    sor.filter(*cloud_filtered);              // 执行滤波处理，存储输出

    return cloud_filtered;

}
// 点云提取
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> PclTool::cloudExtraction(pcl::PCLPointCloud2::Ptr cloud)
{
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vecCloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

      std::cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points." << std::endl;

    // 先对点云做VoxelGrid滤波器对数据进行下采样，在这里进行下才样是为了加速处理过程
    pcl::PCLPointCloud2::Ptr cloud_filtered_blob = voxelGridFilter(cloud, 0.1, 0.1, 0.1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    // 转换为模板点云
    pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

    std::cout << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

    pcl::SACSegmentation<pcl::PointXYZ> seg;  // 创建分割对象
    seg.setOptimizeCoefficients(true);        // 设置对估计模型参数进行优化处理
    seg.setModelType(pcl::SACMODEL_PLANE);    // 设置分割模型类别
    seg.setMethodType(pcl::SAC_RANSAC);       // 设置用哪个随机参数估计方法
    seg.setMaxIterations(1000);               // 设置最大迭代次数
    seg.setDistanceThreshold(0.01);           // 判断是否为模型内点的距离阀值

      // 设置ExtractIndices的实际参数
    pcl::ExtractIndices<pcl::PointXYZ> extract;  // 创建点云提取对象
    int i = 0;
    int nr_points = (int)cloud_filtered->points.size();  // 点云总数
    for (int i = 0; cloud_filtered->points.size() > 0.3 * nr_points; i++)
    {
        // 为了处理点云包含的多个模型，在一个循环中执行该过程并在每次模型被提取后，保存剩余的点进行迭代
        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;
        }
        //提取入口
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*temp_cloud);
        vecCloud.push_back(temp_cloud);
        std::cout << "Extract the " << i << "point cloud : " << temp_cloud->width * temp_cloud->height << " data points." << std::endl;

        //创建筛选对象
        extract.setNegative(true);
        extract.filter(*cloud_f);
        cloud_filtered.swap(cloud_f);
    }

    return vecCloud;
}