Point Cloud Library (PCL)  1.7.1
organized_multi_plane_segmentation.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
41 #define PCL_SEGMENTATION_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_
42 
43 #include <pcl/segmentation/planar_region.h>
44 #include <pcl/pcl_base.h>
45 #include <pcl/common/angles.h>
46 #include <pcl/PointIndices.h>
47 #include <pcl/ModelCoefficients.h>
48 #include <pcl/segmentation/plane_coefficient_comparator.h>
49 #include <pcl/segmentation/plane_refinement_comparator.h>
50 
51 namespace pcl
52 {
53  /** \brief OrganizedMultiPlaneSegmentation finds all planes present in the
54  * input cloud, and outputs a vector of plane equations, as well as a vector
55  * of point clouds corresponding to the inliers of each detected plane. Only
56  * planes with more than min_inliers points are detected.
57  * Templated on point type, normal type, and label type
58  *
59  * \author Alex Trevor, Suat Gedikli
60  */
61  template<typename PointT, typename PointNT, typename PointLT>
63  {
68 
69  public:
71  typedef typename PointCloud::Ptr PointCloudPtr;
73 
75  typedef typename PointCloudN::Ptr PointCloudNPtr;
77 
79  typedef typename PointCloudL::Ptr PointCloudLPtr;
81 
85 
89 
90  /** \brief Constructor for OrganizedMultiPlaneSegmentation. */
92  normals_ (),
93  min_inliers_ (1000),
94  angular_threshold_ (pcl::deg2rad (3.0)),
95  distance_threshold_ (0.02),
96  maximum_curvature_ (0.001),
97  project_points_ (false),
99  {
100  }
101 
102  /** \brief Destructor for OrganizedMultiPlaneSegmentation. */
103  virtual
105  {
106  }
107 
108  /** \brief Provide a pointer to the input normals.
109  * \param[in] normals the input normal cloud
110  */
111  inline void
113  {
114  normals_ = normals;
115  }
116 
117  /** \brief Get the input normals. */
118  inline PointCloudNConstPtr
120  {
121  return (normals_);
122  }
123 
124  /** \brief Set the minimum number of inliers required for a plane
125  * \param[in] min_inliers the minimum number of inliers required per plane
126  */
127  inline void
128  setMinInliers (unsigned min_inliers)
129  {
130  min_inliers_ = min_inliers;
131  }
132 
133  /** \brief Get the minimum number of inliers required per plane. */
134  inline unsigned
135  getMinInliers () const
136  {
137  return (min_inliers_);
138  }
139 
140  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
141  * \param[in] angular_threshold the tolerance in radians
142  */
143  inline void
144  setAngularThreshold (double angular_threshold)
145  {
146  angular_threshold_ = angular_threshold;
147  }
148 
149  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
150  inline double
152  {
153  return (angular_threshold_);
154  }
155 
156  /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
157  * \param[in] distance_threshold the tolerance in meters
158  */
159  inline void
160  setDistanceThreshold (double distance_threshold)
161  {
162  distance_threshold_ = distance_threshold;
163  }
164 
165  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
166  inline double
168  {
169  return (distance_threshold_);
170  }
171 
172  /** \brief Set the maximum curvature allowed for a planar region.
173  * \param[in] maximum_curvature the maximum curvature
174  */
175  inline void
176  setMaximumCurvature (double maximum_curvature)
177  {
178  maximum_curvature_ = maximum_curvature;
179  }
180 
181  /** \brief Get the maximum curvature allowed for a planar region. */
182  inline double
184  {
185  return (maximum_curvature_);
186  }
187 
188  /** \brief Provide a pointer to the comparator to be used for segmentation.
189  * \param[in] compare A pointer to the comparator to be used for segmentation.
190  */
191  void
193  {
194  compare_ = compare;
195  }
196 
197  /** \brief Provide a pointer to the comparator to be used for refinement.
198  * \param[in] compare A pointer to the comparator to be used for refinement.
199  */
200  void
202  {
203  refinement_compare_ = compare;
204  }
205 
206  /** \brief Set whether or not to project boundary points to the plane, or leave them in the original 3D space.
207  * \param[in] project_points true if points should be projected, false if not.
208  */
209  void
210  setProjectPoints (bool project_points)
211  {
212  project_points_ = project_points;
213  }
214 
215  /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
216  * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud
217  * \param[out] inlier_indices a vector of inliers for each detected plane
218  * \param[out] centroids a vector of centroids for each plane
219  * \param[out] covariances a vector of covariance matricies for the inliers of each plane
220  * \param[out] labels a point cloud for the connected component labels of each pixel
221  * \param[out] label_indices a vector of PointIndices for each labeled component
222  */
223  void
224  segment (std::vector<ModelCoefficients>& model_coefficients,
225  std::vector<PointIndices>& inlier_indices,
226  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
227  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
228  pcl::PointCloud<PointLT>& labels,
229  std::vector<pcl::PointIndices>& label_indices);
230 
231  /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
232  * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud
233  * \param[out] inlier_indices a vector of inliers for each detected plane
234  */
235  void
236  segment (std::vector<ModelCoefficients>& model_coefficients,
237  std::vector<PointIndices>& inlier_indices);
238 
239  /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
240  * \param[out] regions a list of resultant planar polygonal regions
241  */
242  void
243  segment (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions);
244 
245  /** \brief Perform a segmentation, as well as an additional refinement step. This helps with including points whose normals may not match neighboring points well, but may match the planar model well.
246  * \param[out] regions A list of regions generated by segmentation and refinement.
247  */
248  void
249  segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions);
250 
251  /** \brief Perform a segmentation, as well as additional refinement step. Returns intermediate data structures for use in
252  * subsequent processing.
253  * \param[out] regions A vector of PlanarRegions generated by segmentation
254  * \param[out] model_coefficients A vector of model coefficients for each segmented plane
255  * \param[out] inlier_indices A vector of PointIndices, indicating the inliers to each segmented plane
256  * \param[out] labels A PointCloud<PointLT> corresponding to the resulting segmentation.
257  * \param[out] label_indices A vector of PointIndices for each label
258  * \param[out] boundary_indices A vector of PointIndices corresponding to the outer boundary / contour of each label
259  */
260  void
261  segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions,
262  std::vector<ModelCoefficients>& model_coefficients,
263  std::vector<PointIndices>& inlier_indices,
264  PointCloudLPtr& labels,
265  std::vector<pcl::PointIndices>& label_indices,
266  std::vector<pcl::PointIndices>& boundary_indices);
267 
268  /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation.
269  * \param [in] model_coefficients The list of segmented model coefficients
270  * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model
271  * \param [in] centroids The list of centroids corresponding to each segmented plane
272  * \param [in] covariances The list of covariances corresponding to each segemented plane
273  * \param [in] labels The labels produced by the initial segmentation
274  * \param [in] label_indices The list of indices corresponding to each label
275  */
276  void
277  refine (std::vector<ModelCoefficients>& model_coefficients,
278  std::vector<PointIndices>& inlier_indices,
279  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
280  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
281  PointCloudLPtr& labels,
282  std::vector<pcl::PointIndices>& label_indices);
283 
284  protected:
285 
286  /** \brief A pointer to the input normals */
288 
289  /** \brief The minimum number of inliers required for each plane. */
290  unsigned min_inliers_;
291 
292  /** \brief The tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
294 
295  /** \brief The tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane. */
297 
298  /** \brief The tolerance for maximum curvature after fitting a plane. Used to remove smooth, but non-planar regions. */
300 
301  /** \brief Whether or not points should be projected to the plane, or left in the original 3D space. */
303 
304  /** \brief A comparator for comparing neighboring pixels' plane equations. */
306 
307  /** \brief A comparator for use on the refinement step. Compares points to regions segmented in the first pass. */
309 
310  /** \brief Class getName method. */
311  virtual std::string
312  getClassName () const
313  {
314  return ("OrganizedMultiPlaneSegmentation");
315  }
316  };
317 
318 }
319 
320 #ifdef PCL_NO_PRECOMPILE
321 #include <pcl/segmentation/impl/organized_multi_plane_segmentation.hpp>
322 #endif
323 
324 #endif //#ifndef PCL_SEGMENTATION_ORGANIZED_MULTI_PLANE_SEGMENTATION_H_