Point Cloud Library (PCL)  1.9.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 //////////////////////////////////////////////////////////////////////////////////////////////
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 }
339 
340 //////////////////////////////////////////////////////////////////////////////////////////////
341 template <typename PointT, typename PointNT> bool
343 {
344  if (!input_ || !normals_)
345  {
346  PCL_ERROR ("[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
347  return (false);
348  }
349  // Check if input is synced with the normals
350  if (input_->points.size () != normals_->points.size ())
351  {
352  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 ());
353  return (false);
354  }
355 
356  if (model_)
357  model_.reset ();
358 
359  // Build the model
360  switch (model_type)
361  {
362  case SACMODEL_CYLINDER:
363  {
364  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
365  model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_, random_));
366  typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_);
367 
368  // Set the input normals
369  model_cylinder->setInputNormals (normals_);
370  double min_radius, max_radius;
371  model_cylinder->getRadiusLimits (min_radius, max_radius);
372  if (radius_min_ != min_radius && radius_max_ != max_radius)
373  {
374  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
375  model_cylinder->setRadiusLimits (radius_min_, radius_max_);
376  }
377  if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
378  {
379  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
380  model_cylinder->setNormalDistanceWeight (distance_weight_);
381  }
382  if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
383  {
384  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
385  model_cylinder->setAxis (axis_);
386  }
387  if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
388  {
389  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);
390  model_cylinder->setEpsAngle (eps_angle_);
391  }
392  break;
393  }
395  {
396  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
397  model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_, random_));
398  typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_);
399  // Set the input normals
400  model_normals->setInputNormals (normals_);
401  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
402  {
403  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
404  model_normals->setNormalDistanceWeight (distance_weight_);
405  }
406  break;
407  }
409  {
410  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
411  model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_, random_));
413  // Set the input normals
414  model_normals->setInputNormals (normals_);
415  if (distance_weight_ != model_normals->getNormalDistanceWeight ())
416  {
417  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
418  model_normals->setNormalDistanceWeight (distance_weight_);
419  }
420  if (distance_from_origin_ != model_normals->getDistanceFromOrigin ())
421  {
422  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
423  model_normals->setDistanceFromOrigin (distance_from_origin_);
424  }
425  if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
426  {
427  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
428  model_normals->setAxis (axis_);
429  }
430  if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
431  {
432  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);
433  model_normals->setEpsAngle (eps_angle_);
434  }
435  break;
436  }
437  case SACMODEL_CONE:
438  {
439  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
440  model_.reset (new SampleConsensusModelCone<PointT, PointNT> (input_, *indices_, random_));
441  typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = boost::static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_);
442 
443  // Set the input normals
444  model_cone->setInputNormals (normals_);
445  double min_angle, max_angle;
446  model_cone->getMinMaxOpeningAngle(min_angle, max_angle);
447  if (min_angle_ != min_angle && max_angle_ != max_angle)
448  {
449  PCL_DEBUG ("[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
450  model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_);
451  }
452 
453  if (distance_weight_ != model_cone->getNormalDistanceWeight ())
454  {
455  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
456  model_cone->setNormalDistanceWeight (distance_weight_);
457  }
458  if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_)
459  {
460  PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
461  model_cone->setAxis (axis_);
462  }
463  if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_)
464  {
465  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);
466  model_cone->setEpsAngle (eps_angle_);
467  }
468  break;
469  }
471  {
472  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
473  model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_, random_));
474  typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = boost::static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_);
475  // Set the input normals
476  model_normals_sphere->setInputNormals (normals_);
477  double min_radius, max_radius;
478  model_normals_sphere->getRadiusLimits (min_radius, max_radius);
479  if (radius_min_ != min_radius && radius_max_ != max_radius)
480  {
481  PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
482  model_normals_sphere->setRadiusLimits (radius_min_, radius_max_);
483  }
484 
485  if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ())
486  {
487  PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
488  model_normals_sphere->setNormalDistanceWeight (distance_weight_);
489  }
490  break;
491  }
492  // If nothing else, try SACSegmentation
493  default:
494  {
495  return (pcl::SACSegmentation<PointT>::initSACModel (model_type));
496  }
497  }
498 
499  return (true);
500 }
501 
502 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
503 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>;
504 
505 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
506 
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.
boost::shared_ptr< SampleConsensusModelNormalParallelPlane< PointT, PointNT > > Ptr
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator...
Definition: rmsac.h:57
SampleConsensusModelLine defines a model for 3D line segmentation.
boost::shared_ptr< SampleConsensusModelPerpendicularPlane< PointT > > Ptr
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.
boost::shared_ptr< SampleConsensusModelSphere< PointT > > Ptr
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:380
boost::shared_ptr< SampleConsensusModelCone< PointT, PointNT > > Ptr
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.
boost::shared_ptr< SampleConsensusModelCylinder< PointT, PointNT > > Ptr
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...
boost::shared_ptr< SampleConsensusModelNormalSphere< PointT, PointNT > > Ptr
LeastMedianSquares represents an implementation of the LMedS (Least Median of Squares) algorithm...
Definition: lmeds.h:56
boost::shared_ptr< SampleConsensusModelNormalPlane< PointT, PointNT > > Ptr
boost::shared_ptr< SampleConsensusModelParallelPlane< PointT > > Ptr
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.
boost::shared_ptr< SampleConsensusModelCircle2D< PointT > > Ptr
double getNormalDistanceWeight() const
Get the normal angular distance weight.
Definition: sac_model.h:607
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:600
boost::shared_ptr< SampleConsensusModelCircle3D< PointT > > 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:615
SampleConsensusModelCone defines a model for 3D cone segmentation.
::pcl::PCLHeader header
Definition: PointIndices.h:17
SampleConsensusModelStick defines a model for 3D stick segmentation.
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.
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:367
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:55
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
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...
boost::shared_ptr< SampleConsensusModelParallelLine< PointT > > Ptr
SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional ang...