从一个点云中提取一个子集

6.2.5 从一个点云中提取一个子集

代码：书中第六章例五
修改两个代码：

//sensor_msgs::PointCloud2::Ptr cloud_blob(new sensor_msgs::PointCloud2), cloud_filtered_blob(new sensor_msgs::PointCloud2);
//以上的是旧的方式，要改为
    pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);

    //pcl::fromROSMsg(*cloud_filtered_blob, *cloud_filtered);
    //改为：
    pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

修改后的完整代码：

#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>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
int
main(int argc, char** argv)
{
    //sensor_msgs::PointCloud2::Ptr cloud_blob(new sensor_msgs::PointCloud2), cloud_filtered_blob(new sensor_msgs::PointCloud2);

    pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
    // 填入点云数据
    pcl::PCDReader reader;
    reader.read("C:\\Users\\oh_clm\\Desktop\\点云库PCL从入门到精通\\第六章\\5 extract_indices\\source\\table_scene_lms400.pcd", *cloud_blob);
    std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
    // 创建滤波器对象:使用叶大小为1cm的下采样
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;//体素栅格下采样对象
    sor.setInputCloud(cloud_blob);//设置下采样原始点云数据
    sor.setLeafSize(0.01f, 0.01f, 0.01f);//设置采样的提速大小
    sor.filter(*cloud_filtered_blob);//执行采样保存数据
    // 转化为模板点云
    //pcl::fromROSMsg(*cloud_filtered_blob, *cloud_filtered);
    pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

    std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
    // 将下采样后的数据存入磁盘
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ>("C:\\Users\\oh_clm\\Desktop\\点云库PCL从入门到精通\\第六章\\5 extract_indices\\source\\table_scene_lms400_downsampled.pcd", *cloud_filtered, false);

    /*处理参数化分割*/
    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);//判断是否为模型内点的距离阈值

    // 创建滤波器对象
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    int i = 0, nr_points = (int)cloud_filtered->points.size();
    // 当还有30%原始点云数据时
    while (cloud_filtered->points.size() > 0.3 * nr_points)
    {
        // 从余下的点云中分割最大平面组成部分
        seg.setInputCloud(cloud_filtered);
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0)
        {
            std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }
        // 分离内层
        extract.setInputCloud(cloud_filtered);//设置输入点云
        extract.setIndices(inliers);//分割后的内点为需要提取的点集
        extract.setNegative(false);//提取内点 而非外点
        extract.filter(*cloud_p);//提取输出存储到cloud_p

        std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
        std::stringstream ss;
        ss << "table_scene_lms400_plane_" << i << ".pcd";
        writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);
        // 创建滤波器对象
        extract.setNegative(true);
        extract.filter(*cloud_f);
        cloud_filtered.swap(cloud_f);
        i++;
    }
    return (0);
}

运行结果：

原始点云：

下采样后的点云

分割后的点云