extract_indices.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <pcl/ModelCoefficients.h>
00003 #include <pcl/io/pcd_io.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl/sample_consensus/method_types.h>
00006 #include <pcl/sample_consensus/model_types.h>
00007 #include <pcl/segmentation/sac_segmentation.h>
00008 #include <pcl/filters/voxel_grid.h>
00009 #include <pcl/filters/extract_indices.h>
00010 
00011 int
00012 main (int argc, char** argv)
00013 {
00014   sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2);
00015   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>);
00016 
00017   // Fill in the cloud data
00018   pcl::PCDReader reader;
00019   reader.read ("table_scene_lms400.pcd", *cloud_blob);
00020 
00021   std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
00022 
00023   // Create the filtering object: downsample the dataset using a leaf size of 1cm
00024   pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
00025   sor.setInputCloud (cloud_blob);
00026   sor.setLeafSize (0.01f, 0.01f, 0.01f);
00027   sor.filter (*cloud_filtered_blob);
00028 
00029   // Convert to the templated PointCloud
00030   pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
00031 
00032   std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
00033 
00034   // Write the downsampled version to disk
00035   pcl::PCDWriter writer;
00036   writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
00037 
00038   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
00039   pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
00040   // Create the segmentation object
00041   pcl::SACSegmentation<pcl::PointXYZ> seg;
00042   // Optional
00043   seg.setOptimizeCoefficients (true);
00044   // Mandatory
00045   seg.setModelType (pcl::SACMODEL_PLANE);
00046   seg.setMethodType (pcl::SAC_RANSAC);
00047   seg.setMaxIterations (1000);
00048   seg.setDistanceThreshold (0.01);
00049 
00050   // Create the filtering object
00051   pcl::ExtractIndices<pcl::PointXYZ> extract;
00052 
00053   int i = 0, nr_points = (int) cloud_filtered->points.size ();
00054   // While 30% of the original cloud is still there
00055   while (cloud_filtered->points.size () > 0.3 * nr_points)
00056   {
00057     // Segment the largest planar component from the remaining cloud
00058     seg.setInputCloud (cloud_filtered);
00059     seg.segment (*inliers, *coefficients);
00060     if (inliers->indices.size () == 0)
00061     {
00062       std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
00063       break;
00064     }
00065 
00066     // Extract the inliers
00067     extract.setInputCloud (cloud_filtered);
00068     extract.setIndices (inliers);
00069     extract.setNegative (false);
00070     extract.filter (*cloud_p);
00071     std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
00072 
00073     std::stringstream ss;
00074     ss << "table_scene_lms400_plane_" << i << ".pcd";
00075     writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
00076 
00077     // Create the filtering object
00078     extract.setNegative (true);
00079     extract.filter (*cloud_f);
00080     cloud_filtered.swap (cloud_f);
00081     i++;
00082   }
00083 
00084   return (0);
00085 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:57