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;
112 sac_->getModelCoefficients (coeff);
115 if (optimize_coefficients_)
117 Eigen::VectorXf coeff_refined;
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);
263 PCL_ERROR (
"[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
271 template <
typename Po
intT>
void
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_);
348 template <
typename Po
intT,
typename Po
intNT>
bool
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 ());
377 double 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_);
386 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), 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);
403 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
410 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
417 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
424 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
429 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), 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);
446 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
452 double 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_);
462 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), 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]);
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);
479 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
484 double 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_);
494 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
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>;
512 #endif // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_