Point Cloud Library (PCL)
1.3.1
|
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_ */