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