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 PCL_EXPORTS 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.05f;
00081           object_cluster_min_size_ = 500;
00082           k_ = 50;
00083           sac_distance_threshold_ = 0.01;
00084           downsample_leaf_ = 0.005f;
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> & 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> & 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> & clusters);
00109 
00110         /* \brief Computes the table plane.
00111          */
00112         void
00113         compute_table_plane();
00114 
00115         /* \brief Sets the input point cloud.
00116          * \param cloud_in The input point cloud.
00117          */
00118         void
00119         setInputCloud (CloudPtr & cloud_in)
00120         {
00121           input_ = cloud_in;
00122         }
00123 
00124         /* \brief Returns the table coefficients after computation
00125          * \param model represents the normal and the position of the plane (a,b,c,d)
00126          */
00127         void
00128         getTableCoefficients (Eigen::Vector4f & model)
00129         {
00130           model = table_coeffs_;
00131         }
00132 
00133         /* \brief Sets minimum distance between clusters
00134          * \param d distance (in meters)
00135          */
00136         void
00137         setDistanceBetweenClusters (float d) 
00138         {
00139           object_cluster_tolerance_ = d;
00140         }
00141 
00142         /* \brief Sets minimum size of the clusters.
00143          * \param size number of points
00144          */
00145         void 
00146         setMinClusterSize (int size) 
00147         {
00148           object_cluster_min_size_ = size;
00149         }
00150 
00151         /* \brief Sets the min height of the clusters in order to be considered.
00152          * \param h minimum height (in meters)
00153          */
00154         void
00155         setObjectMinHeight (double h)
00156         {
00157           object_min_height_ = h;
00158         }
00159 
00160         /* \brief Sets the max height of the clusters in order to be considered.
00161          * \param h max height (in meters)
00162          */
00163         void
00164         setObjectMaxHeight (double h)
00165         {
00166           object_max_height_ = h;
00167         }
00168 
00169         /* \brief Sets minimum distance from the camera for a point to be considered.
00170          * \param z distance (in meters)
00171          */
00172         void
00173         setMinZBounds (double z)
00174         {
00175           min_z_bounds_ = z;
00176         }
00177         /* \brief Sets maximum distance from the camera for a point to be considered.
00178          * \param z distance (in meters)
00179          */
00180         void
00181         setMaxZBounds (double z)
00182         {
00183           max_z_bounds_ = z;
00184         }
00185 
00186         /* \brief Sets the number of neighbors used for normal estimation.
00187          * \param k number of neighbors
00188          */
00189         void setKNeighbors(int k) {
00190           k_ = k;
00191         }
00192 
00193         /* \brief Set threshold for SAC plane segmentation
00194          * \param d threshold (in meters)
00195          */
00196         void setSACThreshold(double d) {
00197           sac_distance_threshold_ = d;
00198         }
00199 
00200         /* \brief Set downsampling resolution.
00201          * \param d resolution (in meters)
00202          */
00203         void 
00204         setDownsamplingSize (float d) 
00205         {
00206           downsample_leaf_ = d;
00207         }
00208 
00209         /* \brief Set window size in pixels for CC used in compute_fast method
00210          * \param w window size (in pixels)
00211          */
00212         void setWSize(int w) {
00213          wsize_ = w;
00214         }
00215 
00216         /* \brief Returns the indices of the clusters found by the segmentation
00217          * NOTE: This function returns only valid indices if the compute_fast method is used
00218          * \param indices indices of the clusters
00219          */
00220         void getIndicesClusters(std::vector<pcl::PointIndices> & indices) {
00221           indices = indices_clusters_;
00222         }
00223 
00224       private:
00225 
00226         int
00227         check (pcl::PointXYZI & p1, pcl::PointXYZI & p2, float, float max_dist)
00228         {
00229           if (p1.intensity == 0) //new label
00230             return 1;
00231           else
00232           {
00233             //compute distance and check aginst max_dist
00234             if ((p1.getVector3fMap () - p2.getVector3fMap ()).norm () <= max_dist)
00235             {
00236               p2.intensity = p1.intensity;
00237               return 0;
00238             }
00239             else //new label
00240               return 1;
00241           }
00242         }
00243 
00244         //components needed for cluster segmentation and plane extraction
00245         pcl::PassThrough<PointType> pass_;
00246         pcl::VoxelGrid<PointType> grid_;
00247         pcl::NormalEstimation<PointType, pcl::Normal> n3d_;
00248         pcl::SACSegmentationFromNormals<PointType, pcl::Normal> seg_;
00249         pcl::ProjectInliers<PointType> proj_;
00250         pcl::ProjectInliers<PointType> bb_cluster_proj_;
00251         pcl::ConvexHull<PointType> hull_;
00252         pcl::ExtractPolygonalPrismData<PointType> prism_;
00253         pcl::EuclideanClusterExtraction<PointType> cluster_;
00254 
00256         CloudPtr input_;
00258         Eigen::Vector4f table_coeffs_;
00260         float downsample_leaf_;
00262         int k_;
00264         double min_z_bounds_;
00266         double max_z_bounds_;
00268         double sac_distance_threshold_;
00270         double object_min_height_;
00272         double object_max_height_;
00274         float object_cluster_tolerance_;
00276         int object_cluster_min_size_;
00278         int wsize_;
00280         std::vector<pcl::PointIndices> indices_clusters_;
00281 
00282       };
00283   }
00284 }
00285 
00286 #endif /* DOMINANT_PLANE_SEGMENTATION_H_ */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:48