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 pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
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
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
00024 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
00025 sor.setInputCloud (cloud_blob);
00026 sor.setLeafSize (0.01f, 0.01f, 0.01f);
00027 sor.filter (*cloud_filtered_blob);
00028
00029
00030 pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
00031
00032 std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
00033
00034
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
00041 pcl::SACSegmentation<pcl::PointXYZ> seg;
00042
00043 seg.setOptimizeCoefficients (true);
00044
00045 seg.setModelType (pcl::SACMODEL_PLANE);
00046 seg.setMethodType (pcl::SAC_RANSAC);
00047 seg.setMaxIterations (1000);
00048 seg.setDistanceThreshold (0.01);
00049
00050
00051 pcl::ExtractIndices<pcl::PointXYZ> extract;
00052
00053 int i = 0, nr_points = (int) cloud_filtered->points.size ();
00054
00055 while (cloud_filtered->points.size () > 0.3 * nr_points)
00056 {
00057
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
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
00078 extract.setNegative (true);
00079 extract.filter (*cloud_f);
00080 cloud_filtered.swap (cloud_f);
00081 i++;
00082 }
00083
00084 return (0);
00085 }