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