Point Cloud Library (PCL)  1.10.0
sac_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
42 #define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
43 
44 #include <pcl/segmentation/sac_segmentation.h>
45 
46 // Sample Consensus methods
47 #include <pcl/sample_consensus/sac.h>
48 #include <pcl/sample_consensus/lmeds.h>
49 #include <pcl/sample_consensus/mlesac.h>
50 #include <pcl/sample_consensus/msac.h>
51 #include <pcl/sample_consensus/ransac.h>
52 #include <pcl/sample_consensus/rmsac.h>
53 #include <pcl/sample_consensus/rransac.h>
54 #include <pcl/sample_consensus/prosac.h>
55 
56 // Sample Consensus models
57 #include <pcl/sample_consensus/sac_model.h>
58 #include <pcl/sample_consensus/sac_model_circle.h>
59 #include <pcl/sample_consensus/sac_model_circle3d.h>
60 #include <pcl/sample_consensus/sac_model_cone.h>
61 #include <pcl/sample_consensus/sac_model_cylinder.h>
62 #include <pcl/sample_consensus/sac_model_line.h>
63 #include <pcl/sample_consensus/sac_model_normal_plane.h>
64 #include <pcl/sample_consensus/sac_model_parallel_plane.h>
65 #include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
66 #include <pcl/sample_consensus/sac_model_parallel_line.h>
67 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
68 #include <pcl/sample_consensus/sac_model_plane.h>
69 #include <pcl/sample_consensus/sac_model_sphere.h>
70 #include <pcl/sample_consensus/sac_model_normal_sphere.h>
71 #include <pcl/sample_consensus/sac_model_stick.h>
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////
74 template <typename PointT> void
76 {
77  // Copy the header information
78  inliers.header = model_coefficients.header = input_->header;
79 
80  if (!initCompute ())
81  {
82  inliers.indices.clear (); model_coefficients.values.clear ();
83  return;
84  }
85 
86  // Initialize the Sample Consensus model and set its parameters
87  if (!initSACModel (model_type_))
88  {
89  PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
90  deinitCompute ();
91  inliers.indices.clear (); model_coefficients.values.clear ();
92  return;
93  }
94  // Initialize the Sample Consensus method and set its parameters
95  initSAC (method_type_);
96 
97  if (!sac_->computeModel (0))
98  {
99  PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
100  deinitCompute ();
101  inliers.indices.clear (); model_coefficients.values.clear ();
102  return;
103  }
104 
105  // Get the model inliers
106  sac_->getInliers (inliers.indices);
107 
108  // Get the model coefficients
109  Eigen::VectorXf coeff;
110  sac_->getModelCoefficients (coeff);
111 
112  // If the user needs optimized coefficients
113  if (optimize_coefficients_)
114  {
115  Eigen::VectorXf coeff_refined;
116  model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
117  model_coefficients.values.resize (coeff_refined.size ());
118  memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float));
119  // Refine inliers
120  model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices);
121  }
122  else
123  {
124  model_coefficients.values.resize (coeff.size ());
125  memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float));
126  }
127 
128  deinitCompute ();
129 }
130 
131 //////////////////////////////////////////////////////////////////////////////////////////////
132 template <typename PointT> bool
134 {
135  if (model_)
136  model_.reset ();
137 
138  // Build the model
139  switch (model_type)
140  {
141  case SACMODEL_PLANE:
142  {
143  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
144  model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_, random_));
145  break;
146  }
147  case SACMODEL_LINE:
148  {
149  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
150  model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_, random_));
151  break;
152  }
153  case SACMODEL_STICK:
154  {
155  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
156  model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_));
157  double min_radius, max_radius;
158  model_->getRadiusLimits (min_radius, max_radius);
159  if (radius_min_ != min_radius && radius_max_ != max_radius)
160  {
161  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
162  model_->setRadiusLimits (radius_min_, radius_max_);
163  }
164  break;
165  }
166  case SACMODEL_CIRCLE2D:
167  {
168  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
169  model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_, random_));
170  typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
171  double min_radius, max_radius;
172  model_circle->getRadiusLimits (min_radius, max_radius);
173  if (radius_min_ != min_radius && radius_max_ != max_radius)
174  {
175  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
176  model_circle->setRadiusLimits (radius_min_, radius_max_);
177  }
178  break;
179  }
180  case SACMODEL_CIRCLE3D:
181  {
182  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
183  model_.reset (new SampleConsensusModelCircle3D<PointT> (input_, *indices_));
184  typename SampleConsensusModelCircle3D<PointT>::Ptr model_circle3d = boost::static_pointer_cast<SampleConsensusModelCircle3D<PointT> > (model_);
185  double min_radius, max_radius;
186  model_circle3d->getRadiusLimits (min_radius, max_radius);
187  if (radius_min_ != min_radius && radius_max_ != max_radius)
188  {
189  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
190  model_circle3d->setRadiusLimits (radius_min_, radius_max_);
191  }
192  break;
193  }
194  case SACMODEL_SPHERE:
195  {
196  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
197  model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_, random_));
198  typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
199  double min_radius, max_radius;
200  model_sphere->getRadiusLimits (min_radius, max_radius);
201  if (radius_min_ != min_radius && radius_max_ != max_radius)
202  {
203  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
204  model_sphere->setRadiusLimits (radius_min_, radius_max_);
205  }
206  break;
207  }
209  {
210  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
211  model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_, random_));
212  typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
213  if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
214  {
215  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
216  model_parallel->setAxis (axis_);
217  }
218  if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
219  {
220  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
221  model_parallel->setEpsAngle (eps_angle_);
222  }
223  break;
224  }
226  {
227  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
228  model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_, random_));
229  typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
230  if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
231  {
232  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
233  model_perpendicular->setAxis (axis_);
234  }
235  if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
236  {
237  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
238  model_perpendicular->setEpsAngle (eps_angle_);
239  }
240  break;
241  }
243  {
244  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
245  model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_, random_));
246  typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
247  if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
248  {
249  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
250  model_parallel->setAxis (axis_);
251  }
252  if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
253  {
254  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
255  model_parallel->setEpsAngle (eps_angle_);
256  }
257  break;
258  }
259  default:
260  {
261  PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
262  return (false);
263  }
264  }
265  return (true);
266 }
267 
268 //////////////////////////////////////////////////////////////////////////////////////////////
269 template <typename PointT> void
271 {
272  if (sac_)
273  sac_.reset ();
274  // Build the sample consensus method
275  switch (method_type)
276  {
277  case SAC_RANSAC:
278  default:
279  {
280  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
281  sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
282  break;
283  }
284  case SAC_LMEDS:
285  {
286  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
287  sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_));
288  break;
289  }
290  case SAC_MSAC:
291  {
292  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
293  sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_));
294  break;
295  }
296  case SAC_RRANSAC:
297  {
298  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
299  sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_));
300  break;
301  }
302  case SAC_RMSAC:
303  {
304  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
305  sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_));
306  break;
307  }
308  case SAC_MLESAC:
309  {
310  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
311  sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_));
312  break;
313  }
314  case SAC_PROSAC:
315  {
316  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
317  sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_));
318  break;
319  }
320  }
321  // Set the Sample Consensus parameters if they are given/changed
322  if (sac_->getProbability () != probability_)
323  {
324  PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
325  sac_->setProbability (probability_);
326  }
327  if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
328  {
329  PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
330  sac_->setMaxIterations (max_iterations_);
331  }
332  if (samples_radius_ > 0.)
333  {
334  PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
335  // Set maximum distance for radius search during random sampling
336  model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
337  }
338  if (sac_->getNumberOfThreads () != threads_)
339  {
340  PCL_DEBUG ("[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_);
341  sac_->setNumberOfThreads (threads_);
342  }
343 }
344 
345 //////////////////////////////////////////////////////////////////////////////////////////////
346 template <typename PointT, typename PointNT> bool
348 {
349  if (!input_ || !normals_)
350  {
351  PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
352  return (false);
353  }
354  // Check if input is synced with the normals
355  if (input_->points.size () != normals_->points.size ())
356  {
357  PCL_ERROR ("[pcl::%s::initSACModel] The number of points in the input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ());
358  return (false);
359  }
360 
361  if (model_)
362  model_.reset ();
363 
364  // Build the model
365  switch (model_type)
366  {
367  case SACMODEL_CYLINDER:
368  {
369  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
370  model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_, random_));
371  typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
372 
373  // Set the input normals
374  model_cylinder->setInputNormals (normals_);
375  double min_radius, max_radius;
376  model_cylinder->getRadiusLimits (min_radius, max_radius);
377  if (radius_min_ != min_radius && radius_max_ != max_radius)
378  {
379  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
380  model_cylinder->setRadiusLimits (radius_min_, radius_max_);
381  }
382  if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
383  {
384  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
385  model_cylinder->setNormalDistanceWeight (distance_weight_);
386  }
387  if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
388  {
389  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
390  model_cylinder->setAxis (axis_);
391  }
392  if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
393  {
394  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
395  model_cylinder->setEpsAngle (eps_angle_);
396  }
397  break;
398  }
400  {
401  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
402  model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_, random_));
403  typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
404  // Set the input normals
405  model_normals->setInputNormals (normals_);
406  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
407  {
408  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
409  model_normals->setNormalDistanceWeight (distance_weight_);
410  }
411  break;
412  }
414  {
415  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
416  model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_, random_));
417  typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
418  // Set the input normals
419  model_normals->setInputNormals (normals_);
420  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
421  {
422  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
423  model_normals->setNormalDistanceWeight (distance_weight_);
424  }
425  if (distance_from_origin_ != model_normals->getDistanceFromOrigin ())
426  {
427  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
428  model_normals->setDistanceFromOrigin (distance_from_origin_);
429  }
430  if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
431  {
432  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
433  model_normals->setAxis (axis_);
434  }
435  if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
436  {
437  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
438  model_normals->setEpsAngle (eps_angle_);
439  }
440  break;
441  }
442  case SACMODEL_CONE:
443  {
444  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
445  model_.reset (new SampleConsensusModelCone<PointT, PointNT> (input_, *indices_, random_));
446  typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = boost::static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_);
447 
448  // Set the input normals
449  model_cone->setInputNormals (normals_);
450  double min_angle, max_angle;
451  model_cone->getMinMaxOpeningAngle(min_angle, max_angle);
452  if (min_angle_ != min_angle && max_angle_ != max_angle)
453  {
454  PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
455  model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_);
456  }
457 
458  if (distance_weight_ != model_cone->getNormalDistanceWeight ())
459  {
460  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
461  model_cone->setNormalDistanceWeight (distance_weight_);
462  }
463  if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_)
464  {
465  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
466  model_cone->setAxis (axis_);
467  }
468  if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_)
469  {
470  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
471  model_cone->setEpsAngle (eps_angle_);
472  }
473  break;
474  }
476  {
477  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
478  model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_, random_));
479  typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = boost::static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_);
480  // Set the input normals
481  model_normals_sphere->setInputNormals (normals_);
482  double min_radius, max_radius;
483  model_normals_sphere->getRadiusLimits (min_radius, max_radius);
484  if (radius_min_ != min_radius && radius_max_ != max_radius)
485  {
486  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
487  model_normals_sphere->setRadiusLimits (radius_min_, radius_max_);
488  }
489 
490  if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ())
491  {
492  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
493  model_normals_sphere->setNormalDistanceWeight (distance_weight_);
494  }
495  break;
496  }
497  // If nothing else, try SACSegmentation
498  default:
499  {
500  return (pcl::SACSegmentation<PointT>::initSACModel (model_type));
501  }
502  }
503 
504  return (true);
505 }
506 
507 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
508 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>;
509 
510 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
511 
pcl::SampleConsensusModelCone::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
Definition: sac_model_cone.h:164
pcl::SampleConsensusModelCylinder::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
Definition: sac_model_cylinder.h:159
pcl::SampleConsensusModelCircle2D::Ptr
shared_ptr< SampleConsensusModelCircle2D< PointT > > Ptr
Definition: sac_model_circle.h:73
pcl::SACMODEL_PERPENDICULAR_PLANE
@ SACMODEL_PERPENDICULAR_PLANE
Definition: model_types.h:56
pcl::SampleConsensusModelCylinder
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
Definition: sac_model_cylinder.h:64
pcl::SampleConsensusModelSphere::Ptr
shared_ptr< SampleConsensusModelSphere< PointT > > Ptr
Definition: sac_model_sphere.h:73
pcl::SampleConsensusModelCone::getMinMaxOpeningAngle
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
Definition: sac_model_cone.h:187
pcl::SampleConsensusModelCone::setEpsAngle
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_cone.h:154
pcl::RandomSampleConsensus
RandomSampleConsensus represents an implementation of the RANSAC (RANdom SAmple Consensus) algorithm,...
Definition: ransac.h:56
pcl::SampleConsensusModelPerpendicularPlane::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_perpendicular_plane.h:126
pcl::SampleConsensusModelPlane
SampleConsensusModelPlane defines a model for 3D plane segmentation.
Definition: sac_model_plane.h:135
pcl::SampleConsensusModelCylinder::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_cylinder.h:149
pcl::SACMODEL_CIRCLE3D
@ SACMODEL_CIRCLE3D
Definition: model_types.h:50
pcl::SampleConsensusModelFromNormals::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: sac_model.h:616
pcl::SampleConsensusModelPerpendicularPlane::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_perpendicular_plane.h:133
pcl::SampleConsensusModelNormalParallelPlane::getDistanceFromOrigin
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
Definition: sac_model_normal_parallel_plane.h:175
pcl::SampleConsensusModelParallelLine::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a line.
Definition: sac_model_parallel_line.h:116
pcl::MEstimatorSampleConsensus
MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) alg...
Definition: msac.h:56
pcl::SampleConsensusModelFromNormals::getNormalDistanceWeight
double getNormalDistanceWeight() const
Get the normal angular distance weight.
Definition: sac_model.h:608
pcl::PointIndices::header
::pcl::PCLHeader header
Definition: PointIndices.h:17
pcl::SACMODEL_PARALLEL_PLANE
@ SACMODEL_PARALLEL_PLANE
Definition: model_types.h:62
pcl::SACSegmentation::initSACModel
virtual bool initSACModel(const int model_type)
Initialize the Sample Consensus model and set its parameters.
Definition: sac_segmentation.hpp:133
pcl::SampleConsensusModelParallelLine::Ptr
shared_ptr< SampleConsensusModelParallelLine< PointT > > Ptr
Definition: sac_model_parallel_line.h:74
pcl::SampleConsensusModel::getRadiusLimits
void getRadiusLimits(double &min_radius, double &max_radius) const
Get the minimum and maximum allowable radius limits for the model as set by the user.
Definition: sac_model.h:381
pcl::SampleConsensusModelNormalPlane
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
Definition: sac_model_normal_plane.h:76
pcl::ProgressiveSampleConsensus
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm,...
Definition: prosac.h:55
pcl::SampleConsensusModelCone::Ptr
shared_ptr< SampleConsensusModelCone< PointT, PointNT > > Ptr
Definition: sac_model_cone.h:80
pcl::ModelCoefficients::values
std::vector< float > values
Definition: ModelCoefficients.h:20
pcl::SampleConsensusModelParallelPlane::Ptr
shared_ptr< SampleConsensusModelParallelPlane< PointT > > Ptr
Definition: sac_model_parallel_plane.h:75
pcl::SampleConsensusModelCylinder::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cylinder direction.
Definition: sac_model_cylinder.h:163
pcl::SampleConsensusModelLine
SampleConsensusModelLine defines a model for 3D line segmentation.
Definition: sac_model_line.h:62
pcl::SampleConsensusModelPerpendicularPlane::Ptr
shared_ptr< SampleConsensusModelPerpendicularPlane< PointT > > Ptr
Definition: sac_model_perpendicular_plane.h:80
pcl::SACMODEL_NORMAL_PARALLEL_PLANE
@ SACMODEL_NORMAL_PARALLEL_PLANE
Definition: model_types.h:63
pcl::SampleConsensusModelNormalParallelPlane::Ptr
shared_ptr< SampleConsensusModelNormalParallelPlane< PointT, PointNT > > Ptr
Definition: sac_model_normal_parallel_plane.h:102
pcl::SACMODEL_NORMAL_PLANE
@ SACMODEL_NORMAL_PLANE
Definition: model_types.h:58
pcl::SampleConsensusModelParallelLine::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a line.
Definition: sac_model_parallel_line.h:120
pcl::SACSegmentationFromNormals::initSACModel
bool initSACModel(const int model_type) override
Initialize the Sample Consensus model and set its parameters.
Definition: sac_segmentation.hpp:347
pcl::SampleConsensusModelStick
SampleConsensusModelStick defines a model for 3D stick segmentation.
Definition: sac_model_stick.h:64
pcl::ModelCoefficients
Definition: ModelCoefficients.h:12
pcl::SampleConsensusModelNormalParallelPlane::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:165
pcl::SampleConsensusModelCircle3D::Ptr
shared_ptr< SampleConsensusModelCircle3D< PointT > > Ptr
Definition: sac_model_circle3d.h:73
pcl::SampleConsensusModelParallelPlane::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_parallel_plane.h:119
pcl::SACMODEL_LINE
@ SACMODEL_LINE
Definition: model_types.h:48
pcl::SampleConsensusModelParallelPlane::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_parallel_plane.h:123
pcl::SampleConsensusModelCone::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_cone.h:158
pcl::SampleConsensusModelPerpendicularPlane::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_perpendicular_plane.h:122
pcl::SampleConsensusModelFromNormals::setNormalDistanceWeight
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
Definition: sac_model.h:601
pcl::SACMODEL_CIRCLE2D
@ SACMODEL_CIRCLE2D
Definition: model_types.h:49
pcl::SampleConsensusModel::setRadiusLimits
void setRadiusLimits(const double &min_radius, const double &max_radius)
Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate...
Definition: sac_model.h:368
pcl::ModelCoefficients::header
::pcl::PCLHeader header
Definition: ModelCoefficients.h:18
pcl::SACMODEL_SPHERE
@ SACMODEL_SPHERE
Definition: model_types.h:51
pcl::SampleConsensusModelCircle3D
SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
Definition: sac_model_circle3d.h:60
pcl::SampleConsensusModelParallelLine::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold (in radians).
Definition: sac_model_parallel_line.h:129
pcl::SampleConsensusModelNormalParallelPlane
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
Definition: sac_model_normal_parallel_plane.h:85
pcl::SampleConsensusModelCircle2D
SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane.
Definition: sac_model_circle.h:59
pcl::SampleConsensusModelCone
SampleConsensusModelCone defines a model for 3D cone segmentation.
Definition: sac_model_cone.h:64
pcl::PointIndices::indices
std::vector< int > indices
Definition: PointIndices.h:19
pcl::SampleConsensusModelParallelPlane::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_parallel_plane.h:130
pcl::SACMODEL_NORMAL_SPHERE
@ SACMODEL_NORMAL_SPHERE
Definition: model_types.h:59
pcl::PointIndices
Definition: PointIndices.h:12
pcl::LeastMedianSquares
LeastMedianSquares represents an implementation of the LMedS (Least Median of Squares) algorithm.
Definition: lmeds.h:56
pcl::SACSegmentation::segment
virtual void segment(PointIndices &inliers, ModelCoefficients &model_coefficients)
Base method for segmentation of a model in a PointCloud given by <setInputCloud (),...
Definition: sac_segmentation.hpp:75
pcl::SampleConsensusModelCylinder::Ptr
shared_ptr< SampleConsensusModelCylinder< PointT, PointNT > > Ptr
Definition: sac_model_cylinder.h:80
pcl::SampleConsensusModelCone::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
Definition: sac_model_cone.h:168
pcl::SACMODEL_PARALLEL_LINE
@ SACMODEL_PARALLEL_LINE
Definition: model_types.h:55
pcl::SampleConsensusModelParallelPlane::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_parallel_plane.h:134
pcl::SACSegmentation::initSAC
virtual void initSAC(const int method_type)
Initialize the Sample Consensus method and set its parameters.
Definition: sac_segmentation.hpp:270
pcl::SampleConsensusModelPerpendicularPlane
SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional ang...
Definition: sac_model_perpendicular_plane.h:71
pcl::SampleConsensusModelPerpendicularPlane::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_perpendicular_plane.h:137
pcl::SampleConsensusModelParallelLine::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_parallel_line.h:126
pcl::RandomizedRandomSampleConsensus
RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RAndom SAmple...
Definition: rransac.h:56
pcl::MaximumLikelihoodSampleConsensus
MaximumLikelihoodSampleConsensus represents an implementation of the MLESAC (Maximum Likelihood Estim...
Definition: mlesac.h:57
pcl::SampleConsensusModelParallelLine
SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular co...
Definition: sac_model_parallel_line.h:65
pcl::SampleConsensusModelCylinder::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_cylinder.h:153
pcl::SampleConsensusModelNormalSphere
SampleConsensusModelNormalSphere defines a model for 3D sphere segmentation using additional surface ...
Definition: sac_model_normal_sphere.h:69
pcl::SampleConsensusModelNormalParallelPlane::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:161
pcl::SampleConsensusModelNormalSphere::Ptr
shared_ptr< SampleConsensusModelNormalSphere< PointT, PointNT > > Ptr
Definition: sac_model_normal_sphere.h:88
pcl::SampleConsensusModelNormalPlane::Ptr
shared_ptr< SampleConsensusModelNormalPlane< PointT, PointNT > > Ptr
Definition: sac_model_normal_plane.h:94
pcl::RandomizedMEstimatorSampleConsensus
RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator...
Definition: rmsac.h:57
pcl::SACMODEL_CONE
@ SACMODEL_CONE
Definition: model_types.h:53
pcl::SampleConsensusModelParallelPlane
SampleConsensusModelParallelPlane defines a model for 3D plane segmentation using additional angular ...
Definition: sac_model_parallel_plane.h:66
pcl::SampleConsensusModelNormalParallelPlane::setDistanceFromOrigin
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
Definition: sac_model_normal_parallel_plane.h:171
pcl::SACMODEL_STICK
@ SACMODEL_STICK
Definition: model_types.h:64
pcl::SACMODEL_PLANE
@ SACMODEL_PLANE
Definition: model_types.h:47
pcl::SACSegmentation
SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models,...
Definition: sac_segmentation.h:64
pcl::SACMODEL_CYLINDER
@ SACMODEL_CYLINDER
Definition: model_types.h:52
pcl::SampleConsensusModelSphere
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
Definition: sac_model_sphere.h:59
pcl::SampleConsensusModelNormalParallelPlane::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_normal_parallel_plane.h:154
pcl::SampleConsensusModelCone::setMinMaxOpeningAngle
void setMinMaxOpeningAngle(const double &min_angle, const double &max_angle)
Set the minimum and maximum allowable opening angle for a cone model given from a user.
Definition: sac_model_cone.h:176
pcl::SampleConsensusModelNormalParallelPlane::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_normal_parallel_plane.h:150