Point Cloud Library (PCL)  1.3.1
dominant_plane_segmentation.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00036 #include <boost/thread/thread.hpp>
00037 #include <boost/date_time/posix_time/posix_time.hpp>
00038 
00039 #include <pcl/apps/dominant_plane_segmentation.h>
00040 #include <pcl/visualization/pcl_visualizer.h>
00041 #include <pcl/features/integral_image_normal.h>
00042 #include <pcl/common/time.h>
00043 
00044 template<typename PointType>
00045 void
00046 pcl::apps::DominantPlaneSegmentation<PointType>::compute_fast (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters)
00047 {
00048   // Has the input dataset been set already?
00049   if (!input_)
00050   {
00051     PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
00052     return;
00053   }
00054 
00055   // Is the input dataset organized?
00056   if (input_->is_dense)
00057   {
00058     PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] compute_fast can only be used with organized point clouds!\n");
00059     return;
00060   }
00061 
00062   CloudConstPtr cloud_;
00063   CloudPtr cloud_filtered_ (new Cloud ());
00064   CloudPtr cloud_downsampled_ (new Cloud ());
00065   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
00066   pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
00067   pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
00068   CloudPtr table_projected_ (new Cloud ());
00069   CloudPtr table_hull_ (new Cloud ());
00070   CloudPtr cloud_objects_ (new Cloud ());
00071   CloudPtr cluster_object_ (new Cloud ());
00072 
00073   typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
00074   typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
00075   clusters_tree_->setEpsilon (1);
00076 
00077   // Normal estimation parameters
00078   n3d_.setKSearch (k_);
00079   n3d_.setSearchMethod (normals_tree_);
00080 
00081   // Table model fitting parameters
00082   seg_.setDistanceThreshold (sac_distance_threshold_);
00083   seg_.setMaxIterations (2000);
00084   seg_.setNormalDistanceWeight (0.1);
00085   seg_.setOptimizeCoefficients (true);
00086   seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00087   seg_.setMethodType (pcl::SAC_RANSAC);
00088   seg_.setProbability (0.99);
00089 
00090   proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00091   bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00092 
00093   prism_.setHeightLimits (object_min_height_, object_max_height_);
00094 
00095   // Clustering parameters
00096   cluster_.setClusterTolerance (object_cluster_tolerance_);
00097   cluster_.setMinClusterSize (object_cluster_min_size_);
00098   cluster_.setSearchMethod (clusters_tree_);
00099 
00100   // ---[ PassThroughFilter
00101   pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
00102   pass_.setFilterFieldName ("z");
00103   pass_.setInputCloud (input_);
00104   pass_.filter (*cloud_filtered_);
00105 
00106   if ((int)cloud_filtered_->points.size () < k_)
00107   {
00108     PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %d points! Aborting.",
00109         (int)cloud_filtered_->points.size ());
00110     return;
00111   }
00112 
00113   // Downsample the point cloud
00114   grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
00115   grid_.setDownsampleAllData (false);
00116   grid_.setInputCloud (cloud_filtered_);
00117   grid_.filter (*cloud_downsampled_);
00118 
00119   // ---[ Estimate the point normals
00120   n3d_.setInputCloud (cloud_downsampled_);
00121   n3d_.compute (*cloud_normals_);
00122 
00123   // ---[ Perform segmentation
00124   seg_.setInputCloud (cloud_downsampled_);
00125   seg_.setInputNormals (cloud_normals_);
00126   seg_.segment (*table_inliers_, *table_coefficients_);
00127 
00128   if (table_inliers_->indices.size () == 0)
00129   {
00130     PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
00131     return;
00132   }
00133 
00134   // ---[ Extract the plane
00135   proj_.setInputCloud (cloud_downsampled_);
00136   proj_.setIndices (table_inliers_);
00137   proj_.setModelCoefficients (table_coefficients_);
00138   proj_.filter (*table_projected_);
00139 
00140   // ---[ Estimate the convex hull
00141   std::vector<pcl::Vertices> polygons;
00142   CloudPtr table_hull (new Cloud ());
00143   hull_.setInputCloud (table_projected_);
00144   hull_.reconstruct (*table_hull, polygons);
00145 
00146   // Compute the plane coefficients
00147   Eigen::Vector4f model_coefficients;
00148   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00149 
00150   model_coefficients[0] = table_coefficients_->values[0];
00151   model_coefficients[1] = table_coefficients_->values[1];
00152   model_coefficients[2] = table_coefficients_->values[2];
00153   model_coefficients[3] = table_coefficients_->values[3];
00154 
00155   // Need to flip the plane normal towards the viewpoint
00156   Eigen::Vector4f vp (0, 0, 0, 0);
00157   // See if we need to flip any plane normals
00158   vp -= table_hull->points[0].getVector4fMap ();
00159   vp[3] = 0;
00160   // Dot product between the (viewpoint - point) and the plane normal
00161   float cos_theta = vp.dot (model_coefficients);
00162   // Flip the plane normal
00163   if (cos_theta < 0)
00164   {
00165     model_coefficients *= -1;
00166     model_coefficients[3] = 0;
00167     // Hessian form (D = nc . p_plane (centroid here) + p)
00168     model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
00169   }
00170 
00171   //Set table_coeffs
00172   table_coeffs_ = model_coefficients;
00173 
00174   // ---[ Get the objects on top of the table
00175   pcl::PointIndices cloud_object_indices;
00176   prism_.setInputCloud (input_);
00177   prism_.setInputPlanarHull (table_hull);
00178   prism_.segment (cloud_object_indices);
00179 
00180   pcl::ExtractIndices<PointType> extract_object_indices;
00181   extract_object_indices.setInputCloud (input_);
00182   extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00183   extract_object_indices.filter (*cloud_objects_);
00184 
00185   //create new binary pointcloud with intensity values (0 and 1), 0 for non-object pixels and 1 otherwise
00186   pcl::PointCloud<pcl::PointXYZI>::Ptr binary_cloud (new pcl::PointCloud<pcl::PointXYZI>);
00187 
00188   {
00189     binary_cloud->width = input_->width;
00190     binary_cloud->height = input_->height;
00191     binary_cloud->points.resize (input_->points.size ());
00192     binary_cloud->is_dense = input_->is_dense;
00193 
00194     size_t idx;
00195     for (size_t i = 0; i < cloud_object_indices.indices.size (); ++i)
00196     {
00197       idx = cloud_object_indices.indices[i];
00198       binary_cloud->points[idx].getVector4fMap () = input_->points[idx].getVector4fMap ();
00199       binary_cloud->points[idx].intensity = 1.0;
00200     }
00201   }
00202 
00203   //connected components on the binary image
00204 
00205   std::map<float, float> connected_labels;
00206   float c_intensity = 0.1;
00207   float intensity_incr = 0.1;
00208 
00209   {
00210 
00211     size_t wsize = wsize_;
00212     for (size_t i = 0; i < binary_cloud->width; i++)
00213     {
00214       for (size_t j = 0; j < binary_cloud->height; j++)
00215       {
00216         if (binary_cloud->at (i, j).intensity != 0)
00217         {
00218           //check neighboring pixels, first left and then top
00219           //be aware of margins
00220 
00221           if ((i - 1) < 0 && (j - 1) < 0)
00222           {
00223             //top-left pixel
00224             (*binary_cloud) (i, j).intensity = c_intensity;
00225           }
00226           else
00227           {
00228             if ((j - 1) < 0)
00229             {
00230               //top-row, check on the left of pixel to assign a new label or not
00231               int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00232               if (left)
00233               {
00234                 //Nothing found on the left, check bigger window
00235 
00236                 bool found = false;
00237                 for (size_t kk = 2; kk < wsize && !found; kk++)
00238                 {
00239                   if ((i - kk) < 0)
00240                     continue;
00241 
00242                   int left = check ((*binary_cloud) (i - kk, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00243                   if (left == 0)
00244                   {
00245                     found = true;
00246                   }
00247                 }
00248 
00249                 if (!found)
00250                 {
00251                   c_intensity += intensity_incr;
00252                   (*binary_cloud) (i, j).intensity = c_intensity;
00253                 }
00254 
00255               }
00256             }
00257             else
00258             {
00259               if ((i - 1) == 0)
00260               {
00261                 //check only top
00262                 int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00263                 if (top)
00264                 {
00265                   bool found = false;
00266                   for (size_t kk = 2; kk < wsize && !found; kk++)
00267                   {
00268                     if ((j - kk) < 0)
00269                       continue;
00270 
00271                     int top = check ((*binary_cloud) (i, j - kk), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00272                     if (top == 0)
00273                     {
00274                       found = true;
00275                     }
00276                   }
00277 
00278                   if (!found)
00279                   {
00280                     c_intensity += intensity_incr;
00281                     (*binary_cloud) (i, j).intensity = c_intensity;
00282                   }
00283                 }
00284 
00285               }
00286               else
00287               {
00288                 //check left and top
00289                 int left = check ((*binary_cloud) (i - 1, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00290                 int top = check ((*binary_cloud) (i, j - 1), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00291 
00292                 if (left == 0 && top == 0)
00293                 {
00294                   //both top and left had labels, check if they are different
00295                   //if they are, take the smallest one and mark labels to be connected..
00296 
00297                   if ((*binary_cloud) (i - 1, j).intensity != (*binary_cloud) (i, j - 1).intensity)
00298                   {
00299                     float smaller_intensity = (*binary_cloud) (i - 1, j).intensity;
00300                     float bigger_intensity = (*binary_cloud) (i, j - 1).intensity;
00301 
00302                     if ((*binary_cloud) (i - 1, j).intensity > (*binary_cloud) (i, j - 1).intensity)
00303                     {
00304                       smaller_intensity = (*binary_cloud) (i, j - 1).intensity;
00305                       bigger_intensity = (*binary_cloud) (i - 1, j).intensity;
00306                     }
00307 
00308                     connected_labels[bigger_intensity] = smaller_intensity;
00309                     (*binary_cloud) (i, j).intensity = smaller_intensity;
00310                   }
00311                 }
00312 
00313                 if (left == 1 && top == 1)
00314                 {
00315                   //if none had labels, increment c_intensity
00316                   //search first on bigger window
00317                   bool found = false;
00318                   for (size_t dist = 2; dist < wsize && !found; dist++)
00319                   {
00320                     if (((i - dist) < 0) || ((j - dist) < 0))
00321                       continue;
00322 
00323                     int left = check ((*binary_cloud) (i - dist, j), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00324                     int top = check ((*binary_cloud) (i, j - dist), (*binary_cloud) (i, j), c_intensity, object_cluster_tolerance_);
00325 
00326                     if (left == 0 && top == 0)
00327                     {
00328                       if ((*binary_cloud) (i - dist, j).intensity != (*binary_cloud) (i, j - dist).intensity)
00329                       {
00330                         float smaller_intensity = (*binary_cloud) (i - dist, j).intensity;
00331                         float bigger_intensity = (*binary_cloud) (i, j - dist).intensity;
00332 
00333                         if ((*binary_cloud) (i - dist, j).intensity > (*binary_cloud) (i, j - dist).intensity)
00334                         {
00335                           smaller_intensity = (*binary_cloud) (i, j - dist).intensity;
00336                           bigger_intensity = (*binary_cloud) (i - dist, j).intensity;
00337                         }
00338 
00339                         connected_labels[bigger_intensity] = smaller_intensity;
00340                         (*binary_cloud) (i, j).intensity = smaller_intensity;
00341                         found = true;
00342                       }
00343                     }
00344                     else if (left == 0 || top == 0)
00345                     {
00346                       //one had label
00347                       found = true;
00348                     }
00349                   }
00350 
00351                   if (!found)
00352                   {
00353                     //none had label in the bigger window
00354                     c_intensity += intensity_incr;
00355                     (*binary_cloud) (i, j).intensity = c_intensity;
00356                   }
00357                 }
00358               }
00359             }
00360           }
00361 
00362         }
00363       }
00364     }
00365   }
00366 
00367   std::map<float, pcl::PointIndices> clusters_map;
00368 
00369   {
00370     std::map<float, float>::iterator it;
00371 
00372     for (size_t i = 0; i < binary_cloud->width; i++)
00373     {
00374       for (size_t j = 0; j < binary_cloud->height; j++)
00375       {
00376         if (binary_cloud->at (i, j).intensity != 0)
00377         {
00378           //check if this is a root label...
00379           it = connected_labels.find (binary_cloud->at (i, j).intensity);
00380           while (it != connected_labels.end ())
00381           {
00382             //the label is on the list, change pixel intensity until it has a root label
00383             (*binary_cloud) (i, j).intensity = (*it).second;
00384             it = connected_labels.find (binary_cloud->at (i, j).intensity);
00385           }
00386 
00387           std::map<float, pcl::PointIndices>::iterator it_indices;
00388           it_indices = clusters_map.find (binary_cloud->at (i, j).intensity);
00389           if (it_indices == clusters_map.end ())
00390           {
00391             pcl::PointIndices indices;
00392             clusters_map[binary_cloud->at (i, j).intensity] = indices;
00393           }
00394 
00395           clusters_map[binary_cloud->at (i, j).intensity].indices.push_back (j * binary_cloud->width + i);
00396         }
00397       }
00398     }
00399   }
00400 
00401   clusters.resize (clusters_map.size ());
00402 
00403   std::map<float, pcl::PointIndices>::iterator it_indices;
00404   int k = 0;
00405   for (it_indices = clusters_map.begin (); it_indices != clusters_map.end (); it_indices++)
00406   {
00407 
00408     if ((*it_indices).second.indices.size () >= object_cluster_min_size_)
00409     {
00410 
00411       clusters[k] = (CloudPtr)(new Cloud ());
00412       pcl::copyPointCloud (*input_, (*it_indices).second.indices, *clusters[k]);
00413       k++;
00414     }
00415   }
00416 
00417   clusters.resize (k);
00418 
00419 }
00420 
00421 template<typename PointType>
00422 void
00423 pcl::apps::DominantPlaneSegmentation<PointType>::compute (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters)
00424 {
00425 
00426   // Has the input dataset been set already?
00427   if (!input_)
00428   {
00429     PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
00430     return;
00431   }
00432 
00433   CloudConstPtr cloud_;
00434   CloudPtr cloud_filtered_ (new Cloud ());
00435   CloudPtr cloud_downsampled_ (new Cloud ());
00436   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
00437   pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
00438   pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
00439   CloudPtr table_projected_ (new Cloud ());
00440   CloudPtr table_hull_ (new Cloud ());
00441   CloudPtr cloud_objects_ (new Cloud ());
00442   CloudPtr cluster_object_ (new Cloud ());
00443 
00444   typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
00445   typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
00446   clusters_tree_->setEpsilon (1);
00447 
00448   // Normal estimation parameters
00449   n3d_.setKSearch (k_);
00450   n3d_.setSearchMethod (normals_tree_);
00451 
00452   // Table model fitting parameters
00453   seg_.setDistanceThreshold (sac_distance_threshold_);
00454   seg_.setMaxIterations (2000);
00455   seg_.setNormalDistanceWeight (0.1);
00456   seg_.setOptimizeCoefficients (true);
00457   seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00458   seg_.setMethodType (pcl::SAC_RANSAC);
00459   seg_.setProbability (0.99);
00460 
00461   proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00462   bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00463 
00464   prism_.setHeightLimits (object_min_height_, object_max_height_);
00465 
00466   // Clustering parameters
00467   cluster_.setClusterTolerance (object_cluster_tolerance_);
00468   cluster_.setMinClusterSize (object_cluster_min_size_);
00469   cluster_.setSearchMethod (clusters_tree_);
00470 
00471   // ---[ PassThroughFilter
00472   pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
00473   pass_.setFilterFieldName ("z");
00474   pass_.setInputCloud (input_);
00475   pass_.filter (*cloud_filtered_);
00476 
00477   if ((int)cloud_filtered_->points.size () < k_)
00478   {
00479     PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %d points! Aborting.",
00480         (int)cloud_filtered_->points.size ());
00481     return;
00482   }
00483 
00484   // Downsample the point cloud
00485   grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
00486   grid_.setDownsampleAllData (false);
00487   grid_.setInputCloud (cloud_filtered_);
00488   grid_.filter (*cloud_downsampled_);
00489 
00490   PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering (%f -> %f): %d out of %d\n",
00491       min_z_bounds_, max_z_bounds_, (int)cloud_downsampled_->points.size (), (int)input_->points.size ());
00492 
00493   // ---[ Estimate the point normals
00494   n3d_.setInputCloud (cloud_downsampled_);
00495   n3d_.compute (*cloud_normals_);
00496 
00497   PCL_INFO ("[DominantPlaneSegmentation] %d normals estimated. \n", (int)cloud_normals_->points.size ());
00498 
00499   // ---[ Perform segmentation
00500   seg_.setInputCloud (cloud_downsampled_);
00501   seg_.setInputNormals (cloud_normals_);
00502   seg_.segment (*table_inliers_, *table_coefficients_);
00503 
00504   if (table_inliers_->indices.size () == 0)
00505   {
00506     PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
00507     return;
00508   }
00509 
00510   // ---[ Extract the plane
00511   proj_.setInputCloud (cloud_downsampled_);
00512   proj_.setIndices (table_inliers_);
00513   proj_.setModelCoefficients (table_coefficients_);
00514   proj_.filter (*table_projected_);
00515 
00516   // ---[ Estimate the convex hull
00517   std::vector<pcl::Vertices> polygons;
00518   CloudPtr table_hull (new Cloud ());
00519   hull_.setInputCloud (table_projected_);
00520   hull_.reconstruct (*table_hull, polygons);
00521 
00522   // Compute the plane coefficients
00523   Eigen::Vector4f model_coefficients;
00524   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00525 
00526   model_coefficients[0] = table_coefficients_->values[0];
00527   model_coefficients[1] = table_coefficients_->values[1];
00528   model_coefficients[2] = table_coefficients_->values[2];
00529   model_coefficients[3] = table_coefficients_->values[3];
00530 
00531   // Need to flip the plane normal towards the viewpoint
00532   Eigen::Vector4f vp (0, 0, 0, 0);
00533   // See if we need to flip any plane normals
00534   vp -= table_hull->points[0].getVector4fMap ();
00535   vp[3] = 0;
00536   // Dot product between the (viewpoint - point) and the plane normal
00537   float cos_theta = vp.dot (model_coefficients);
00538   // Flip the plane normal
00539   if (cos_theta < 0)
00540   {
00541     model_coefficients *= -1;
00542     model_coefficients[3] = 0;
00543     // Hessian form (D = nc . p_plane (centroid here) + p)
00544     model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
00545   }
00546 
00547   //Set table_coeffs
00548   table_coeffs_ = model_coefficients;
00549 
00550   // ---[ Get the objects on top of the table
00551   pcl::PointIndices cloud_object_indices;
00552   prism_.setInputCloud (cloud_downsampled_);
00553   prism_.setInputPlanarHull (table_hull);
00554   prism_.segment (cloud_object_indices);
00555 
00556   pcl::ExtractIndices<PointType> extract_object_indices;
00557   extract_object_indices.setInputCloud (cloud_downsampled_);
00558   extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00559   extract_object_indices.filter (*cloud_objects_);
00560 
00561   if (cloud_objects_->points.size () == 0)
00562     return;
00563 
00564   //down_.reset(new Cloud(*cloud_downsampled_));
00565 
00566   // ---[ Split the objects into Euclidean clusters
00567   std::vector<pcl::PointIndices> clusters2;
00568   cluster_.setInputCloud (cloud_downsampled_);
00569   cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00570   cluster_.extract (clusters2);
00571 
00572   PCL_INFO ("[DominantPlaneSegmentation::compute()] Number of clusters found matching the given constraints: %d.\n",
00573       (int)clusters2.size ());
00574 
00575   clusters.resize (clusters2.size ());
00576 
00577   for (size_t i = 0; i < clusters2.size (); ++i)
00578   {
00579     clusters[i] = (CloudPtr)(new Cloud ());
00580     pcl::copyPointCloud (*cloud_downsampled_, clusters2[i].indices, *clusters[i]);
00581   }
00582 }
00583 
00584 template<typename PointType>
00585 void
00586 pcl::apps::DominantPlaneSegmentation<PointType>::compute_full (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters)
00587 {
00588 
00589   // Has the input dataset been set already?
00590   if (!input_)
00591   {
00592     PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
00593     return;
00594   }
00595 
00596   CloudConstPtr cloud_;
00597   CloudPtr cloud_filtered_ (new Cloud ());
00598   CloudPtr cloud_downsampled_ (new Cloud ());
00599   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
00600   pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
00601   pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
00602   CloudPtr table_projected_ (new Cloud ());
00603   CloudPtr table_hull_ (new Cloud ());
00604   CloudPtr cloud_objects_ (new Cloud ());
00605   CloudPtr cluster_object_ (new Cloud ());
00606 
00607   typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
00608   typename pcl::search::KdTree<PointType>::Ptr clusters_tree_ (new pcl::search::KdTree<PointType>);
00609   clusters_tree_->setEpsilon (1);
00610 
00611   // Normal estimation parameters
00612   n3d_.setKSearch (10);
00613   n3d_.setSearchMethod (normals_tree_);
00614 
00615   // Table model fitting parameters
00616   seg_.setDistanceThreshold (sac_distance_threshold_);
00617   seg_.setMaxIterations (2000);
00618   seg_.setNormalDistanceWeight (0.1);
00619   seg_.setOptimizeCoefficients (true);
00620   seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00621   seg_.setMethodType (pcl::SAC_MSAC);
00622   seg_.setProbability (0.98);
00623 
00624   proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00625   bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00626 
00627   prism_.setHeightLimits (object_min_height_, object_max_height_);
00628 
00629   // Clustering parameters
00630   cluster_.setClusterTolerance (object_cluster_tolerance_);
00631   cluster_.setMinClusterSize (object_cluster_min_size_);
00632   cluster_.setSearchMethod (clusters_tree_);
00633 
00634   // ---[ PassThroughFilter
00635   pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
00636   pass_.setFilterFieldName ("z");
00637   pass_.setInputCloud (input_);
00638   pass_.filter (*cloud_filtered_);
00639 
00640   if ((int)cloud_filtered_->points.size () < k_)
00641   {
00642     PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %d points! Aborting.",
00643         (int)cloud_filtered_->points.size ());
00644     return;
00645   }
00646 
00647   // Downsample the point cloud
00648   grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
00649   grid_.setDownsampleAllData (false);
00650   grid_.setInputCloud (cloud_filtered_);
00651   grid_.filter (*cloud_downsampled_);
00652 
00653   PCL_INFO ("[DominantPlaneSegmentation] Number of points left after filtering&downsampling (%f -> %f): %d out of %d\n",
00654       min_z_bounds_, max_z_bounds_, (int)cloud_downsampled_->points.size (), (int)input_->points.size ());
00655 
00656   // ---[ Estimate the point normals
00657   n3d_.setInputCloud (cloud_downsampled_);
00658   n3d_.compute (*cloud_normals_);
00659 
00660   PCL_INFO ("[DominantPlaneSegmentation] %d normals estimated. \n", (int)cloud_normals_->points.size ());
00661 
00662   // ---[ Perform segmentation
00663   seg_.setInputCloud (cloud_downsampled_);
00664   seg_.setInputNormals (cloud_normals_);
00665   seg_.segment (*table_inliers_, *table_coefficients_);
00666 
00667   if (table_inliers_->indices.size () == 0)
00668   {
00669     PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
00670     return;
00671   }
00672 
00673   // ---[ Extract the plane
00674   proj_.setInputCloud (cloud_downsampled_);
00675   proj_.setIndices (table_inliers_);
00676   proj_.setModelCoefficients (table_coefficients_);
00677   proj_.filter (*table_projected_);
00678 
00679   // ---[ Estimate the convex hull
00680   std::vector<pcl::Vertices> polygons;
00681   CloudPtr table_hull (new Cloud ());
00682   hull_.setInputCloud (table_projected_);
00683   hull_.reconstruct (*table_hull, polygons);
00684 
00685   // Compute the plane coefficients
00686   Eigen::Vector4f model_coefficients;
00687   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00688 
00689   model_coefficients[0] = table_coefficients_->values[0];
00690   model_coefficients[1] = table_coefficients_->values[1];
00691   model_coefficients[2] = table_coefficients_->values[2];
00692   model_coefficients[3] = table_coefficients_->values[3];
00693 
00694   // Need to flip the plane normal towards the viewpoint
00695   Eigen::Vector4f vp (0, 0, 0, 0);
00696   // See if we need to flip any plane normals
00697   vp -= table_hull->points[0].getVector4fMap ();
00698   vp[3] = 0;
00699   // Dot product between the (viewpoint - point) and the plane normal
00700   float cos_theta = vp.dot (model_coefficients);
00701   // Flip the plane normal
00702   if (cos_theta < 0)
00703   {
00704     model_coefficients *= -1;
00705     model_coefficients[3] = 0;
00706     // Hessian form (D = nc . p_plane (centroid here) + p)
00707     model_coefficients[3] = -1 * (model_coefficients.dot (table_hull->points[0].getVector4fMap ()));
00708   }
00709 
00710   //Set table_coeffs
00711   table_coeffs_ = model_coefficients;
00712 
00713   // ---[ Get the objects on top of the table
00714   pcl::PointIndices cloud_object_indices;
00715   prism_.setInputCloud (cloud_filtered_);
00716   prism_.setInputPlanarHull (table_hull);
00717   prism_.segment (cloud_object_indices);
00718 
00719   pcl::ExtractIndices<PointType> extract_object_indices;
00720   extract_object_indices.setInputCloud (cloud_downsampled_);
00721   extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00722   extract_object_indices.filter (*cloud_objects_);
00723 
00724   if (cloud_objects_->points.size () == 0)
00725     return;
00726 
00727   // ---[ Split the objects into Euclidean clusters
00728   std::vector<pcl::PointIndices> clusters2;
00729   cluster_.setInputCloud (cloud_filtered_);
00730   cluster_.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00731   cluster_.extract (clusters2);
00732 
00733   PCL_INFO ("[DominantPlaneSegmentation::compute_full()] Number of clusters found matching the given constraints: %d.\n",
00734       (int)clusters2.size ());
00735 
00736   clusters.resize (clusters2.size ());
00737 
00738   for (size_t i = 0; i < clusters2.size (); ++i)
00739   {
00740     clusters[i] = (CloudPtr)(new Cloud ());
00741     pcl::copyPointCloud (*cloud_filtered_, clusters2[i].indices, *clusters[i]);
00742   }
00743 }
00744 
00745 #define PCL_INSTANTIATE_DominantPlaneSegmentation(T) template class PCL_EXPORTS pcl::apps::DominantPlaneSegmentation<T>;
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines