Point Cloud Library (PCL)  1.3.1
dominant_plane_segmentation.h
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 #ifndef DOMINANT_PLANE_SEGMENTATION_H_
00037 #define DOMINANT_PLANE_SEGMENTATION_H_
00038 
00039 #include <pcl/common/common.h>
00040 #include <pcl/point_cloud.h>
00041 #include <pcl/point_types.h>
00042 #include <pcl/console/parse.h>
00043 #include <pcl/filters/voxel_grid.h>
00044 #include <pcl/filters/passthrough.h>
00045 #include <pcl/filters/extract_indices.h>
00046 #include <pcl/features/normal_3d.h>
00047 #include <pcl/search/pcl_search.h>
00048 #include <pcl/sample_consensus/method_types.h>
00049 #include <pcl/sample_consensus/model_types.h>
00050 #include <pcl/segmentation/sac_segmentation.h>
00051 #include <pcl/filters/project_inliers.h>
00052 #include <pcl/surface/convex_hull.h>
00053 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00054 #include <pcl/segmentation/extract_clusters.h>
00055 
00056 namespace pcl
00057 {
00058   namespace apps
00059   {
00065     template<typename PointType>
00066       class DominantPlaneSegmentation
00067       {
00068       public:
00069         typedef pcl::PointCloud<PointType> Cloud;
00070         typedef typename Cloud::Ptr CloudPtr;
00071         typedef typename Cloud::ConstPtr CloudConstPtr;
00072         typedef typename pcl::search::KdTree<PointType>::Ptr KdTreePtr;
00073 
00074         DominantPlaneSegmentation ()
00075         {
00076           min_z_bounds_ = 0;
00077           max_z_bounds_ = 1.5;
00078           object_min_height_ = 0.01;
00079           object_max_height_ = 0.7;
00080           object_cluster_tolerance_ = 0.05;
00081           object_cluster_min_size_ = 500;
00082           k_ = 50;
00083           sac_distance_threshold_ = 0.01;
00084           downsample_leaf_ = 0.005;
00085           wsize_ = 5;
00086         }
00087 
00088         /* \brief Extract the clusters.
00089          * \param clusters Clusters extracted from the initial point cloud at the resolution size
00090          * specified by downsample_leaf_
00091          */
00092         void
00093         compute (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters);
00094 
00095         /* \brief Extract the clusters.
00096          * \param clusters Clusters extracted from the initial point cloud. The returned
00097          * clusters are not downsampled.
00098          */
00099         void
00100         compute_full (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters);
00101 
00102         /* \brief Extract clusters on a plane using connected components on an organized pointcloud.
00103          * The method expects a the input cloud to have the is_dense attribute set to false.
00104           * \param clusters Clusters extracted from the initial point cloud. The returned
00105           * clusters are not downsampled.
00106           */
00107         void
00108         compute_fast (std::vector<CloudPtr, Eigen::aligned_allocator<CloudPtr> > & clusters);
00109 
00110         /* \brief Sets the input point cloud.
00111          * \param cloud_in The input point cloud.
00112          */
00113         void
00114         setInputCloud (CloudPtr & cloud_in)
00115         {
00116           input_ = cloud_in;
00117         }
00118 
00119         /* \brief Returns the table coefficients after computation
00120          * \param model represents the normal and the position of the plane (a,b,c,d)
00121          */
00122         void
00123         getTableCoefficients (Eigen::Vector4f & model)
00124         {
00125           model = table_coeffs_;
00126         }
00127 
00128         /* \brief Sets minimum distance between clusters
00129          * \param d distance (in meters)
00130          */
00131         void
00132         setDistanceBetweenClusters(double d) {
00133           object_cluster_tolerance_ = d;
00134         }
00135 
00136         /* \brief Sets minimum size of the clusters.
00137          * \param size number of points
00138          */
00139         void setMinClusterSize(int size) {
00140           object_cluster_min_size_ = size;
00141         }
00142 
00143         /* \brief Sets the min height of the clusters in order to be considered.
00144          * \param h minimum height (in meters)
00145          */
00146         void
00147         setObjectMinHeight (double h)
00148         {
00149           object_min_height_ = h;
00150         }
00151 
00152         /* \brief Sets the max height of the clusters in order to be considered.
00153          * \param h max height (in meters)
00154          */
00155         void
00156         setObjectMaxHeight (double h)
00157         {
00158           object_max_height_ = h;
00159         }
00160 
00161         /* \brief Sets minimum distance from the camera for a point to be considered.
00162          * \param z distance (in meters)
00163          */
00164         void
00165         setMinZBounds (double z)
00166         {
00167           min_z_bounds_ = z;
00168         }
00169         /* \brief Sets maximum distance from the camera for a point to be considered.
00170          * \param z distance (in meters)
00171          */
00172         void
00173         setMaxZBounds (double z)
00174         {
00175           max_z_bounds_ = z;
00176         }
00177 
00178         /* \brief Sets the number of neighbors used for normal estimation.
00179          * \param k number of neighbors
00180          */
00181         void setKNeighbors(int k) {
00182           k_ = k;
00183         }
00184 
00185         /* \brief Set threshold for SAC plane segmentation
00186          * \param d threshold (in meters)
00187          */
00188         void setSACThreshold(double d) {
00189           sac_distance_threshold_ = d;
00190         }
00191 
00192         /* \brief Set downsampling resolution.
00193          * \param d resolution (in meters)
00194          */
00195         void setDownsamplingSize(double d) {
00196           downsample_leaf_ = d;
00197         }
00198 
00199         /* \brief Set window size in pixels for CC used in compute_fast method
00200          * \param w window size (in pixels)
00201          */
00202         void setWSize(int w) {
00203          wsize_ = w;
00204         }
00205 
00206       private:
00207 
00208         int
00209         check (pcl::PointXYZI & p1, pcl::PointXYZI & p2, float c_intensity, float max_dist)
00210         {
00211           if (p1.intensity == 0) //new label
00212             return 1;
00213           else
00214           {
00215             //compute distance and check aginst max_dist
00216             if ((p1.getVector3fMap () - p2.getVector3fMap ()).norm () <= max_dist)
00217             {
00218               p2.intensity = p1.intensity;
00219               return 0;
00220             }
00221             else //new label
00222               return 1;
00223           }
00224         }
00225 
00226         //components needed for cluster segmentation and plane extraction
00227         pcl::PassThrough<PointType> pass_;
00228         pcl::VoxelGrid<PointType> grid_;
00229         pcl::NormalEstimation<PointType, pcl::Normal> n3d_;
00230         pcl::SACSegmentationFromNormals<PointType, pcl::Normal> seg_;
00231         pcl::ProjectInliers<PointType> proj_;
00232         pcl::ProjectInliers<PointType> bb_cluster_proj_;
00233         pcl::ConvexHull<PointType> hull_;
00234         pcl::ExtractPolygonalPrismData<PointType> prism_;
00235         pcl::EuclideanClusterExtraction<PointType> cluster_;
00236 
00238         CloudPtr input_;
00240         Eigen::Vector4f table_coeffs_;
00242         double downsample_leaf_;
00244         int k_;
00246         double min_z_bounds_;
00248         double max_z_bounds_;
00250         double sac_distance_threshold_;
00252         double object_min_height_;
00254         double object_max_height_;
00256         double object_cluster_tolerance_;
00258         double object_cluster_min_size_;
00260         int wsize_;
00261       };
00262   }
00263 }
00264 
00265 #endif /* DOMINANT_PLANE_SEGMENTATION_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines