예제 #1
0
  bool EuclideanClustering::serviceCallback(
    jsk_recognition_msgs::EuclideanSegment::Request &req,
    jsk_recognition_msgs::EuclideanSegment::Response &res)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(req.input, *cloud);

#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
    pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
    tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif

    vector<pcl::PointIndices> cluster_indices;
    EuclideanClusterExtraction<pcl::PointXYZ> impl;
    double tor;
    if ( req.tolerance < 0) {
      tor = tolerance;
    } else {
      tor = req.tolerance;
    }
    impl.setClusterTolerance (tor);
    impl.setMinClusterSize (minsize_);
    impl.setMaxClusterSize (maxsize_);
    impl.setSearchMethod (tree);
    impl.setInputCloud (cloud);
    impl.extract (cluster_indices);

    res.output.resize( cluster_indices.size() );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
    pcl::PCLPointCloud2::Ptr pcl_cloud(new pcl::PCLPointCloud2);
    pcl_conversions::toPCL(req.input, *pcl_cloud);
    pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
    ex.setInputCloud(pcl_cloud);
#else
    pcl::ExtractIndices<sensor_msgs::PointCloud2> ex;
    ex.setInputCloud ( boost::make_shared< sensor_msgs::PointCloud2 > (req.input) );
#endif
    for ( size_t i = 0; i < cluster_indices.size(); i++ ) {
      ex.setIndices ( boost::make_shared< pcl::PointIndices > (cluster_indices[i]) );
      ex.setNegative ( false );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
      pcl::PCLPointCloud2 output_cloud;
      ex.filter ( output_cloud );
      pcl_conversions::fromPCL(output_cloud, res.output[i]);
#else
      ex.filter ( res.output[i] );
#endif
    }
    return true;
  }
예제 #2
0
	static void clustering(PointCloudPtr cloudPCLInput, vector<PointIndices> &vecSegmentIndices, double dClusterTolerance, unsigned int unMinClusterSize, unsigned int unMaxClusterSize)
	{
		vecSegmentIndices.clear();

		typedef pcl::search::KdTree<PointT> KdTree;
		typedef typename KdTree::Ptr KdTreePtr;

		EuclideanClusterExtraction<PointT> cluster;
		KdTreePtr cluster_tree = boost::make_shared<pcl::search::KdTree<PointT> > ();
		cluster.setInputCloud(cloudPCLInput);
		cluster.setClusterTolerance(dClusterTolerance);
		cluster.setMinClusterSize(unMinClusterSize);
		cluster.setMaxClusterSize(unMaxClusterSize);
		cluster.setSearchMethod(cluster_tree);
		cluster.extract(vecSegmentIndices);
	}
