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);
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>;
MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) alg...
MaximumLikelihoodSampleConsensus represents an implementation of the MLESAC (Maximum Likelihood Estim...
ProgressiveSampleConsensus represents an implementation of the PROSAC (PROgressive SAmple Consensus) ...
RandomSampleConsensus represents an implementation of the RANSAC (RANdom SAmple Consensus) algorithm,...
RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator...
RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RANdom SAmple...
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
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
double getNormalDistanceWeight() const
Get the normal angular distance weight.
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...
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.
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.
@ SACMODEL_PARALLEL_PLANE
@ SACMODEL_NORMAL_PARALLEL_PLANE
@ SACMODEL_PERPENDICULAR_PLANE
std::vector< float > values