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