예제 #3
0
    /** \brief Given a plane, and the set of inlier indices representing it,
      * segment out the object of intererest supported by it. 
      *
      * \param[in] picked_idx the index of a point on the object
      * \param[in] cloud the full point cloud dataset
      * \param[in] plane_indices a set of indices representing the plane supporting the object of interest
      * \param[out] object the segmented resultant object 
      */
    void
    segmentObject (int picked_idx, 
                   const typename PointCloud<PointT>::ConstPtr &cloud, 
                   const PointIndices::Ptr &plane_indices, 
                   PointCloud<PointT> &object)
    {
      typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);

      // Compute the convex hull of the plane
      ConvexHull<PointT> chull;
      chull.setDimension (2);
      chull.setInputCloud (cloud);
      chull.setIndices (plane_indices);
      chull.reconstruct (*plane_hull);

      // Remove the plane indices from the data
      typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
      ExtractIndices<PointT> extract (true);
      extract.setInputCloud (cloud);
      extract.setIndices (plane_indices);
      extract.setNegative (false);
      extract.filter (*plane);
      PointIndices::Ptr indices_but_the_plane (new PointIndices);
      extract.getRemovedIndices (*indices_but_the_plane);

      // Extract all clusters above the hull
      PointIndices::Ptr points_above_plane (new PointIndices);
      ExtractPolygonalPrismData<PointT> exppd;
      exppd.setInputCloud (cloud);
      exppd.setIndices (indices_but_the_plane);
      exppd.setInputPlanarHull (plane_hull);
      exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
      exppd.setHeightLimits (0.001, 0.5);           // up to half a meter
      exppd.segment (*points_above_plane);

      vector<PointIndices> euclidean_label_indices;
      // Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
      if (cloud_->isOrganized ())
      {
        // Use an organized clustering segmentation to extract the individual clusters
        typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
        euclidean_cluster_comparator->setInputCloud (cloud);
        euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
        // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
        Label l; l.label = 0;
        PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
        // Mask the objects that we want to split into clusters
        for (const int &index : points_above_plane->indices)
          scene->points[index].label = 1;
        euclidean_cluster_comparator->setLabels (scene);

        boost::shared_ptr<std::set<uint32_t> > exclude_labels = boost::make_shared<std::set<uint32_t> > ();
        exclude_labels->insert (0);
        euclidean_cluster_comparator->setExcludeLabels (exclude_labels);

        OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
        euclidean_segmentation.setInputCloud (cloud);

        PointCloud<Label> euclidean_labels;
        euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
      }
      else
      {
        print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
        TicToc tt; tt.tic ();

        EuclideanClusterExtraction<PointT> ec;
        ec.setClusterTolerance (0.02); // 2cm
        ec.setMinClusterSize (100);
        ec.setSearchMethod (search_);
        ec.setInputCloud (cloud);
        ec.setIndices (points_above_plane);
        ec.extract (euclidean_label_indices);
        
        print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
      }

      // For each cluster found
      bool cluster_found = false;
      for (const auto &euclidean_label_index : euclidean_label_indices)
      {
        if (cluster_found)
          break;
        // Check if the point that we picked belongs to it
        for (size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
        {
          if (picked_idx != euclidean_label_index.indices[j])
            continue;
          copyPointCloud (*cloud, euclidean_label_index.indices, object);
          cluster_found = true;
          break;
        }
      }
    }
