41 #ifndef PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
42 #define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
44 #include <pcl/segmentation/sac_segmentation.h>
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>
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>
76 template <
typename Po
intT>
void
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));
134 template <
typename Po
intT>
bool
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;
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_);
184 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
187 double 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_);
198 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
201 double 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_);
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);
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);
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);
267 PCL_ERROR (
"[pcl::%s::initSACModel] Use SACSegmentationFromNormals for this model instead!\n", getClassName ().c_str ());
272 PCL_ERROR (
"[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
280 template <
typename Po
intT>
void
291 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
297 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
303 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
309 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
315 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
321 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
327 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
333 if (sac_->getProbability () != probability_)
335 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
336 sac_->setProbability (probability_);
338 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
340 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
341 sac_->setMaxIterations (max_iterations_);
343 if (samples_radius_ > 0.)
345 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
347 model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
349 if (sac_->getNumberOfThreads () != threads_)
351 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_);
352 sac_->setNumberOfThreads (threads_);
357 template <
typename Po
intT,
typename Po
intNT>
bool
360 if (!input_ || !normals_)
362 PCL_ERROR (
"[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
366 if (input_->size () != normals_->size ())
368 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 ());
380 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
386 double min_radius, max_radius;
388 if (radius_min_ != min_radius && radius_max_ != max_radius)
390 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
395 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
398 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->
getAxis () != axis_)
400 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
401 model_cylinder->
setAxis (axis_);
403 if (eps_angle_ != 0.0 && model_cylinder->
getEpsAngle () != eps_angle_)
405 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);
412 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
419 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
426 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
433 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
438 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
441 if (axis_ != Eigen::Vector3f::Zero () && model_normals->
getAxis () != axis_)
443 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
444 model_normals->
setAxis (axis_);
446 if (eps_angle_ != 0.0 && model_normals->
getEpsAngle () != eps_angle_)
448 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);
455 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
461 double min_angle, max_angle;
463 if (min_angle_ != min_angle && max_angle_ != max_angle)
465 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
471 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
474 if (axis_ != Eigen::Vector3f::Zero () && model_cone->
getAxis () != axis_)
476 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
479 if (eps_angle_ != 0.0 && model_cone->
getEpsAngle () != eps_angle_)
481 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);
488 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
493 double min_radius, max_radius;
495 if (radius_min_ != min_radius && radius_max_ != max_radius)
497 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
503 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
518 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
519 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>;
521 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_