openni_floodfill_planar_segmentation.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-2012, 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  */
00037 
00038 #include <pcl/io/openni_grabber.h>
00039 #include <pcl/visualization/cloud_viewer.h>
00040 #include <boost/thread/thread.hpp>
00041 #include <pcl/visualization/pcl_visualizer.h>
00042 #include <pcl/io/io.h>
00043 #include <boost/make_shared.hpp>
00044 #include <pcl/common/time.h>
00045 #include <pcl/filters/voxel_grid.h>
00046 #include <pcl/search/pcl_search.h>
00047 #include <pcl/features/integral_image_normal.h>
00048 #include <pcl/filters/extract_indices.h>
00049 #include <pcl/features/normal_3d.h>
00050 #include <pcl/ModelCoefficients.h>
00051 #include <boost/graph/incremental_components.hpp>
00052 
00053 typedef pcl::PointXYZ PointT;
00054 
00061 template<typename PointInT> bool
00062 comparePoints (PointInT p1, PointInT p2, double p1_d, double p2_d, float ang_thresh, float dist_thresh)
00063 {
00064   float dot_p = p1.normal[0] * p2.normal[0] + p1.normal[1] * p2.normal[1] + p1.normal[2] * p2.normal[2];
00065   float dist = fabsf (float (p1_d - p2_d));
00066   if ((dot_p > ang_thresh) && (dist < dist_thresh))
00067     return (true);
00068   else
00069     return (false);
00070 }
00071 
00072 
00078 template<typename PointInT, typename PointOutT> void
00079 extractPlanesByFloodFill (
00080     const pcl::PointCloud<PointInT> & cloud_in, 
00081     std::vector<pcl::ModelCoefficients>& normals_out, 
00082     std::vector<pcl::PointCloud<PointOutT> >& inliers, 
00083     unsigned min_inliers, 
00084     float ang_thresh, 
00085     float dist_thresh)
00086 {
00087   ang_thresh = cosf (ang_thresh);
00088 
00089   pcl::PointCloud<PointOutT> normal_cloud;
00090   pcl::copyPointCloud (cloud_in, normal_cloud);
00091 
00092   //estimate the normals
00093   double normal_start = pcl::getTime ();
00094   pcl::IntegralImageNormalEstimation<PointOutT, PointOutT> ne;
00095   ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
00096   ne.setMaxDepthChangeFactor (0.02f);
00097   ne.setNormalSmoothingSize (10.0f);
00098   ne.setInputCloud (boost::make_shared<pcl::PointCloud<PointOutT> >(normal_cloud));
00099   ne.compute (normal_cloud);
00100   double normal_end = pcl::getTime ();
00101   std::cout << "Normal Estimation took: " << double(normal_end - normal_start) << std::endl;
00102 
00103   //Calculate range part of planes' hessian normal form
00104   double plane_d_start = pcl::getTime ();
00105   float *plane_d = new float[normal_cloud.points.size ()];
00106   for (unsigned int i = 0; i < normal_cloud.points.size (); ++i)
00107   {
00108     plane_d[i] = normal_cloud[i].x * normal_cloud[i].normal[0] + normal_cloud[i].y * normal_cloud[i].normal[1] + normal_cloud[i].z * normal_cloud[i].normal[2];
00109   }
00110   double plane_d_end = pcl::getTime ();
00111   std::cout << "Plane_d calc took: " << double(plane_d_end - plane_d_start) << std::endl;
00112 
00113   //Connected Components
00114   //Set up for union-find
00115   double clust_start = pcl::getTime ();
00116   std::vector<int> rank (normal_cloud.points.size ());
00117   std::vector<int> parent (normal_cloud.points.size ());
00118   boost::disjoint_sets<int*, int*> ds (&rank[0], &parent[0]);
00119 
00120   int *clusts = new int[normal_cloud.points.size ()];
00121   memset (clusts, -1, normal_cloud.points.size () * sizeof(int));
00122   int clust_id = 0;
00123 
00124   //First row
00125   for (size_t ri = 1; ri < normal_cloud.height; ++ri)
00126   {
00127     if (comparePoints (normal_cloud[ri], normal_cloud[ri - 1], plane_d[ri], plane_d[ri - 1], ang_thresh, dist_thresh))
00128     {
00129       clusts[ri] = clusts[ri - 1];
00130     }
00131     else
00132     {
00133       clusts[ri] = clust_id++;
00134       ds.make_set (clusts[ri]);
00135     }
00136   }
00137 
00138   //Everything else
00139   unsigned int index = 0;
00140   unsigned int last_row_idx = 0;
00141   std::vector<std::vector<int> > same;
00142   for (size_t ri = 1; ri < normal_cloud.height; ++ri)
00143   {
00144 
00145     index = unsigned (ri) * normal_cloud.width;
00146     last_row_idx = unsigned (ri - 1) * normal_cloud.width;
00147 
00148     //First pixel
00149     if (comparePoints (normal_cloud[index], normal_cloud[last_row_idx], plane_d[index], plane_d[last_row_idx], ang_thresh, dist_thresh))
00150     {
00151       clusts[index] = clusts[last_row_idx];
00152     }
00153     else
00154     {
00155       clusts[index] = clust_id++;
00156       ds.make_set (clusts[index]);
00157     }
00158 
00159     //Rest of row
00160     for (size_t ci = 1; ci < normal_cloud.width; ++ci, ++index)
00161     {
00162       int label = -1;
00163 
00164       if (comparePoints (normal_cloud[index], normal_cloud[index - 1], plane_d[index], plane_d[index - 1], ang_thresh, dist_thresh))
00165       {
00166         label = clusts[index - 1];
00167       }
00168 
00169       if (comparePoints (normal_cloud[index], normal_cloud[last_row_idx + ci], plane_d[index], plane_d[last_row_idx + ci], ang_thresh, dist_thresh))
00170       {
00171         if ((label > 0) && (label != clusts[last_row_idx + ci]))
00172         {
00173           ds.union_set (label, clusts[last_row_idx + ci]);
00174         }
00175         else
00176         {
00177           label = clusts[last_row_idx + ci];
00178         }
00179       }
00180 
00181       if (label >= 0)
00182       {
00183         clusts[index] = label;
00184       }
00185       else
00186       {
00187         clusts[index] = clust_id++;
00188         ds.make_set (clusts[index]);
00189       }
00190     }
00191   }
00192 
00193   //Second pass
00194   index = 0;
00195   std::vector<pcl::PointIndices> clust_inds;
00196   clust_inds.resize (10);
00197   for (unsigned int ri = 0; ri < normal_cloud.height; ++ri)
00198   {
00199     for (unsigned int ci = 0; ci < normal_cloud.width; ++ci, ++index)
00200     {
00201       clusts[index] = ds.find_set (clusts[index]);
00202       if (clusts[index] >= int (clust_inds.size ()))
00203       {
00204         clust_inds.resize (clusts[index] * 2, pcl::PointIndices ());
00205       }
00206       clust_inds[clusts[index]].indices.push_back (index);
00207     }
00208   }
00209 
00210   double clust_end = pcl::getTime ();
00211   std::cout << "Clust took: " << double(clust_end - clust_start) << std::endl;
00212 
00213   //Fit planes to each cluster
00214   double fitting_start = pcl::getTime ();
00215   for (size_t i = 0; i < clust_inds.size (); i++)
00216   {
00217     if (clust_inds[i].indices.size () > min_inliers)
00218     {
00219       pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract;
00220       pcl::PointCloud<pcl::PointXYZRGBNormal> clust_inliers;
00221       extract.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> >(normal_cloud));
00222       extract.setIndices (boost::make_shared<pcl::PointIndices>(clust_inds[i]));
00223       extract.filter (clust_inliers);
00224 
00225       Eigen::Vector4f clust_centroid;
00226       Eigen::Matrix3f clust_cov;
00227       pcl::computeMeanAndCovarianceMatrix (normal_cloud, clust_inds[i].indices, clust_cov, clust_centroid);
00228       Eigen::Vector4f plane_params;
00229       float curvature;
00230       pcl::solvePlaneParameters (clust_cov, clust_centroid, plane_params, curvature);
00231       pcl::flipNormalTowardsViewpoint (normal_cloud[clust_inds[i].indices[0]], 0.0f, 0.0f, 0.0f, plane_params);
00232       pcl::ModelCoefficients model;
00233       model.values.push_back(plane_params[0]);
00234       model.values.push_back(plane_params[1]);
00235       model.values.push_back(plane_params[2]);
00236       model.values.push_back(plane_params[3]);
00237       normals_out.push_back(model);
00238       //normals_out.push_back (plane_params);
00239       inliers.push_back (clust_inliers);
00240     }
00241   }
00242   double fitting_end = pcl::getTime ();
00243   std::cout << "Fitting took: " << double(fitting_end - fitting_start) << std::endl;
00244 }
00245 
00246 
00247 //modified extractEuclideanClusters from pcl/segmentation/extract_clusters
00248 //we can save on fabs(acos()) in inner loop by doing cos(eps_angle) instead.
00249 template<typename PointT, typename Normal> void
00250 extractEuclideanClustersModified (
00251   const pcl::PointCloud<PointT> &cloud, const pcl::PointCloud<Normal> &normals,
00252   float tolerance, const boost::shared_ptr<pcl::search::KdTree<PointT> > &tree,
00253   std::vector<pcl::PointIndices> &clusters,
00254   double eps_angle,
00255   unsigned int min_pts_per_cluster,
00256   unsigned int max_pts_per_cluster)
00257 {
00258   if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00259   {
00260     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
00261     return;
00262   }
00263   if (cloud.points.size () != normals.points.size ())
00264   {
00265     PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ());
00266     return;
00267   }
00268 
00269   //Allows us to avoid acos in the loop
00270   eps_angle = cos (eps_angle);
00271 
00272   // Create a bool vector of processed point indices, and initialize it to false
00273   std::vector<bool> processed (cloud.points.size (), false);
00274 
00275   std::vector<int> nn_indices;
00276   std::vector<float> nn_distances;
00277   // Process all points in the indices vector
00278   for (size_t i = 0; i < cloud.points.size (); ++i)
00279   {
00280     if (processed[i])
00281       continue;
00282 
00283     std::vector<unsigned int> seed_queue;
00284     int sq_idx = 0;
00285     seed_queue.push_back (i);
00286 
00287     processed[i] = true;
00288 
00289     while (sq_idx < int (seed_queue.size ()))
00290     {
00291       // Search for sq_idx
00292       if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
00293       {
00294         sq_idx++;
00295         continue;
00296       }
00297 
00298       for (size_t j = 1; j < nn_indices.size (); ++j)                 // nn_indices[0] should be sq_idx
00299       {
00300         if (processed[nn_indices[j]])                               // Has this point been processed before ?
00301           continue;
00302 
00303         //processed[nn_indices[j]] = true;
00304         // [-1;1]
00305         double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
00306                        normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] +
00307                        normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2];
00308 
00309         //if ( fabs (acos (dot_p)) < eps_angle )
00310         if (dot_p > eps_angle)
00311         {
00312           processed[nn_indices[j]] = true;
00313           seed_queue.push_back (nn_indices[j]);
00314         }
00315       }
00316 
00317       sq_idx++;
00318     }
00319 
00320     // If this queue is satisfactory, add to the clusters
00321     if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
00322     {
00323       pcl::PointIndices r;
00324       r.indices.resize (seed_queue.size ());
00325       for (size_t j = 0; j < seed_queue.size (); ++j)
00326         r.indices[j] = seed_queue[j];
00327 
00328       std::sort (r.indices.begin (), r.indices.end ());
00329       r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
00330 
00331       r.header = cloud.header;
00332       clusters.push_back (r);     // We could avoid a copy by working directly in the vector
00333     }
00334   }
00335 }
00336 
00341 template<typename PointInT, typename PointOutT> void
00342 extractPlanesByClustering (const pcl::PointCloud<PointInT> & cloud_in, std::vector<Eigen::Vector4f>& normals_out, std::vector<pcl::PointCloud<PointOutT> >& inliers, int min_inliers, double ang_thresh, double dist_thresh)
00343 {
00344   //TODO: Check that PointOutT is valid -- it must have a spot for normals
00345 
00346   //Some parameters -- perhaps some of these should be args
00347   double voxel_grid_res = 0.01;
00348 
00349   pcl::PointCloud<PointOutT> normal_cloud;
00350   pcl::copyPointCloud (cloud_in, normal_cloud); //TODO: is there a better way to do this?
00351 
00352   //Estimate Normals
00353   double normal_start = pcl::getTime ();
00354   pcl::IntegralImageNormalEstimation<PointOutT, PointOutT> ne;
00355   ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
00356   ne.setMaxDepthChangeFactor (0.02f);
00357   ne.setNormalSmoothingSize (10.0f);
00358   ne.setInputCloud (boost::make_shared<pcl::PointCloud<PointOutT> >(normal_cloud));
00359   ne.compute (normal_cloud);
00360   double normal_end = pcl::getTime ();
00361   std::cout << "Normal Estimation took: " << double(normal_end - normal_start) << std::endl;
00362 
00363   //Downsample the cloud with Normals
00364   double downsample_start = pcl::getTime ();
00365   pcl::PointCloud<PointOutT> downsampled_cloud;
00366   pcl::VoxelGrid<PointOutT> grid;
00367   grid.setInputCloud (boost::make_shared<pcl::PointCloud<PointOutT> >(normal_cloud));
00368   grid.setLeafSize (voxel_grid_res, voxel_grid_res, voxel_grid_res);
00369   grid.filter (downsampled_cloud);
00370   double downsample_end = pcl::getTime ();
00371   std::cout << "Downsampling took: " << double(downsample_end - downsample_start) << std::endl;
00372 
00373   //Cluster the data
00374   double cluster_start = pcl::getTime ();
00375   std::vector<pcl::PointIndices> clusters;
00376   typename pcl::search::KdTree<PointOutT>::Ptr tree (new pcl::search::KdTree<PointOutT>());
00377   tree->setInputCloud (boost::make_shared<pcl::PointCloud<PointOutT> >(downsampled_cloud));
00378   extractEuclideanClustersModified (downsampled_cloud, downsampled_cloud, dist_thresh, tree, clusters, ang_thresh, min_inliers, 400000);
00379   double cluster_end = pcl::getTime ();
00380   std::cout << "Clustering took: " << double(cluster_end - cluster_start) << std::endl;
00381 
00382   //Fit planes to each cluster
00383   double fitting_start = pcl::getTime ();
00384   for (int i = 0; i < clusters.size (); i++)
00385   {
00386     pcl::ExtractIndices<PointOutT> extract;
00387     pcl::PointCloud<PointOutT> clust_inliers;
00388     extract.setInputCloud (boost::make_shared<pcl::PointCloud<PointOutT> >(downsampled_cloud));
00389     extract.setIndices (boost::make_shared<pcl::PointIndices>(clusters[i]));
00390     extract.filter (clust_inliers);
00391 
00392     Eigen::Vector4f clust_centroid;
00393     pcl::compute3DCentroid (clust_inliers, clust_centroid);
00394     Eigen::Matrix3f clust_cov;
00395     pcl::computeCovarianceMatrix (clust_inliers, clust_centroid, clust_cov);
00396     Eigen::Vector4f plane_params;
00397     float curvature;
00398     pcl::solvePlaneParameters (clust_cov, clust_centroid, plane_params, curvature);
00399 
00400     pcl::flipNormalTowardsViewpoint (clust_inliers.points[0], 0.0f, 0.0f, 0.0f, plane_params);
00401 
00402     normals_out.push_back (plane_params);
00403     inliers.push_back (clust_inliers);
00404   }
00405   double fitting_end = pcl::getTime ();
00406   std::cout << "Fitting took: " << double(fitting_end - fitting_start) << std::endl;
00407 }
00408 
00409 class PlaneSegTest
00410 {
00411 private:
00412 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
00413 pcl::PointCloud<PointT>::ConstPtr prev_cloud;
00414 unsigned int text_id;
00415 boost::mutex cloud_mutex;
00416 boost::mutex viewer_mutex;
00417 
00418 public:
00419 PlaneSegTest()
00420 {
00421   text_id = 0;
00422 }
00423 ~PlaneSegTest(){
00424 }
00425 
00426 boost::shared_ptr<pcl::visualization::PCLVisualizer>
00427 cloudViewer (pcl::PointCloud<PointT>::ConstPtr cloud)
00428 {
00429   boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
00430   viewer->setBackgroundColor (0, 0, 0);
00431   pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
00432   viewer->addPointCloud<PointT> (cloud, single_color, "cloud");
00433   viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
00434   viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");
00435   viewer->addCoordinateSystem (1.0);
00436   viewer->initCameraParameters ();
00437   return (viewer);
00438 }
00439 
00440 void
00441 cloud_cb_ (const pcl::PointCloud<PointT>::ConstPtr& cloud)
00442 {
00443   if (!viewer->wasStopped ())
00444   {
00445     cloud_mutex.lock ();
00446     prev_cloud = cloud;
00447     cloud_mutex.unlock ();
00448   }
00449 }
00450 
00451 void
00452 run ()
00453 {
00454   pcl::Grabber* interface = new pcl::OpenNIGrabber ();
00455 
00456   boost::function<void (const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&PlaneSegTest::cloud_cb_, this, _1);
00457 
00458   //make a viewer
00459   pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
00460   viewer = cloudViewer (init_cloud_ptr);
00461 
00462   boost::signals2::connection c = interface->registerCallback (f);
00463 
00464   interface->start ();
00465 
00466   while (!viewer->wasStopped ())
00467   {
00468     viewer->spinOnce (100);
00469 
00470     if (prev_cloud && cloud_mutex.try_lock ())
00471     {
00472       printf ("Extracting planes...\n");
00473       //std::vector<Eigen::Vector4f> plane_normals; //This doesn't work on Win, using the following instead:
00474       std::vector<pcl::ModelCoefficients> plane_normals;
00475       std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > plane_inliers;
00476 
00477       double start = pcl::getTime ();
00478       //extractPlanesByClustering(*prev_cloud,plane_normals,plane_inliers,1000,0.174,0.04);
00479       extractPlanesByFloodFill (*prev_cloud, plane_normals, plane_inliers, 10000, 0.017453f * 3.0f, 0.02f);
00480       double end = pcl::getTime ();
00481       std::cout << "Full Plane Extraction using Flood Fill for frame took: " << double(end - start) << std::endl << std::endl;
00482 
00483       //Do some visualization
00484       viewer_mutex.lock ();
00485       viewer->removeAllPointClouds ();
00486 
00487       //Display cloud
00488       pcl::PointCloud<pcl::PointXYZ> downsampled_cloud;
00489       float grid_size = 0.01f;
00490       pcl::VoxelGrid<pcl::PointXYZ> grid;
00491       grid.setInputCloud (prev_cloud);
00492       grid.setLeafSize (grid_size, grid_size, grid_size);
00493       grid.filter (downsampled_cloud);
00494 
00495       //Display normals
00496       pcl::PointCloud<pcl::Normal> normals;
00497 
00498       pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
00499       pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
00500       ne.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(downsampled_cloud));
00501       ne.setSearchMethod (tree);
00502       ne.setRadiusSearch (0.05); //(0.1);//0.5
00503       ne.compute (normals);
00504 
00505       viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(downsampled_cloud), boost::make_shared<pcl::PointCloud<pcl::Normal> >(normals), 10, 0.05f, "normals");
00506 
00507       viewer->removeAllShapes ();
00508 
00509       //Draw planar normals
00510       for (size_t i = 0; i < plane_inliers.size (); i++)
00511       {
00512         Eigen::Vector4f centroid;
00513         pcl::compute3DCentroid (plane_inliers[i], centroid);
00514         pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
00515         pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * plane_normals[i].values[0]),
00516                                            centroid[1] + (0.5f * plane_normals[i].values[1]),
00517                                            centroid[2] + (0.5f * plane_normals[i].values[2]));
00518         char normal_name[500];
00519         sprintf (normal_name, "normal_%d", unsigned (i));
00520         viewer->addArrow (pt2, pt1, 1.0, 0, 0, normal_name);
00521       }
00522 
00523       viewer->addPointCloud<pcl::PointXYZ>(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(downsampled_cloud), "cloud");
00524 
00525       //Draw a couple clusters
00526       if (plane_inliers.size () > 0)
00527       {
00528         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr clust0 = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> >(plane_inliers[0]);
00529         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBNormal> color0 (clust0, 255, 0, 0);
00530         viewer->addPointCloud (clust0, color0, "clust0");
00531         viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "clust0");
00532         viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "clust0");
00533       }
00534       if (plane_inliers.size () > 1)
00535       {
00536         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr clust1 = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> >(plane_inliers[1]);
00537         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBNormal> color1 (clust1, 0, 255, 0);
00538         viewer->addPointCloud (clust1, color1, "clust1");
00539         viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "clust1");
00540         viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "clust1");
00541       }
00542       if (plane_inliers.size () > 2)
00543       {
00544         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr clust2 = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> >(plane_inliers[2]);
00545         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBNormal> color2 (clust2, 0, 0, 255);
00546         viewer->addPointCloud (clust2, color2, "clust2");
00547         viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "clust2");
00548         viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "clust2");
00549       }
00550 
00551       cloud_mutex.unlock ();
00552       viewer_mutex.unlock ();
00553     }
00554 
00555     boost::this_thread::sleep (boost::posix_time::microseconds (100));
00556   }
00557 
00558   interface->stop ();
00559 
00560 }
00561 
00562 };
00563 
00564 int
00565 main ()
00566 {
00567   PlaneSegTest v;
00568   v.run ();
00569   return 0;
00570 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:01