Point Cloud Library (PCL)  1.11.1-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 
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;
112  sac_->getModelCoefficients (coeff);
113 
114  // If the user needs optimized coefficients
115  if (optimize_coefficients_)
116  {
117  Eigen::VectorXf coeff_refined;
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 
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:162
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:158
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:63
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:185
pcl::SampleConsensusModelCone::setEpsAngle
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_cone.h:152
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:125
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:148
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:646
pcl::SampleConsensusModelPerpendicularPlane::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_perpendicular_plane.h:132
pcl::SampleConsensusModelNormalParallelPlane::getDistanceFromOrigin
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
Definition: sac_model_normal_parallel_plane.h:176
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
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:55
pcl::SampleConsensusModelFromNormals::getNormalDistanceWeight
double getNormalDistanceWeight() const
Get the normal angular distance weight.
Definition: sac_model.h:638
pcl::PointIndices::header
::pcl::PCLHeader header
Definition: PointIndices.h:19
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:135
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:388
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:78
pcl::ModelCoefficients::values
std::vector< float > values
Definition: ModelCoefficients.h:19
pcl::SampleConsensusModelParallelPlane::Ptr
shared_ptr< SampleConsensusModelParallelPlane< PointT > > Ptr
Definition: sac_model_parallel_plane.h:74
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:162
pcl::SampleConsensusModelLine
SampleConsensusModelLine defines a model for 3D line segmentation.
Definition: sac_model_line.h:61
pcl::SampleConsensusModelPerpendicularPlane::Ptr
shared_ptr< SampleConsensusModelPerpendicularPlane< PointT > > Ptr
Definition: sac_model_perpendicular_plane.h:79
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:103
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:349
pcl::SampleConsensusModelStick
SampleConsensusModelStick defines a model for 3D stick segmentation.
Definition: sac_model_stick.h:63
pcl::ModelCoefficients
Definition: ModelCoefficients.h:11
pcl::SampleConsensusModelNormalParallelPlane::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:166
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:118
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:122
pcl::SampleConsensusModelCone::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_cone.h:156
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:121
pcl::SampleConsensusModelFromNormals::setNormalDistanceWeight
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
Definition: sac_model.h:631
pcl::SACMODEL_CIRCLE2D
@ SACMODEL_CIRCLE2D
Definition: model_types.h:49
M_PI
#define M_PI
Definition: pcl_macros.h:201
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:375
pcl::ModelCoefficients::header
::pcl::PCLHeader header
Definition: ModelCoefficients.h:17
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:86
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:62
pcl::SampleConsensusModelParallelPlane::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_parallel_plane.h:129
pcl::SACMODEL_NORMAL_SPHERE
@ SACMODEL_NORMAL_SPHERE
Definition: model_types.h:59
pcl::PointIndices
Definition: PointIndices.h:11
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:77
pcl::SampleConsensusModelCylinder::Ptr
shared_ptr< SampleConsensusModelCylinder< PointT, PointNT > > Ptr
Definition: sac_model_cylinder.h:79
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:166
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:133
pcl::SACSegmentation::initSAC
virtual void initSAC(const int method_type)
Initialize the Sample Consensus method and set its parameters.
Definition: sac_segmentation.hpp:272
pcl::SampleConsensusModelPerpendicularPlane
SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional ang...
Definition: sac_model_perpendicular_plane.h:70
pcl::SampleConsensusModelPerpendicularPlane::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_perpendicular_plane.h:136
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:152
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:162
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:56
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:65
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:172
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
memory.h
Defines functions, macros and traits for allocating and using memory.
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:155
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:174
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:151