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