Point Cloud Library (PCL)  1.13.0-dev
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 #include <pcl/sample_consensus/sac_model_ellipse3d.h>
73 
74 #include <pcl/memory.h> // for static_pointer_cast
75 
76 //////////////////////////////////////////////////////////////////////////////////////////////
77 template <typename PointT> void
79 {
80  // Copy the header information
81  inliers.header = model_coefficients.header = input_->header;
82 
83  if (!initCompute ())
84  {
85  inliers.indices.clear (); model_coefficients.values.clear ();
86  return;
87  }
88 
89  // Initialize the Sample Consensus model and set its parameters
90  if (!initSACModel (model_type_))
91  {
92  PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
93  deinitCompute ();
94  inliers.indices.clear (); model_coefficients.values.clear ();
95  return;
96  }
97  // Initialize the Sample Consensus method and set its parameters
98  initSAC (method_type_);
99 
100  if (!sac_->computeModel (0))
101  {
102  PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
103  deinitCompute ();
104  inliers.indices.clear (); model_coefficients.values.clear ();
105  return;
106  }
107 
108  // Get the model inliers
109  sac_->getInliers (inliers.indices);
110 
111  // Get the model coefficients
112  Eigen::VectorXf coeff (model_->getModelSize ());
113  sac_->getModelCoefficients (coeff);
114 
115  // If the user needs optimized coefficients
116  if (optimize_coefficients_)
117  {
118  Eigen::VectorXf coeff_refined (model_->getModelSize ());
119  model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
120  model_coefficients.values.resize (coeff_refined.size ());
121  memcpy (model_coefficients.values.data(), coeff_refined.data(), coeff_refined.size () * sizeof (float));
122  // Refine inliers
123  model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices);
124  }
125  else
126  {
127  model_coefficients.values.resize (coeff.size ());
128  memcpy (model_coefficients.values.data(), coeff.data(), coeff.size () * sizeof (float));
129  }
130 
131  deinitCompute ();
132 }
133 
134 //////////////////////////////////////////////////////////////////////////////////////////////
135 template <typename PointT> bool
137 {
138  if (model_)
139  model_.reset ();
140 
141  // Build the model
142  switch (model_type)
143  {
144  case SACMODEL_PLANE:
145  {
146  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
147  model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_, random_));
148  break;
149  }
150  case SACMODEL_LINE:
151  {
152  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
153  model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_, random_));
154  break;
155  }
156  case SACMODEL_STICK:
157  {
158  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
159  model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_));
160  double min_radius, max_radius;
161  model_->getRadiusLimits (min_radius, max_radius);
162  if (radius_min_ != min_radius && radius_max_ != max_radius)
163  {
164  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
165  model_->setRadiusLimits (radius_min_, radius_max_);
166  }
167  break;
168  }
169  case SACMODEL_CIRCLE2D:
170  {
171  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
172  model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_, random_));
173  typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
174  double min_radius, max_radius;
175  model_circle->getRadiusLimits (min_radius, max_radius);
176  if (radius_min_ != min_radius && radius_max_ != max_radius)
177  {
178  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
179  model_circle->setRadiusLimits (radius_min_, radius_max_);
180  }
181  break;
182  }
183  case SACMODEL_CIRCLE3D:
184  {
185  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
186  model_.reset (new SampleConsensusModelCircle3D<PointT> (input_, *indices_));
187  typename SampleConsensusModelCircle3D<PointT>::Ptr model_circle3d = static_pointer_cast<SampleConsensusModelCircle3D<PointT> > (model_);
188  double min_radius, max_radius;
189  model_circle3d->getRadiusLimits (min_radius, max_radius);
190  if (radius_min_ != min_radius && radius_max_ != max_radius)
191  {
192  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
193  model_circle3d->setRadiusLimits (radius_min_, radius_max_);
194  }
195  break;
196  }
197  case SACMODEL_SPHERE:
198  {
199  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
200  model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_, random_));
201  typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
202  double min_radius, max_radius;
203  model_sphere->getRadiusLimits (min_radius, max_radius);
204  if (radius_min_ != min_radius && radius_max_ != max_radius)
205  {
206  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
207  model_sphere->setRadiusLimits (radius_min_, radius_max_);
208  }
209  break;
210  }
212  {
213  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
214  model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_, random_));
215  typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
216  if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
217  {
218  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
219  model_parallel->setAxis (axis_);
220  }
221  if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
222  {
223  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);
224  model_parallel->setEpsAngle (eps_angle_);
225  }
226  break;
227  }
229  {
230  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
231  model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_, random_));
232  typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
233  if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
234  {
235  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
236  model_perpendicular->setAxis (axis_);
237  }
238  if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
239  {
240  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);
241  model_perpendicular->setEpsAngle (eps_angle_);
242  }
243  break;
244  }
246  {
247  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
248  model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_, random_));
249  typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
250  if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
251  {
252  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
253  model_parallel->setAxis (axis_);
254  }
255  if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
256  {
257  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);
258  model_parallel->setEpsAngle (eps_angle_);
259  }
260  break;
261  }
262  case SACMODEL_ELLIPSE3D:
263  {
264  PCL_DEBUG("[pcl::%s::initSACModel] Using a model of type: SACMODEL_ELLIPSE3D\n", getClassName().c_str());
265  model_.reset(new SampleConsensusModelEllipse3D<PointT>(input_, *indices_));
266  typename SampleConsensusModelEllipse3D<PointT>::Ptr model_ellipse3d = static_pointer_cast<SampleConsensusModelEllipse3D<PointT>>(model_);
267  double min_radius, max_radius;
268  model_ellipse3d->getRadiusLimits(min_radius, max_radius);
269  if (radius_min_ != min_radius && radius_max_ != max_radius) {
270  PCL_DEBUG("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName().c_str(), radius_min_, radius_max_);
271  model_ellipse3d->setRadiusLimits(radius_min_, radius_max_);
272  }
273  break;
274  }
275  case SACMODEL_CYLINDER:
278  case SACMODEL_CONE:
280  {
281  PCL_ERROR ("[pcl::%s::initSACModel] Use SACSegmentationFromNormals for this model instead!\n", getClassName ().c_str ());
282  return (false);
283  }
284  default:
285  {
286  PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
287  return (false);
288  }
289  }
290  return (true);
291 }
292 
293 //////////////////////////////////////////////////////////////////////////////////////////////
294 template <typename PointT> void
296 {
297  if (sac_)
298  sac_.reset ();
299  // Build the sample consensus method
300  switch (method_type)
301  {
302  case SAC_RANSAC:
303  default:
304  {
305  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
306  sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
307  break;
308  }
309  case SAC_LMEDS:
310  {
311  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
312  sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_));
313  break;
314  }
315  case SAC_MSAC:
316  {
317  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
318  sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_));
319  break;
320  }
321  case SAC_RRANSAC:
322  {
323  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
324  sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_));
325  break;
326  }
327  case SAC_RMSAC:
328  {
329  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
330  sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_));
331  break;
332  }
333  case SAC_MLESAC:
334  {
335  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
336  sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_));
337  break;
338  }
339  case SAC_PROSAC:
340  {
341  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
342  sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_));
343  break;
344  }
345  }
346  // Set the Sample Consensus parameters if they are given/changed
347  if (sac_->getProbability () != probability_)
348  {
349  PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
350  sac_->setProbability (probability_);
351  }
352  if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
353  {
354  PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
355  sac_->setMaxIterations (max_iterations_);
356  }
357  if (samples_radius_ > 0.)
358  {
359  PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
360  // Set maximum distance for radius search during random sampling
361  model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
362  }
363  if (sac_->getNumberOfThreads () != threads_)
364  {
365  PCL_DEBUG ("[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_);
366  sac_->setNumberOfThreads (threads_);
367  }
368 }
369 
370 //////////////////////////////////////////////////////////////////////////////////////////////
371 template <typename PointT, typename PointNT> bool
373 {
374  if (!input_ || !normals_)
375  {
376  PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
377  return (false);
378  }
379  // Check if input is synced with the normals
380  if (input_->size () != normals_->size ())
381  {
382  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 ());
383  return (false);
384  }
385 
386  if (model_)
387  model_.reset ();
388 
389  // Build the model
390  switch (model_type)
391  {
392  case SACMODEL_CYLINDER:
393  {
394  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
395  model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_, random_));
396  typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
397 
398  // Set the input normals
399  model_cylinder->setInputNormals (normals_);
400  double min_radius, max_radius;
401  model_cylinder->getRadiusLimits (min_radius, max_radius);
402  if (radius_min_ != min_radius && radius_max_ != max_radius)
403  {
404  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
405  model_cylinder->setRadiusLimits (radius_min_, radius_max_);
406  }
407  if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
408  {
409  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
410  model_cylinder->setNormalDistanceWeight (distance_weight_);
411  }
412  if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
413  {
414  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
415  model_cylinder->setAxis (axis_);
416  }
417  if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
418  {
419  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);
420  model_cylinder->setEpsAngle (eps_angle_);
421  }
422  break;
423  }
425  {
426  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
427  model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_, random_));
428  typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
429  // Set the input normals
430  model_normals->setInputNormals (normals_);
431  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
432  {
433  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
434  model_normals->setNormalDistanceWeight (distance_weight_);
435  }
436  break;
437  }
439  {
440  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
441  model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_, random_));
442  typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
443  // Set the input normals
444  model_normals->setInputNormals (normals_);
445  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
446  {
447  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
448  model_normals->setNormalDistanceWeight (distance_weight_);
449  }
450  if (distance_from_origin_ != model_normals->getDistanceFromOrigin ())
451  {
452  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
453  model_normals->setDistanceFromOrigin (distance_from_origin_);
454  }
455  if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
456  {
457  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
458  model_normals->setAxis (axis_);
459  }
460  if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
461  {
462  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);
463  model_normals->setEpsAngle (eps_angle_);
464  }
465  break;
466  }
467  case SACMODEL_CONE:
468  {
469  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
470  model_.reset (new SampleConsensusModelCone<PointT, PointNT> (input_, *indices_, random_));
471  typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_);
472 
473  // Set the input normals
474  model_cone->setInputNormals (normals_);
475  double min_angle, max_angle;
476  model_cone->getMinMaxOpeningAngle(min_angle, max_angle);
477  if (min_angle_ != min_angle && max_angle_ != max_angle)
478  {
479  PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
480  model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_);
481  }
482 
483  if (distance_weight_ != model_cone->getNormalDistanceWeight ())
484  {
485  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
486  model_cone->setNormalDistanceWeight (distance_weight_);
487  }
488  if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_)
489  {
490  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
491  model_cone->setAxis (axis_);
492  }
493  if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_)
494  {
495  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);
496  model_cone->setEpsAngle (eps_angle_);
497  }
498  break;
499  }
501  {
502  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
503  model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_, random_));
504  typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_);
505  // Set the input normals
506  model_normals_sphere->setInputNormals (normals_);
507  double min_radius, max_radius;
508  model_normals_sphere->getRadiusLimits (min_radius, max_radius);
509  if (radius_min_ != min_radius && radius_max_ != max_radius)
510  {
511  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
512  model_normals_sphere->setRadiusLimits (radius_min_, radius_max_);
513  }
514 
515  if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ())
516  {
517  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
518  model_normals_sphere->setNormalDistanceWeight (distance_weight_);
519  }
520  break;
521  }
522  // If nothing else, try SACSegmentation
523  default:
524  {
525  return (pcl::SACSegmentation<PointT>::initSACModel (model_type));
526  }
527  }
528 
529  return (true);
530 }
531 
532 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
533 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>;
534 
535 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
536 
LeastMedianSquares represents an implementation of the LMedS (Least Median of Squares) algorithm.
Definition: lmeds.h:60
MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) alg...
Definition: msac.h:61
MaximumLikelihoodSampleConsensus represents an implementation of the MLESAC (Maximum Likelihood Estim...
Definition: mlesac.h:58
ProgressiveSampleConsensus represents an implementation of the PROSAC (PROgressive SAmple Consensus) ...
Definition: prosac.h:56
RandomSampleConsensus represents an implementation of the RANSAC (RANdom SAmple Consensus) algorithm,...
Definition: ransac.h:66
RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator...
Definition: rmsac.h:57
RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RANdom SAmple...
Definition: rransac.h:61
bool initSACModel(const int model_type) override
Initialize the Sample Consensus model and set its parameters.
SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models,...
virtual void initSAC(const int method_type)
Initialize the Sample Consensus method and set its parameters.
virtual void segment(PointIndices &inliers, ModelCoefficients &model_coefficients)
Base method for segmentation of a model in a PointCloud given by <setInputCloud (),...
virtual bool initSACModel(const int model_type)
Initialize the Sample Consensus model and set its parameters.
SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane.
shared_ptr< SampleConsensusModelCircle2D< PointT > > Ptr
SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
shared_ptr< SampleConsensusModelCircle3D< PointT > > Ptr
SampleConsensusModelCone defines a model for 3D cone segmentation.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
shared_ptr< SampleConsensusModelCone< PointT, PointNT > > Ptr
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.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cylinder direction.
shared_ptr< SampleConsensusModelCylinder< PointT, PointNT > > Ptr
SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
shared_ptr< SampleConsensusModelEllipse3D< PointT > > Ptr
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
Definition: sac_model.h:632
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:652
double getNormalDistanceWeight() const
Get the normal angular distance weight.
Definition: sac_model.h:644
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:375
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:388
SampleConsensusModelLine defines a model for 3D line segmentation.
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
shared_ptr< SampleConsensusModelNormalParallelPlane< PointT, PointNT > > Ptr
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
shared_ptr< SampleConsensusModelNormalPlane< PointT, PointNT > > Ptr
SampleConsensusModelNormalSphere defines a model for 3D sphere segmentation using additional surface ...
shared_ptr< SampleConsensusModelNormalSphere< PointT, PointNT > > Ptr
SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular co...
shared_ptr< SampleConsensusModelParallelLine< PointT > > Ptr
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a line.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a line.
double getEpsAngle() const
Get the angle epsilon (delta) threshold (in radians).
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelParallelPlane defines a model for 3D plane segmentation using additional angular ...
shared_ptr< SampleConsensusModelParallelPlane< PointT > > Ptr
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional ang...
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
shared_ptr< SampleConsensusModelPerpendicularPlane< PointT > > Ptr
SampleConsensusModelPlane defines a model for 3D plane segmentation.
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
shared_ptr< SampleConsensusModelSphere< PointT > > Ptr
SampleConsensusModelStick defines a model for 3D stick segmentation.
Defines functions, macros and traits for allocating and using memory.
constexpr int SAC_RANSAC
Definition: method_types.h:45
constexpr int SAC_MLESAC
Definition: method_types.h:50
constexpr int SAC_RMSAC
Definition: method_types.h:49
@ SACMODEL_CYLINDER
Definition: model_types.h:52
@ SACMODEL_PLANE
Definition: model_types.h:47
@ SACMODEL_PARALLEL_PLANE
Definition: model_types.h:62
@ SACMODEL_SPHERE
Definition: model_types.h:51
@ SACMODEL_PARALLEL_LINE
Definition: model_types.h:55
@ SACMODEL_CIRCLE3D
Definition: model_types.h:50
@ SACMODEL_NORMAL_PARALLEL_PLANE
Definition: model_types.h:63
@ SACMODEL_PERPENDICULAR_PLANE
Definition: model_types.h:56
@ SACMODEL_NORMAL_SPHERE
Definition: model_types.h:59
@ SACMODEL_STICK
Definition: model_types.h:64
@ SACMODEL_ELLIPSE3D
Definition: model_types.h:65
@ SACMODEL_CIRCLE2D
Definition: model_types.h:49
@ SACMODEL_NORMAL_PLANE
Definition: model_types.h:58
@ SACMODEL_CONE
Definition: model_types.h:53
@ SACMODEL_LINE
Definition: model_types.h:48
constexpr int SAC_MSAC
Definition: method_types.h:47
constexpr int SAC_LMEDS
Definition: method_types.h:46
constexpr int SAC_RRANSAC
Definition: method_types.h:48
constexpr int SAC_PROSAC
Definition: method_types.h:51
#define M_PI
Definition: pcl_macros.h:201
::pcl::PCLHeader header
std::vector< float > values
::pcl::PCLHeader header
Definition: PointIndices.h:18