6.2.5 Extract a subset from a point cloud

Code ： Example 5 in Chapter 6 of the book
Modify two codes ：

//sensor_msgs::PointCloud2::Ptr cloud_blob(new sensor_msgs::PointCloud2), cloud_filtered_blob(new sensor_msgs::PointCloud2);
// The above is the old way , To be changed to 
    pcl::PCLPointCloud2::Ptr cloud_blob(new pcl::PCLPointCloud2), cloud_filtered_blob(new pcl::PCLPointCloud2);

    //pcl::fromROSMsg(*cloud_filtered_blob, *cloud_filtered);
    // Change it to ：
    pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

The whole code after modification ：

#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>);
    //  Fill in point cloud data 
    pcl::PCDReader reader;
    reader.read("C:\\Users\\oh_clm\\Desktop\\ Point cloud Library PCL From entry to mastery \\ Chapter six \\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;
    //  Create filter object : The leaf size used is 1cm Under sampling 
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;// Sample objects under the voxel grid 
    sor.setInputCloud(cloud_blob);// Set the down sampling of the original point cloud data 
    sor.setLeafSize(0.01f, 0.01f, 0.01f);// Set the speed-up size of sampling 
    sor.filter(*cloud_filtered_blob);// Execute sampling to save data 
    //  Convert to template point cloud 
    //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;
    //  Save the down sampled data to disk 
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ>("C:\\Users\\oh_clm\\Desktop\\ Point cloud Library PCL From entry to mastery \\ Chapter six \\5 extract_indices\\source\\table_scene_lms400_downsampled.pcd", *cloud_filtered, false);

    /* Handle parametric segmentation */
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    //  Create split objects 
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    //  Optional 
    seg.setOptimizeCoefficients(true);// Set to optimize the estimated model parameters 
    //  Mandatory 
    seg.setModelType(pcl::SACMODEL_PLANE);// Set the split model category 
    seg.setMethodType(pcl::SAC_RANSAC);// Set which random parameter estimation method to use 
    seg.setMaxIterations(1000);// Set the maximum number of iterations 
    seg.setDistanceThreshold(0.01);// Determine whether it is the distance threshold of the inner point of the model 

    //  Create filter object 
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    int i = 0, nr_points = (int)cloud_filtered->points.size();
    //  When still 30% Original point cloud data 
    while (cloud_filtered->points.size() > 0.3 * nr_points)
    {
    
        //  Divide the largest plane component from the remaining point cloud 
        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;
        }
        //  Separate the inner layer 
        extract.setInputCloud(cloud_filtered);// Set the input point cloud 
        extract.setIndices(inliers);// The internal points after segmentation are the point sets to be extracted 
        extract.setNegative(false);// Extract interior points   Not the outer point 
        extract.filter(*cloud_p);// Extract the output and store it in 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);
        //  Create filter object 
        extract.setNegative(true);
        extract.filter(*cloud_f);
        cloud_filtered.swap(cloud_f);
        i++;
    }
    return (0);
}

Running results ：

The original point cloud ：

Point cloud after down sampling

Split point cloud