예제 #4
0
  void EuclideanClustering::extract(
    const sensor_msgs::PointCloud2ConstPtr &input)
  {
    boost::mutex::scoped_lock lock(mutex_);
    vital_checker_->poke();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
      (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*input, *cloud);
    
    std::vector<pcl::PointIndices> cluster_indices;
    // list up indices which are not NaN to use
    // organized pointcloud
    pcl::PointIndices::Ptr nonnan_indices (new pcl::PointIndices);
    for (size_t i = 0; i < cloud->points.size(); i++) {
      pcl::PointXYZ p = cloud->points[i];
      if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
        nonnan_indices->indices.push_back(i);
      }
    }

    if (nonnan_indices->indices.size() == 0) {
      // if input points is 0, publish empty data as result
      jsk_recognition_msgs::ClusterPointIndices result;
      result.header = input->header;
      result_pub_.publish(result);
      // do nothing and return it
      jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
      cluster_num_msg->header = input->header;
      cluster_num_msg->data = 0;
      cluster_num_pub_.publish(cluster_num_msg);
      return;
    }
    
    EuclideanClusterExtraction<pcl::PointXYZ> impl;
    {
      jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer();
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
      pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
      tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
      pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
      tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif
      tree->setInputCloud (cloud);
      impl.setClusterTolerance (tolerance);
      impl.setMinClusterSize (minsize_);
      impl.setMaxClusterSize (maxsize_);
      impl.setSearchMethod (tree);
      impl.setIndices(nonnan_indices);
      impl.setInputCloud (cloud);
    }
    
    {
      jsk_topic_tools::ScopedTimer timer = segmentation_acc_.scopedTimer();
      impl.extract (cluster_indices);
    }
    
    // Publish result indices
    jsk_recognition_msgs::ClusterPointIndices result;
    result.cluster_indices.resize(cluster_indices.size());
    cluster_counter_.add(cluster_indices.size());
    result.header = input->header;
    if (cogs_.size() != 0 && cogs_.size() == cluster_indices.size()) {
      // tracking the labels
      //ROS_INFO("computing distance matrix");
      // compute distance matrix
      // D[i][j] --> distance between the i-th previous cluster
      //             and the current j-th cluster
      Vector4fVector new_cogs;
      computeCentroidsOfClusters(new_cogs, cloud, cluster_indices);
      double D[cogs_.size() * new_cogs.size()];
      computeDistanceMatrix(D, cogs_, new_cogs);
      std::vector<int> pivot_table = buildLabelTrackingPivotTable(D, cogs_, new_cogs, label_tracking_tolerance);
      if (pivot_table.size() != 0) {
        cluster_indices = pivotClusterIndices(pivot_table, cluster_indices);
      }
    }
    Vector4fVector tmp_cogs;
    computeCentroidsOfClusters(tmp_cogs, cloud, cluster_indices); // NB: not efficient
    cogs_ = tmp_cogs;
      
    for (size_t i = 0; i < cluster_indices.size(); i++) {
#if ROS_VERSION_MINIMUM(1, 10, 0)
      // hydro and later
      result.cluster_indices[i].header
        = pcl_conversions::fromPCL(cluster_indices[i].header);
#else
      // groovy
      result.cluster_indices[i].header = cluster_indices[i].header;
#endif
      result.cluster_indices[i].indices = cluster_indices[i].indices;
    }

    result_pub_.publish(result);
    
    jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
    cluster_num_msg->header = input->header;
    cluster_num_msg->data = cluster_indices.size();
    cluster_num_pub_.publish(cluster_num_msg);

    diagnostic_updater_->update();
  }
