80 inliers.
header = model_coefficients.
header = input_->header;
89 if (!initSACModel (model_type_))
91 PCL_ERROR (
"[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
97 initSAC (method_type_);
99 if (!sac_->computeModel (0))
101 PCL_ERROR (
"[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
103 inliers.
indices.clear (); model_coefficients.
values.clear ();
108 sac_->getInliers (inliers.
indices);
111 Eigen::VectorXf coeff (model_->getModelSize ());
112 sac_->getModelCoefficients (coeff);
115 if (optimize_coefficients_)
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));
122 model_->selectWithinDistance (coeff_refined, threshold_, inliers.
indices);
126 model_coefficients.
values.resize (coeff.size ());
127 memcpy (&model_coefficients.
values[0], &coeff[0], coeff.size () * sizeof (
float));
145 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
151 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
157 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
159 double min_radius, max_radius;
160 model_->getRadiusLimits (min_radius, max_radius);
161 if (radius_min_ != min_radius && radius_max_ != max_radius)
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_);
170 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
173 double min_radius, max_radius;
174 model_circle->getRadiusLimits (min_radius, max_radius);
175 if (radius_min_ != min_radius && radius_max_ != max_radius)
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_);
184 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
187 double min_radius, max_radius;
188 model_circle3d->getRadiusLimits (min_radius, max_radius);
189 if (radius_min_ != min_radius && radius_max_ != max_radius)
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_);
198 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
201 double min_radius, max_radius;
202 model_sphere->getRadiusLimits (min_radius, max_radius);
203 if (radius_min_ != min_radius && radius_max_ != max_radius)
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_);
212 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
215 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
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_);
220 if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
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_);
229 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
232 if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
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_);
237 if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
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_);
246 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
249 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
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_);
254 if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
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_);
263 PCL_ERROR (
"[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
282 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
288 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
294 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
300 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
306 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
312 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
318 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
324 if (sac_->getProbability () != probability_)
326 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
327 sac_->setProbability (probability_);
329 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
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_);
334 if (samples_radius_ > 0.)
336 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
338 model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
340 if (sac_->getNumberOfThreads () != threads_)
342 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_);
343 sac_->setNumberOfThreads (threads_);
351 if (!input_ || !normals_)
353 PCL_ERROR (
"[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
357 if (input_->size () != normals_->size ())
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 ());
371 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
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)
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_);
384 if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
386 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
387 model_cylinder->setNormalDistanceWeight (distance_weight_);
389 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
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_);
394 if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
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_);
403 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
407 model_normals->setInputNormals (normals_);
408 if (distance_weight_ != model_normals->getNormalDistanceWeight ())
410 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
411 model_normals->setNormalDistanceWeight (distance_weight_);
417 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
421 model_normals->setInputNormals (normals_);
422 if (distance_weight_ != model_normals->getNormalDistanceWeight ())
424 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
425 model_normals->setNormalDistanceWeight (distance_weight_);
427 if (distance_from_origin_ != model_normals->getDistanceFromOrigin ())
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_);
432 if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
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_);
437 if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
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_);
446 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
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)
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_);
460 if (distance_weight_ != model_cone->getNormalDistanceWeight ())
462 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
463 model_cone->setNormalDistanceWeight (distance_weight_);
465 if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_)
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_);
470 if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_)
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_);
479 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
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)
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_);
492 if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ())
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_);