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