예제 #5
0
void segment(PointCloud<PointXYZRGB> cloud)
{
  CloudT object_cloud = get_object_points(cloud);

  objects_pub.publish(object_cloud);

  // Estimate point normals
  //  PointCloud<Normal> cloud_normals;
  //  KdTreeANN<PointXYZRGB>::Ptr tree = boost::make_shared<KdTreeANN<PointXYZRGB> >();
  //  NormalEstimation<PointXYZRGB, Normal> ne;
  //  ne.setSearchMethod(tree);
  //  ne.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud));
  //  ne.setKSearch(50);
  //  ne.compute(cloud_normals);

  // Extract clusters of points (i.e., objects)
  EuclideanClusterExtraction<PointXYZRGB> clustering;
  KdTreeANN<PointXYZRGB>::Ptr cluster_tree = boost::make_shared<KdTreeANN<PointXYZRGB> >();
  vector<PointIndices> clusters;
  clustering.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud));
  clustering.setClusterTolerance(0.05); // ?
  clustering.setMinClusterSize(300);
  clustering.setSearchMethod(cluster_tree); // Not sure if this should be ANN, or FLANN, or if it matters
  clustering.extract(clusters);

  //  cout << (cloud_normals.height * cloud_normals.width) << " " << (object_cloud.height * object_cloud.width) << endl;

  cout << "FOUND " << clusters.size() << " OBJECTS" << endl;
  for (uint i = 0; i < clusters.size(); i++)
  {
    cout << "CLUSTER " << i << " has " << clusters[i].indices.size() << " points" << endl;

    PointCloud<PointXYZRGB> cluster_points;
    ExtractIndices<PointXYZRGB> extract_cluster;
    extract_cluster.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud));
    extract_cluster.setIndices(boost::make_shared<PointIndices>(clusters[i]));
    extract_cluster.filter(cluster_points);

    cluster_pub.publish(cluster_points);

    // Estimate point normals
    PointCloud<Normal> cluster_normals;
    KdTreeANN<PointT>::Ptr tree = boost::make_shared<KdTreeANN<PointT> >();
    // Estimate point normals
    NormalEstimation<PointT, Normal> ne;
    ne.setSearchMethod(tree);
    ne.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cluster_points));
    ne.setKSearch(10);
    ne.compute(cluster_normals);

    //    NormalEstimation<PointT, Normal> ne;
    //    ne.setSearchMethod(tree);
    //    ne.setInputCloud(boost::make_shared<CloudT>(cluster_points));
    //    ne.setKSearch(50);
    //    ne.compute(cluster_normals);

    //    cout << cluster_points.height * cluster_points.width << " " << cluster_normals.height * cluster_normals.width
    //        << endl;

    // Obtain the plane inliers and coefficients
    ModelCoefficients coefficients_plane;
    PointIndices inliers_plane;
    SACSegmentationFromNormals<PointT, Normal> seg_plane;
    seg_plane.setOptimizeCoefficients(true);
    seg_plane.setMethodType(SAC_RANSAC);
    seg_plane.setModelType(SACMODEL_NORMAL_PLANE);
    seg_plane.setDistanceThreshold(0.05);
    seg_plane.setInputCloud(boost::make_shared<CloudT>(cluster_points));
    seg_plane.setInputNormals(boost::make_shared<PointCloud<Normal> >(cluster_normals));
    seg_plane.segment(inliers_plane, coefficients_plane);

    //    cout << "Plane coefficients: " << coefficients_plane << endl;
    cout << "PLANE INLIERS: " << inliers_plane.indices.size() << endl;

    // Obtain the Sphere inliers and coefficients
    ModelCoefficients coefficients_sphere;
    PointIndices inliers_sphere;
    SACSegmentation<PointXYZRGB> seg_sphere;
    seg_sphere.setOptimizeCoefficients(true);
    seg_sphere.setMethodType(SAC_RANSAC);
    seg_sphere.setModelType(SACMODEL_SPHERE);
    seg_sphere.setRadiusLimits(0.01, 0.1);
    seg_sphere.setDistanceThreshold(0.005);
    seg_sphere.setInputCloud(boost::make_shared<CloudT>(cluster_points));
    seg_sphere.segment(inliers_sphere, coefficients_sphere);

    //    cout << "Sphere coefficients: " << coefficients_sphere << endl;
    cout << "SPHERE INLIERS: " << inliers_sphere.indices.size() << endl;

    ModelCoefficients coefficients_cylinder;
    PointIndices inliers_cylinder;
    SACSegmentationFromNormals<PointT, Normal> seg_cylinder;
    seg_cylinder.setOptimizeCoefficients(true);
    seg_cylinder.setModelType(SACMODEL_CYLINDER);
    seg_cylinder.setMethodType(SAC_RANSAC);
    seg_cylinder.setNormalDistanceWeight(0.1);
    seg_cylinder.setMaxIterations(1000);
    seg_cylinder.setDistanceThreshold(0.05);
    seg_cylinder.setRadiusLimits(0.01, 0.1);
    seg_cylinder.setInputCloud(boost::make_shared<CloudT>(cluster_points));
    seg_cylinder.setInputNormals(boost::make_shared<PointCloud<Normal> >(cluster_normals));
    seg_cylinder.segment(inliers_cylinder, coefficients_cylinder);

    cout << "CYLINDER INLIERS: " << inliers_cylinder.indices.size() << endl;
    cout << "Cylinder coefficients: " << coefficients_cylinder << endl;

    cout << endl;

    if (!inliers_sphere.indices.empty())
    {
      visualization_msgs::Marker marker;
      marker.header.frame_id = coefficients_sphere.header.frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = "basic_shapes" + boost::lexical_cast<string>(i);
      marker.id = 0;
      marker.type = visualization_msgs::Marker::SPHERE;
      marker.action = visualization_msgs::Marker::ADD;
      marker.pose.position.x = coefficients_sphere.values[0];
      marker.pose.position.y = coefficients_sphere.values[1];
      marker.pose.position.z = coefficients_sphere.values[2];
      marker.pose.orientation.x = 0.0;
      marker.pose.orientation.y = 0.0;
      marker.pose.orientation.z = 0.0;
      marker.pose.orientation.w = 1.0;
      marker.scale.x = coefficients_sphere.values[3] * 2;
      marker.scale.y = coefficients_sphere.values[3] * 2;
      marker.scale.z = coefficients_sphere.values[3] * 2;
      marker.color.r = 0.0f;
      marker.color.g = 1.0f;
      marker.color.b = 0.0f;
      marker.color.a = 1.0;
      marker.lifetime = ros::Duration();
      marker_pub.publish(marker);
    }

    if (!inliers_cylinder.indices.empty())
    {
      // TODO: The cylinder coefficients are not as straightforward, need to do some
      // math to pull out the location and angle (from axis orientation and point on axis)
//      visualization_msgs::Marker marker;
//      marker.header.frame_id = coefficients_sphere.header.frame_id;
//      marker.header.stamp = ros::Time::now();
//      marker.ns = "basic_shapes" + boost::lexical_cast<string>(i);
//      marker.id = 0;
//      marker.type = visualization_msgs::Marker::SPHERE;
//      marker.action = visualization_msgs::Marker::ADD;
//      marker.pose.position.x = coefficients_sphere.values[0];
//      marker.pose.position.y = coefficients_sphere.values[1];
//      marker.pose.position.z = coefficients_sphere.values[2];
//      marker.pose.orientation.x = 0.0;
//      marker.pose.orientation.y = 0.0;
//      marker.pose.orientation.z = 0.0;
//      marker.pose.orientation.w = 1.0;
//      marker.scale.x = coefficients_sphere.values[3] * 2;
//      marker.scale.y = coefficients_sphere.values[3] * 2;
//      marker.scale.z = coefficients_sphere.values[3] * 2;
//      marker.color.r = 0.0f;
//      marker.color.g = 1.0f;
//      marker.color.b = 0.0f;
//      marker.color.a = 1.0;
//      marker.lifetime = ros::Duration();
//      marker_pub.publish(marker);
    }
  }
}
// call Euclidean Cluster Extraction (ref: http://www.pointclouds.org/documentation/tutorials/cluster_extraction.php)
bool clusterize(ClusterSegmentation::Request  &req, ClusterSegmentation::Response &res){

	// get data and convert in useful format considering also default
	PCLCloudPtr cloud = pcm::PCManager::cloudForRosMsg( req.cloud);
	double tolerance, minClusterSizeRate, maxClusterSizeRate;
	int minInputSize;

    nh_ptr->param(srvm::PARAM_NAME_CLUSTER_TOLERANCE,
                  tolerance, CLUSTER_TOLERANCE_DEFAULT);
    nh_ptr->param(srvm::PARAM_NAME_CLUSTER_MIN_RATE,
                  minClusterSizeRate, CLUSTER_MIN_RATE_DEFAULT);
    nh_ptr->param(srvm::PARAM_NAME_CLUSTER_MAX_RATE,
                  maxClusterSizeRate, CLUSTER_MAX_RATE_DEFAULT);
    nh_ptr->param(srvm::PARAM_NAME_CLUSTER_TOLERANCE,
                  minInputSize, CLUSTER_MIN_INPUT_SIZE_DEFAULT);

	if( cloud->points.size() >= minInputSize){ // skip if input cloud is too small

		// Creating the KdTree object for the search method of the extraction
		search::KdTree< PointXYZ>::Ptr tree (new search::KdTree< PointXYZ>);
		tree->setInputCloud ( cloud);

		// compute clusters
		vector< PointIndices> cluster_indices;
		EuclideanClusterExtraction< PointXYZ> ec;
		ec.setClusterTolerance(tolerance); // in meters
		ec.setMinClusterSize( round(cloud->points.size () * minClusterSizeRate)); // percentage
		ec.setMaxClusterSize( round(cloud->points.size () * maxClusterSizeRate));
		//ROS_ERROR( "%d  %f   %f", cloud->points.size(), cloud->points.size () * minClusterSizeRate, cloud->points.size () * maxClusterSizeRate);
		ec.setSearchMethod( tree);
		ec.setInputCloud( cloud);
		ec.extract( cluster_indices);

		// for all the cluster
		for ( vector< pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
			// build inliers output
			vector< int>* inlier (new std::vector< int>);
			PCLCloudPtr cloudExtraxted (new PCLCloud);
			// build centroid output
			float xSumm = 0, ySumm = 0, zSumm = 0;
			int cnt = 1;
			// for all the point of a cluster (create a new point cloud for every clusters)
			for ( vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
				// set inlier
				inlier->push_back( *pit);
				// set a point of the cloud
				PointXYZ* p ( new PointXYZ( cloud->points[*pit].x, cloud->points[*pit].y, cloud->points[*pit].z));
				cloudExtraxted->push_back( *p);

				// save summ to compute avarage
				xSumm += cloud->points[*pit].x;
				ySumm += cloud->points[*pit].y;
				zSumm += cloud->points[*pit].z;
				cnt++;
			}

			// create returning object
			InliersCluster* cluster ( new InliersCluster);
			cluster->inliers = *inlier;
			cluster->cloud = PCManager::cloudToRosMsg( cloudExtraxted);
			// compute and assign to output the cluster centroid
			cluster->x_centroid = xSumm / cnt;
			cluster->y_centroid = ySumm / cnt;
			cluster->z_centroid = zSumm / cnt;
			// add to returning values
			res.cluster_objs.push_back( *cluster);
		}
	}

	return true;
}
예제 #7
0
int main(int argc, char** argv)
{
	if (argc < 2)
	{
		cout << "Input a PCD file name...\n";
		return 0;
	}

	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>);
	PCDReader reader;
	reader.read(argv[1], *cloud);
	cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n";

	PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>);
	VoxelGrid<PointXYZ> vg;
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_filtered);
	cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n";

	SACSegmentation<PointXYZ> seg;
	PointIndices::Ptr inliers(new PointIndices);
	PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>);

	ModelCoefficients::Ptr coefficients(new ModelCoefficients);
	seg.setOptimizeCoefficients(true);
	seg.setModelType(SACMODEL_PLANE);
	seg.setMethodType(SAC_RANSAC);
	seg.setMaxIterations(100);
	seg.setDistanceThreshold(0.02);

	int i=0, nr_points = (int)cloud_filtered->points.size();
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			cout << "Coud not estimate a planar model for the given dataset.\n";
			break;
		}

		ExtractIndices<PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_plane);
		cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n";

		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered->swap(*cloud_f);
	}

	search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
	kdtree->setInputCloud(cloud_filtered);

	vector<PointIndices> cluster_indices;
	EuclideanClusterExtraction<PointXYZ> ece;
	ece.setClusterTolerance(0.02);
	ece.setMinClusterSize(100);
	ece.setMaxClusterSize(25000);
	ece.setSearchMethod(kdtree);
	ece.setInputCloud(cloud_filtered);
	ece.extract(cluster_indices);

	PCDWriter writer;
	int j = 0;
	for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>);
		for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit)
			cluster_cloud->points.push_back(cloud_filtered->points[*pit]);
		cluster_cloud->width = cluster_cloud->points.size();
		cluster_cloud->height = 1;
		cluster_cloud->is_dense = true;

		cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n";

		stringstream ss;
		ss << "cloud_cluster_" << j << ".pcd";
		writer.write<PointXYZ>(ss.str(), *cluster_cloud, false);
		j++;
	}

	return 0;
}