Point Cloud Library (PCL)  1.12.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 #include <pcl/memory.h> // for static_pointer_cast
74 
75 //////////////////////////////////////////////////////////////////////////////////////////////
76 template <typename PointT> void
78 {
79  // Copy the header information
80  inliers.header = model_coefficients.header = input_->header;
81 
82  if (!initCompute ())
83  {
84  inliers.indices.clear (); model_coefficients.values.clear ();
85  return;
86  }
87 
88  // Initialize the Sample Consensus model and set its parameters
89  if (!initSACModel (model_type_))
90  {
91  PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
92  deinitCompute ();
93  inliers.indices.clear (); model_coefficients.values.clear ();
94  return;
95  }
96  // Initialize the Sample Consensus method and set its parameters
97  initSAC (method_type_);
98 
99  if (!sac_->computeModel (0))
100  {
101  PCL_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
102  deinitCompute ();
103  inliers.indices.clear (); model_coefficients.values.clear ();
104  return;
105  }
106 
107  // Get the model inliers
108  sac_->getInliers (inliers.indices);
109 
110  // Get the model coefficients
111  Eigen::VectorXf coeff (model_->getModelSize ());
112  sac_->getModelCoefficients (coeff);
113 
114  // If the user needs optimized coefficients
115  if (optimize_coefficients_)
116  {
117  Eigen::VectorXf coeff_refined (model_->getModelSize ());
118  model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
119  model_coefficients.values.resize (coeff_refined.size ());
120  memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float));
121  // Refine inliers
122  model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices);
123  }
124  else
125  {
126  model_coefficients.values.resize (coeff.size ());
127  memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float));
128  }
129 
130  deinitCompute ();
131 }
132 
133 //////////////////////////////////////////////////////////////////////////////////////////////
134 template <typename PointT> bool
136 {
137  if (model_)
138  model_.reset ();
139 
140  // Build the model
141  switch (model_type)
142  {
143  case SACMODEL_PLANE:
144  {
145  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
146  model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_, random_));
147  break;
148  }
149  case SACMODEL_LINE:
150  {
151  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
152  model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_, random_));
153  break;
154  }
155  case SACMODEL_STICK:
156  {
157  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
158  model_.reset (new SampleConsensusModelStick<PointT> (input_, *indices_));
159  double min_radius, max_radius;
160  model_->getRadiusLimits (min_radius, max_radius);
161  if (radius_min_ != min_radius && radius_max_ != max_radius)
162  {
163  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
164  model_->setRadiusLimits (radius_min_, radius_max_);
165  }
166  break;
167  }
168  case SACMODEL_CIRCLE2D:
169  {
170  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
171  model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_, random_));
172  typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
173  double min_radius, max_radius;
174  model_circle->getRadiusLimits (min_radius, max_radius);
175  if (radius_min_ != min_radius && radius_max_ != max_radius)
176  {
177  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
178  model_circle->setRadiusLimits (radius_min_, radius_max_);
179  }
180  break;
181  }
182  case SACMODEL_CIRCLE3D:
183  {
184  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
185  model_.reset (new SampleConsensusModelCircle3D<PointT> (input_, *indices_));
186  typename SampleConsensusModelCircle3D<PointT>::Ptr model_circle3d = static_pointer_cast<SampleConsensusModelCircle3D<PointT> > (model_);
187  double min_radius, max_radius;
188  model_circle3d->getRadiusLimits (min_radius, max_radius);
189  if (radius_min_ != min_radius && radius_max_ != max_radius)
190  {
191  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
192  model_circle3d->setRadiusLimits (radius_min_, radius_max_);
193  }
194  break;
195  }
196  case SACMODEL_SPHERE:
197  {
198  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
199  model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_, random_));
200  typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
201  double min_radius, max_radius;
202  model_sphere->getRadiusLimits (min_radius, max_radius);
203  if (radius_min_ != min_radius && radius_max_ != max_radius)
204  {
205  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
206  model_sphere->setRadiusLimits (radius_min_, radius_max_);
207  }
208  break;
209  }
211  {
212  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
213  model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_, random_));
214  typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
215  if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
216  {
217  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
218  model_parallel->setAxis (axis_);
219  }
220  if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
221  {
222  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);
223  model_parallel->setEpsAngle (eps_angle_);
224  }
225  break;
226  }
228  {
229  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
230  model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_, random_));
231  typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
232  if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
233  {
234  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
235  model_perpendicular->setAxis (axis_);
236  }
237  if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
238  {
239  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);
240  model_perpendicular->setEpsAngle (eps_angle_);
241  }
242  break;
243  }
245  {
246  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
247  model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_, random_));
248  typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
249  if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
250  {
251  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
252  model_parallel->setAxis (axis_);
253  }
254  if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
255  {
256  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);
257  model_parallel->setEpsAngle (eps_angle_);
258  }
259  break;
260  }
261  default:
262  {
263  PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
264  return (false);
265  }
266  }
267  return (true);
268 }
269 
270 //////////////////////////////////////////////////////////////////////////////////////////////
271 template <typename PointT> void
273 {
274  if (sac_)
275  sac_.reset ();
276  // Build the sample consensus method
277  switch (method_type)
278  {
279  case SAC_RANSAC:
280  default:
281  {
282  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
283  sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
284  break;
285  }
286  case SAC_LMEDS:
287  {
288  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
289  sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_));
290  break;
291  }
292  case SAC_MSAC:
293  {
294  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
295  sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_));
296  break;
297  }
298  case SAC_RRANSAC:
299  {
300  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
301  sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_));
302  break;
303  }
304  case SAC_RMSAC:
305  {
306  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
307  sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_));
308  break;
309  }
310  case SAC_MLESAC:
311  {
312  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
313  sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_));
314  break;
315  }
316  case SAC_PROSAC:
317  {
318  PCL_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
319  sac_.reset (new ProgressiveSampleConsensus<PointT> (model_, threshold_));
320  break;
321  }
322  }
323  // Set the Sample Consensus parameters if they are given/changed
324  if (sac_->getProbability () != probability_)
325  {
326  PCL_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
327  sac_->setProbability (probability_);
328  }
329  if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
330  {
331  PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
332  sac_->setMaxIterations (max_iterations_);
333  }
334  if (samples_radius_ > 0.)
335  {
336  PCL_DEBUG ("[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
337  // Set maximum distance for radius search during random sampling
338  model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
339  }
340  if (sac_->getNumberOfThreads () != threads_)
341  {
342  PCL_DEBUG ("[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_);
343  sac_->setNumberOfThreads (threads_);
344  }
345 }
346 
347 //////////////////////////////////////////////////////////////////////////////////////////////
348 template <typename PointT, typename PointNT> bool
350 {
351  if (!input_ || !normals_)
352  {
353  PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
354  return (false);
355  }
356  // Check if input is synced with the normals
357  if (input_->size () != normals_->size ())
358  {
359  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 ());
360  return (false);
361  }
362 
363  if (model_)
364  model_.reset ();
365 
366  // Build the model
367  switch (model_type)
368  {
369  case SACMODEL_CYLINDER:
370  {
371  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
372  model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_, random_));
373  typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
374 
375  // Set the input normals
376  model_cylinder->setInputNormals (normals_);
377  double min_radius, max_radius;
378  model_cylinder->getRadiusLimits (min_radius, max_radius);
379  if (radius_min_ != min_radius && radius_max_ != max_radius)
380  {
381  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
382  model_cylinder->setRadiusLimits (radius_min_, radius_max_);
383  }
384  if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
385  {
386  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
387  model_cylinder->setNormalDistanceWeight (distance_weight_);
388  }
389  if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
390  {
391  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
392  model_cylinder->setAxis (axis_);
393  }
394  if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
395  {
396  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);
397  model_cylinder->setEpsAngle (eps_angle_);
398  }
399  break;
400  }
402  {
403  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
404  model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_, random_));
405  typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
406  // Set the input normals
407  model_normals->setInputNormals (normals_);
408  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
409  {
410  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
411  model_normals->setNormalDistanceWeight (distance_weight_);
412  }
413  break;
414  }
416  {
417  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
418  model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_, random_));
419  typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_);
420  // Set the input normals
421  model_normals->setInputNormals (normals_);
422  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
423  {
424  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
425  model_normals->setNormalDistanceWeight (distance_weight_);
426  }
427  if (distance_from_origin_ != model_normals->getDistanceFromOrigin ())
428  {
429  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
430  model_normals->setDistanceFromOrigin (distance_from_origin_);
431  }
432  if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
433  {
434  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
435  model_normals->setAxis (axis_);
436  }
437  if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
438  {
439  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);
440  model_normals->setEpsAngle (eps_angle_);
441  }
442  break;
443  }
444  case SACMODEL_CONE:
445  {
446  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
447  model_.reset (new SampleConsensusModelCone<PointT, PointNT> (input_, *indices_, random_));
448  typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_);
449 
450  // Set the input normals
451  model_cone->setInputNormals (normals_);
452  double min_angle, max_angle;
453  model_cone->getMinMaxOpeningAngle(min_angle, max_angle);
454  if (min_angle_ != min_angle && max_angle_ != max_angle)
455  {
456  PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
457  model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_);
458  }
459 
460  if (distance_weight_ != model_cone->getNormalDistanceWeight ())
461  {
462  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
463  model_cone->setNormalDistanceWeight (distance_weight_);
464  }
465  if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_)
466  {
467  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
468  model_cone->setAxis (axis_);
469  }
470  if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_)
471  {
472  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);
473  model_cone->setEpsAngle (eps_angle_);
474  }
475  break;
476  }
478  {
479  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
480  model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_, random_));
481  typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_);
482  // Set the input normals
483  model_normals_sphere->setInputNormals (normals_);
484  double min_radius, max_radius;
485  model_normals_sphere->getRadiusLimits (min_radius, max_radius);
486  if (radius_min_ != min_radius && radius_max_ != max_radius)
487  {
488  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
489  model_normals_sphere->setRadiusLimits (radius_min_, radius_max_);
490  }
491 
492  if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ())
493  {
494  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
495  model_normals_sphere->setNormalDistanceWeight (distance_weight_);
496  }
497  break;
498  }
499  // If nothing else, try SACSegmentation
500  default:
501  {
502  return (pcl::SACSegmentation<PointT>::initSACModel (model_type));
503  }
504  }
505 
506  return (true);
507 }
508 
509 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
510 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>;
511 
512 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
513 
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
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.
@ 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_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
#define M_PI
Definition: pcl_macros.h:201
::pcl::PCLHeader header
std::vector< float > values
::pcl::PCLHeader header
Definition: PointIndices.h:19