example_extract_clusters_normals.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-2011, Willow Garage, Inc.
00006  *
00007  * All rights reserved.
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met:
00012  *
00013  * * Redistributions of source code must retain the above copyright
00014  *   notice, this list of conditions and the following disclaimer.
00015  * * Redistributions in binary form must reproduce the above
00016  *   copyright notice, this list of conditions and the following
00017  *   disclaimer in the documentation and/or other materials provided
00018  *   with the distribution.
00019  * * Neither the name of Willow Garage, Inc. nor the names of its
00020  *   contributors may be used to endorse or promote products derived
00021  *   from this software without specific prior written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  * POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  *
00039  */
00040 
00041 // STL
00042 #include <iostream>
00043 
00044 // PCL
00045 #include <pcl/point_types.h>
00046 #include <pcl/io/pcd_io.h>
00047 #include <pcl/filters/extract_indices.h>
00048 #include <pcl/features/normal_3d.h>
00049 #include <pcl/kdtree/kdtree.h>
00050 #include <pcl/kdtree/kdtree_flann.h>
00051 #include <pcl/segmentation/extract_clusters.h>
00052 
00053 
00054 int 
00055 main (int, char **argv)
00056 {
00057   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
00058   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
00059   pcl::PCDWriter writer;
00060         
00061   if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud_ptr) == -1)
00062   {
00063     std::cout<<"Couldn't read the file "<<argv[1]<<std::endl;
00064     return (-1);
00065   }
00066   std::cout << "Loaded pcd file " << argv[1] << " with " << cloud_ptr->points.size () << std::endl;
00067 
00068   // Normal estimation
00069   pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
00070   ne.setInputCloud (cloud_ptr);
00071 
00072   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
00073   ne.setSearchMethod (tree_n);
00074   ne.setRadiusSearch (0.03);
00075   ne.compute (*cloud_normals);
00076   std::cout << "Estimated the normals" << std::endl;
00077 
00078   // Creating the kdtree object for the search method of the extraction
00079   boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > tree_ec  (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
00080   tree_ec->setInputCloud (cloud_ptr);
00081   
00082   // Extracting Euclidean clusters using cloud and its normals
00083   std::vector<int> indices;
00084   std::vector<pcl::PointIndices> cluster_indices;
00085   const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system
00086   const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
00087   const unsigned int min_cluster_size = 50;
00088  
00089   pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size);
00090 
00091   std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl;
00092 
00093   // Saving the clusters in seperate pcd files
00094   int j = 0;
00095   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
00096   {
00097     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
00098     for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00099       cloud_cluster->points.push_back (cloud_ptr->points[*pit]); 
00100     cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ());
00101     cloud_cluster->height = 1;
00102     cloud_cluster->is_dense = true;
00103 
00104     std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->points.size () << " data points." << std::endl;
00105     std::stringstream ss;
00106     ss << "./cloud_cluster_" << j << ".pcd";
00107     writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); 
00108     j++;
00109   }
00110 
00111   return (0);
00112 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:35