00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00047 #include "pcl/ModelCoefficients.h"
00048
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051
00052 #include "pcl/sample_consensus/method_types.h"
00053 #include "pcl/sample_consensus/model_types.h"
00054 #include "pcl/segmentation/sac_segmentation.h"
00055 #include "pcl/filters/voxel_grid.h"
00056 #include "pcl/filters/extract_indices.h"
00057
00058
00059 int
00060 main (int argc, char** argv)
00061 {
00062 sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2 ()), cloud_filtered_blob (new sensor_msgs::PointCloud2 ());
00063 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ()), cloud_p (new pcl::PointCloud<pcl::PointXYZ> ());
00064
00065
00066 pcl::PCDReader reader;
00067 reader.read ("data/table_scene_lms400.pcd", *cloud_blob);
00068
00069 ROS_INFO ("PointCloud before filtering: %d data points.", cloud_blob->width * cloud_blob->height);
00070
00071
00072 pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
00073 sor.setInputCloud (cloud_blob);
00074 sor.setLeafSize (0.01, 0.01, 0.01);
00075 sor.filter (*cloud_filtered_blob);
00076
00077
00078 pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
00079
00080 ROS_INFO ("PointCloud after filtering: %d data points.", cloud_filtered->width * cloud_filtered->height);
00081
00082
00083 pcl::PCDWriter writer;
00084 writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
00085
00086 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
00087 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
00088
00089 pcl::SACSegmentation<pcl::PointXYZ> seg;
00090
00091 seg.setOptimizeCoefficients (true);
00092
00093 seg.setModelType (pcl::SACMODEL_PLANE);
00094 seg.setMethodType (pcl::SAC_RANSAC);
00095 seg.setMaxIterations (1000);
00096 seg.setDistanceThreshold (0.01);
00097
00098
00099 pcl::ExtractIndices<pcl::PointXYZ> extract;
00100
00101 int i = 0, nr_points = cloud_filtered->points.size ();
00102
00103 while (cloud_filtered->points.size () > 0.3 * nr_points)
00104 {
00105
00106 seg.setInputCloud (cloud_filtered);
00107 seg.segment (*inliers, *coefficients);
00108 if (inliers->indices.size () == 0)
00109 {
00110 ROS_ERROR ("Could not estimate a planar model for the given dataset.");
00111 break;
00112 }
00113
00114
00115 extract.setInputCloud (cloud_filtered);
00116 extract.setIndices (inliers);
00117 extract.setNegative (false);
00118 extract.filter (*cloud_p);
00119 ROS_INFO ("PointCloud representing the planar component: %d data points.", cloud_p->width * cloud_p->height);
00120
00121 std::stringstream ss;
00122 ss << "table_scene_lms400_plane_" << i << ".pcd";
00123 writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
00124
00125
00126 extract.setNegative (true);
00127 extract.filter (*cloud_filtered);
00128
00129 i++;
00130 }
00131
00132 return (0);
00133 }
00134