Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
41 
42 #include <pcl/segmentation/planar_region.h>
43 #include <pcl/pcl_base.h>
44 #include <pcl/common/angles.h>
45 #include <pcl/PointIndices.h>
46 #include <pcl/ModelCoefficients.h>
47 #include <pcl/segmentation/plane_coefficient_comparator.h>
48 #include <pcl/segmentation/plane_refinement_comparator.h>
49 
50 namespace pcl
51 {
52  /** \brief OrganizedMultiPlaneSegmentation finds all planes present in the
53  * input cloud, and outputs a vector of plane equations, as well as a vector
54  * of point clouds corresponding to the inliers of each detected plane. Only
55  * planes with more than min_inliers points are detected.
56  * Templated on point type, normal type, and label type
57  *
58  * \author Alex Trevor, Suat Gedikli
59  */
60  template<typename PointT, typename PointNT, typename PointLT>
62  {
67 
68  public:
70  using PointCloudPtr = typename PointCloud::Ptr;
72 
74  using PointCloudNPtr = typename PointCloudN::Ptr;
76 
78  using PointCloudLPtr = typename PointCloudL::Ptr;
80 
84 
88 
89  /** \brief Constructor for OrganizedMultiPlaneSegmentation. */
91  normals_ (),
92  min_inliers_ (1000),
93  angular_threshold_ (pcl::deg2rad (3.0)),
94  distance_threshold_ (0.02),
95  maximum_curvature_ (0.001),
96  project_points_ (false),
98  {
99  }
100 
101  /** \brief Destructor for OrganizedMultiPlaneSegmentation. */
102 
104  {
105  }
106 
107  /** \brief Provide a pointer to the input normals.
108  * \param[in] normals the input normal cloud
109  */
110  inline void
112  {
113  normals_ = normals;
114  }
115 
116  /** \brief Get the input normals. */
117  inline PointCloudNConstPtr
119  {
120  return (normals_);
121  }
122 
123  /** \brief Set the minimum number of inliers required for a plane
124  * \param[in] min_inliers the minimum number of inliers required per plane
125  */
126  inline void
127  setMinInliers (unsigned min_inliers)
128  {
129  min_inliers_ = min_inliers;
130  }
131 
132  /** \brief Get the minimum number of inliers required per plane. */
133  inline unsigned
134  getMinInliers () const
135  {
136  return (min_inliers_);
137  }
138 
139  /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
140  * \param[in] angular_threshold the tolerance in radians
141  */
142  inline void
143  setAngularThreshold (double angular_threshold)
144  {
145  angular_threshold_ = angular_threshold;
146  }
147 
148  /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
149  inline double
151  {
152  return (angular_threshold_);
153  }
154 
155  /** \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.
156  * \param[in] distance_threshold the tolerance in meters
157  */
158  inline void
159  setDistanceThreshold (double distance_threshold)
160  {
161  distance_threshold_ = distance_threshold;
162  }
163 
164  /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
165  inline double
167  {
168  return (distance_threshold_);
169  }
170 
171  /** \brief Set the maximum curvature allowed for a planar region.
172  * \param[in] maximum_curvature the maximum curvature
173  */
174  inline void
175  setMaximumCurvature (double maximum_curvature)
176  {
177  maximum_curvature_ = maximum_curvature;
178  }
179 
180  /** \brief Get the maximum curvature allowed for a planar region. */
181  inline double
183  {
184  return (maximum_curvature_);
185  }
186 
187  /** \brief Provide a pointer to the comparator to be used for segmentation.
188  * \param[in] compare A pointer to the comparator to be used for segmentation.
189  */
190  void
192  {
193  compare_ = compare;
194  }
195 
196  /** \brief Provide a pointer to the comparator to be used for refinement.
197  * \param[in] compare A pointer to the comparator to be used for refinement.
198  */
199  void
201  {
202  refinement_compare_ = compare;
203  }
204 
205  /** \brief Set whether or not to project boundary points to the plane, or leave them in the original 3D space.
206  * \param[in] project_points true if points should be projected, false if not.
207  */
208  void
209  setProjectPoints (bool project_points)
210  {
211  project_points_ = project_points;
212  }
213 
214  /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
215  * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud
216  * \param[out] inlier_indices a vector of inliers for each detected plane
217  * \param[out] centroids a vector of centroids for each plane
218  * \param[out] covariances a vector of covariance matricies for the inliers of each plane
219  * \param[out] labels a point cloud for the connected component labels of each pixel
220  * \param[out] label_indices a vector of PointIndices for each labeled component
221  */
222  void
223  segment (std::vector<ModelCoefficients>& model_coefficients,
224  std::vector<PointIndices>& inlier_indices,
225  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
226  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
227  pcl::PointCloud<PointLT>& labels,
228  std::vector<pcl::PointIndices>& label_indices);
229 
230  /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
231  * \param[out] model_coefficients a vector of model_coefficients for each plane found in the input cloud
232  * \param[out] inlier_indices a vector of inliers for each detected plane
233  */
234  void
235  segment (std::vector<ModelCoefficients>& model_coefficients,
236  std::vector<PointIndices>& inlier_indices);
237 
238  /** \brief Segmentation of all planes in a point cloud given by setInputCloud(), setIndices()
239  * \param[out] regions a list of resultant planar polygonal regions
240  */
241  void
242  segment (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions);
243 
244  /** \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.
245  * \param[out] regions A list of regions generated by segmentation and refinement.
246  */
247  void
248  segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions);
249 
250  /** \brief Perform a segmentation, as well as additional refinement step. Returns intermediate data structures for use in
251  * subsequent processing.
252  * \param[out] regions A vector of PlanarRegions generated by segmentation
253  * \param[out] model_coefficients A vector of model coefficients for each segmented plane
254  * \param[out] inlier_indices A vector of PointIndices, indicating the inliers to each segmented plane
255  * \param[out] labels A PointCloud<PointLT> corresponding to the resulting segmentation.
256  * \param[out] label_indices A vector of PointIndices for each label
257  * \param[out] boundary_indices A vector of PointIndices corresponding to the outer boundary / contour of each label
258  */
259  void
260  segmentAndRefine (std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > >& regions,
261  std::vector<ModelCoefficients>& model_coefficients,
262  std::vector<PointIndices>& inlier_indices,
263  PointCloudLPtr& labels,
264  std::vector<pcl::PointIndices>& label_indices,
265  std::vector<pcl::PointIndices>& boundary_indices);
266 
267  /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation.
268  * \param [in] model_coefficients The list of segmented model coefficients
269  * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model
270  * \param [in] labels The labels produced by the initial segmentation
271  * \param [in] label_indices The list of indices corresponding to each label
272  */
273  void
274  refine (std::vector<ModelCoefficients>& model_coefficients,
275  std::vector<PointIndices>& inlier_indices,
276  PointCloudLPtr& labels,
277  std::vector<pcl::PointIndices>& label_indices);
278 
279  /** \brief Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by the initial segmentation.
280  * \param [in] model_coefficients The list of segmented model coefficients
281  * \param [in] inlier_indices The list of segmented inlier indices, corresponding to each model
282  * \param [in] centroids The list of centroids corresponding to each segmented plane
283  * \param [in] covariances The list of covariances corresponding to each segemented plane
284  * \param [in] labels The labels produced by the initial segmentation
285  * \param [in] label_indices The list of indices corresponding to each label
286  */
287  [[deprecated("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases")]]
288  void
289  refine (std::vector<ModelCoefficients>& model_coefficients,
290  std::vector<PointIndices>& inlier_indices,
291  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& /*centroids*/,
292  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& /*covariances*/,
293  PointCloudLPtr& labels,
294  std::vector<pcl::PointIndices>& label_indices)
295  {
296  refine(model_coefficients, inlier_indices, labels, label_indices);
297  }
298 
299  protected:
300 
301  /** \brief A pointer to the input normals */
303 
304  /** \brief The minimum number of inliers required for each plane. */
305  unsigned min_inliers_;
306 
307  /** \brief The tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
309 
310  /** \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. */
312 
313  /** \brief The tolerance for maximum curvature after fitting a plane. Used to remove smooth, but non-planar regions. */
315 
316  /** \brief Whether or not points should be projected to the plane, or left in the original 3D space. */
318 
319  /** \brief A comparator for comparing neighboring pixels' plane equations. */
321 
322  /** \brief A comparator for use on the refinement step. Compares points to regions segmented in the first pass. */
324 
325  /** \brief Class getName method. */
326  virtual std::string
327  getClassName () const
328  {
329  return ("OrganizedMultiPlaneSegmentation");
330  }
331  };
332 
333 }
334 
335 #ifdef PCL_NO_PRECOMPILE
336 #include <pcl/segmentation/impl/organized_multi_plane_segmentation.hpp>
337 #endif
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
void segment(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &centroids, std::vector< Eigen::Matrix3f, Eigen::aligned_allocator< Eigen::Matrix3f > > &covariances, pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices)
Segmentation of all planes in a point cloud given by setInputCloud(), setIndices() ...
void setMaximumCurvature(double maximum_curvature)
Set the maximum curvature allowed for a planar region.
void setMinInliers(unsigned min_inliers)
Set the minimum number of inliers required for a plane.
void segmentAndRefine(std::vector< PlanarRegion< PointT >, Eigen::aligned_allocator< PlanarRegion< PointT > > > &regions)
Perform a segmentation, as well as an additional refinement step.
PlaneRefinementComparatorPtr refinement_compare_
A comparator for use on the refinement step.
PointCloudNConstPtr getInputNormals() const
Get the input normals.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< const PlaneCoefficientComparator< PointT, PointNT > > ConstPtr
unsigned getMinInliers() const
Get the minimum number of inliers required per plane.
PlaneComparatorPtr compare_
A comparator for comparing neighboring pixels&#39; plane equations.
void refine(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > &, std::vector< Eigen::Matrix3f, Eigen::aligned_allocator< Eigen::Matrix3f > > &, PointCloudLPtr &labels, std::vector< pcl::PointIndices > &label_indices)
Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by ...
boost::shared_ptr< PlaneRefinementComparator< PointT, PointNT, PointLT > > Ptr
OrganizedMultiPlaneSegmentation()
Constructor for OrganizedMultiPlaneSegmentation.
void refine(std::vector< ModelCoefficients > &model_coefficients, std::vector< PointIndices > &inlier_indices, PointCloudLPtr &labels, std::vector< pcl::PointIndices > &label_indices)
Perform a refinement of an initial segmentation, by comparing points to adjacent regions detected by ...
virtual std::string getClassName() const
Class getName method.
void setRefinementComparator(const PlaneRefinementComparatorPtr &compare)
Provide a pointer to the comparator to be used for refinement.
PCL base class.
Definition: pcl_base.h:69
void setDistanceThreshold(double distance_threshold)
Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) ...
double distance_threshold_
The tolerance in meters for difference in perpendicular distance (d component of plane equation) to t...
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
PointCloudNConstPtr normals_
A pointer to the input normals.
void setAngularThreshold(double angular_threshold)
Set the tolerance in radians for difference in normal direction between neighboring points...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:444
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
unsigned min_inliers_
The minimum number of inliers required for each plane.
OrganizedMultiPlaneSegmentation finds all planes present in the input cloud, and outputs a vector of ...
double angular_threshold_
The tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
double maximum_curvature_
The tolerance for maximum curvature after fitting a plane.
PlaneRefinementComparator is a Comparator that operates on plane coefficients, for use in planar segm...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:445
double getDistanceThreshold() const
Get the distance threshold in meters (d component of plane equation) between neighboring points...
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input normals.
double getAngularThreshold() const
Get the angular threshold in radians for difference in normal direction between neighboring points...
Define standard C methods to do angle calculations.
void setComparator(const PlaneComparatorPtr &compare)
Provide a pointer to the comparator to be used for segmentation.
~OrganizedMultiPlaneSegmentation()
Destructor for OrganizedMultiPlaneSegmentation.
PlanarRegion represents a set of points that lie in a plane.
Definition: planar_region.h:50
boost::shared_ptr< PlaneCoefficientComparator< PointT, PointNT > > Ptr
double getMaximumCurvature() const
Get the maximum curvature allowed for a planar region.
boost::shared_ptr< const PlaneRefinementComparator< PointT, PointNT, PointLT > > ConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
void setProjectPoints(bool project_points)
Set whether or not to project boundary points to the plane, or leave them in the original 3D space...
bool project_points_
Whether or not points should be projected to the plane, or left in the original 3D space...