Point Cloud Library (PCL)  1.7.0
sac_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
42 #define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
43 
44 #include <pcl/segmentation/sac_segmentation.h>
45 
46 // Sample Consensus methods
47 #include <pcl/sample_consensus/sac.h>
48 #include <pcl/sample_consensus/lmeds.h>
49 #include <pcl/sample_consensus/mlesac.h>
50 #include <pcl/sample_consensus/msac.h>
51 #include <pcl/sample_consensus/ransac.h>
52 #include <pcl/sample_consensus/rmsac.h>
53 #include <pcl/sample_consensus/rransac.h>
54 #include <pcl/sample_consensus/prosac.h>
55 
56 // Sample Consensus models
57 #include <pcl/sample_consensus/sac_model.h>
58 #include <pcl/sample_consensus/sac_model_circle.h>
59 #include <pcl/sample_consensus/sac_model_circle3d.h>
60 #include <pcl/sample_consensus/sac_model_cone.h>
61 #include <pcl/sample_consensus/sac_model_cylinder.h>
62 #include <pcl/sample_consensus/sac_model_line.h>
63 #include <pcl/sample_consensus/sac_model_normal_plane.h>
64 #include <pcl/sample_consensus/sac_model_parallel_plane.h>
65 #include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
66 #include <pcl/sample_consensus/sac_model_parallel_line.h>
67 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
68 #include <pcl/sample_consensus/sac_model_plane.h>
69 #include <pcl/sample_consensus/sac_model_sphere.h>
70 #include <pcl/sample_consensus/sac_model_normal_sphere.h>
71 #include <pcl/sample_consensus/sac_model_stick.h>
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////
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 inthe 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