提取点云

/// <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);              // 设置滤波时创建的体素体积 单位：msor.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);              // 设置滤波时创建的体素体积 单位：msor.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;
}