Point Cloud Library (PCL)  1.7.1
sac_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_SAC_SEGMENTATION_H_
41 #define PCL_SEGMENTATION_SAC_SEGMENTATION_H_
42 
43 #include <pcl/pcl_base.h>
44 #include <pcl/PointIndices.h>
45 #include <pcl/ModelCoefficients.h>
46 
47 // Sample Consensus methods
48 #include <pcl/sample_consensus/method_types.h>
49 #include <pcl/sample_consensus/sac.h>
50 // Sample Consensus models
51 #include <pcl/sample_consensus/model_types.h>
52 #include <pcl/sample_consensus/sac_model.h>
53 
54 #include <pcl/search/search.h>
55 
56 namespace pcl
57 {
58  /** \brief @b SACSegmentation represents the Nodelet segmentation class for
59  * Sample Consensus methods and models, in the sense that it just creates a
60  * Nodelet wrapper for generic-purpose SAC-based segmentation.
61  * \author Radu Bogdan Rusu
62  * \ingroup segmentation
63  */
64  template <typename PointT>
65  class SACSegmentation : public PCLBase<PointT>
66  {
69 
70  public:
73 
75  typedef typename PointCloud::Ptr PointCloudPtr;
78 
81 
82  /** \brief Empty constructor.
83  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
84  */
85  SACSegmentation (bool random = false)
86  : model_ ()
87  , sac_ ()
88  , model_type_ (-1)
89  , method_type_ (0)
90  , threshold_ (0)
91  , optimize_coefficients_ (true)
92  , radius_min_ (-std::numeric_limits<double>::max ())
93  , radius_max_ (std::numeric_limits<double>::max ())
94  , samples_radius_ (0.0)
96  , eps_angle_ (0.0)
97  , axis_ (Eigen::Vector3f::Zero ())
98  , max_iterations_ (50)
99  , probability_ (0.99)
100  , random_ (random)
101  {
102  }
103 
104  /** \brief Empty destructor. */
105  virtual ~SACSegmentation () { /*srv_.reset ();*/ };
106 
107  /** \brief The type of model to use (user given parameter).
108  * \param[in] model the model type (check \a model_types.h)
109  */
110  inline void
111  setModelType (int model) { model_type_ = model; }
112 
113  /** \brief Get the type of SAC model used. */
114  inline int
115  getModelType () const { return (model_type_); }
116 
117  /** \brief Get a pointer to the SAC method used. */
118  inline SampleConsensusPtr
119  getMethod () const { return (sac_); }
120 
121  /** \brief Get a pointer to the SAC model used. */
123  getModel () const { return (model_); }
124 
125  /** \brief The type of sample consensus method to use (user given parameter).
126  * \param[in] method the method type (check \a method_types.h)
127  */
128  inline void
129  setMethodType (int method) { method_type_ = method; }
130 
131  /** \brief Get the type of sample consensus method used. */
132  inline int
133  getMethodType () const { return (method_type_); }
134 
135  /** \brief Distance to the model threshold (user given parameter).
136  * \param[in] threshold the distance threshold to use
137  */
138  inline void
139  setDistanceThreshold (double threshold) { threshold_ = threshold; }
140 
141  /** \brief Get the distance to the model threshold. */
142  inline double
143  getDistanceThreshold () const { return (threshold_); }
144 
145  /** \brief Set the maximum number of iterations before giving up.
146  * \param[in] max_iterations the maximum number of iterations the sample consensus method will run
147  */
148  inline void
149  setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
150 
151  /** \brief Get maximum number of iterations before giving up. */
152  inline int
153  getMaxIterations () const { return (max_iterations_); }
154 
155  /** \brief Set the probability of choosing at least one sample free from outliers.
156  * \param[in] probability the model fitting probability
157  */
158  inline void
159  setProbability (double probability) { probability_ = probability; }
160 
161  /** \brief Get the probability of choosing at least one sample free from outliers. */
162  inline double
163  getProbability () const { return (probability_); }
164 
165  /** \brief Set to true if a coefficient refinement is required.
166  * \param[in] optimize true for enabling model coefficient refinement, false otherwise
167  */
168  inline void
169  setOptimizeCoefficients (bool optimize) { optimize_coefficients_ = optimize; }
170 
171  /** \brief Get the coefficient refinement internal flag. */
172  inline bool
174 
175  /** \brief Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate
176  * a radius)
177  * \param[in] min_radius the minimum radius model
178  * \param[in] max_radius the maximum radius model
179  */
180  inline void
181  setRadiusLimits (const double &min_radius, const double &max_radius)
182  {
183  radius_min_ = min_radius;
184  radius_max_ = max_radius;
185  }
186 
187  /** \brief Get the minimum and maximum allowable radius limits for the model as set by the user.
188  * \param[out] min_radius the resultant minimum radius model
189  * \param[out] max_radius the resultant maximum radius model
190  */
191  inline void
192  getRadiusLimits (double &min_radius, double &max_radius)
193  {
194  min_radius = radius_min_;
195  max_radius = radius_max_;
196  }
197 
198  /** \brief Set the maximum distance allowed when drawing random samples
199  * \param[in] radius the maximum distance (L2 norm)
200  */
201  inline void
202  setSamplesMaxDist (const double &radius, SearchPtr search)
203  {
204  samples_radius_ = radius;
205  samples_radius_search_ = search;
206  }
207 
208  /** \brief Get maximum distance allowed when drawing random samples
209  *
210  * \param[out] radius the maximum distance (L2 norm)
211  */
212  inline void
213  getSamplesMaxDist (double &radius)
214  {
215  radius = samples_radius_;
216  }
217 
218  /** \brief Set the axis along which we need to search for a model perpendicular to.
219  * \param[in] ax the axis along which we need to search for a model perpendicular to
220  */
221  inline void
222  setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
223 
224  /** \brief Get the axis along which we need to search for a model perpendicular to. */
225  inline Eigen::Vector3f
226  getAxis () const { return (axis_); }
227 
228  /** \brief Set the angle epsilon (delta) threshold.
229  * \param[in] ea the maximum allowed difference between the model normal and the given axis in radians.
230  */
231  inline void
232  setEpsAngle (double ea) { eps_angle_ = ea; }
233 
234  /** \brief Get the epsilon (delta) model angle threshold in radians. */
235  inline double
236  getEpsAngle () const { return (eps_angle_); }
237 
238  /** \brief Base method for segmentation of a model in a PointCloud given by <setInputCloud (), setIndices ()>
239  * \param[in] inliers the resultant point indices that support the model found (inliers)
240  * \param[out] model_coefficients the resultant model coefficients
241  */
242  virtual void
243  segment (PointIndices &inliers, ModelCoefficients &model_coefficients);
244 
245  protected:
246  /** \brief Initialize the Sample Consensus model and set its parameters.
247  * \param[in] model_type the type of SAC model that is to be used
248  */
249  virtual bool
250  initSACModel (const int model_type);
251 
252  /** \brief Initialize the Sample Consensus method and set its parameters.
253  * \param[in] method_type the type of SAC method to be used
254  */
255  virtual void
256  initSAC (const int method_type);
257 
258  /** \brief The model that needs to be segmented. */
260 
261  /** \brief The sample consensus segmentation method. */
263 
264  /** \brief The type of model to use (user given parameter). */
266 
267  /** \brief The type of sample consensus method to use (user given parameter). */
269 
270  /** \brief Distance to the model threshold (user given parameter). */
271  double threshold_;
272 
273  /** \brief Set to true if a coefficient refinement is required. */
275 
276  /** \brief The minimum and maximum radius limits for the model. Applicable to all models that estimate a radius. */
278 
279  /** \brief The maximum distance of subsequent samples from the first (radius search) */
281 
282  /** \brief The search object for picking subsequent samples using radius search */
284 
285  /** \brief The maximum allowed difference between the model normal and the given axis. */
286  double eps_angle_;
287 
288  /** \brief The axis along which we need to search for a model perpendicular to. */
289  Eigen::Vector3f axis_;
290 
291  /** \brief Maximum number of iterations before giving up (user given parameter). */
293 
294  /** \brief Desired probability of choosing at least one sample free from outliers (user given parameter). */
295  double probability_;
296 
297  /** \brief Set to true if we need a random seed. */
298  bool random_;
299 
300  /** \brief Class get name method. */
301  virtual std::string
302  getClassName () const { return ("SACSegmentation"); }
303  };
304 
305  /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and
306  * models that require the use of surface normals for estimation.
307  * \ingroup segmentation
308  */
309  template <typename PointT, typename PointNT>
311  {
319 
320  public:
323 
325  typedef typename PointCloud::Ptr PointCloudPtr;
327 
331 
335 
336  /** \brief Empty constructor.
337  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
338  */
339  SACSegmentationFromNormals (bool random = false)
340  : SACSegmentation<PointT> (random)
341  , normals_ ()
342  , distance_weight_ (0.1)
344  , min_angle_ ()
345  , max_angle_ ()
346  {};
347 
348  /** \brief Provide a pointer to the input dataset that contains the point normals of
349  * the XYZ dataset.
350  * \param[in] normals the const boost shared pointer to a PointCloud message
351  */
352  inline void
353  setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
354 
355  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
356  inline PointCloudNConstPtr
357  getInputNormals () const { return (normals_); }
358 
359  /** \brief Set the relative weight (between 0 and 1) to give to the angular
360  * distance (0 to pi/2) between point normals and the plane normal.
361  * \param[in] distance_weight the distance/angular weight
362  */
363  inline void
364  setNormalDistanceWeight (double distance_weight) { distance_weight_ = distance_weight; }
365 
366  /** \brief Get the relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point
367  * normals and the plane normal. */
368  inline double
370 
371  /** \brief Set the minimum opning angle for a cone model.
372  * \param oa the opening angle which we need minumum to validate a cone model.
373  */
374  inline void
375  setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
376  {
377  min_angle_ = min_angle;
378  max_angle_ = max_angle;
379  }
380 
381  /** \brief Get the opening angle which we need minumum to validate a cone model. */
382  inline void
383  getMinMaxOpeningAngle (double &min_angle, double &max_angle)
384  {
385  min_angle = min_angle_;
386  max_angle = max_angle_;
387  }
388 
389  /** \brief Set the distance we expect a plane model to be from the origin
390  * \param[in] d distance from the template plane modl to the origin
391  */
392  inline void
394 
395  /** \brief Get the distance of a plane model from the origin. */
396  inline double
398 
399  protected:
400  /** \brief A pointer to the input dataset that contains the point normals of the XYZ dataset. */
402 
403  /** \brief The relative weight (between 0 and 1) to give to the angular
404  * distance (0 to pi/2) between point normals and the plane normal.
405  */
407 
408  /** \brief The distance from the template plane to the origin. */
410 
411  /** \brief The minimum and maximum allowed opening angle of valid cone model. */
412  double min_angle_;
413  double max_angle_;
414 
415  /** \brief Initialize the Sample Consensus model and set its parameters.
416  * \param[in] model_type the type of SAC model that is to be used
417  */
418  virtual bool
419  initSACModel (const int model_type);
420 
421  /** \brief Class get name method. */
422  virtual std::string
423  getClassName () const { return ("SACSegmentationFromNormals"); }
424  };
425 }
426 
427 #ifdef PCL_NO_PRECOMPILE
428 #include <pcl/segmentation/impl/sac_segmentation.hpp>
429 #endif
430 
431 #endif //#ifndef PCL_SEGMENTATION_SAC_SEGMENTATION_H_