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 #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>
79 template <
typename Po
intT>
void
83 inliers.
header = model_coefficients.
header = input_->header;
92 if (!initSACModel (model_type_))
94 PCL_ERROR (
"[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
100 initSAC (method_type_);
102 if (!sac_->computeModel (0))
104 PCL_ERROR (
"[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
106 inliers.
indices.clear (); model_coefficients.
values.clear ();
111 inliers.
indices = sac_->getInliers ();
114 Eigen::VectorXf coeff = sac_->getModelCoefficients ();
117 if (optimize_coefficients_)
119 Eigen::VectorXf coeff_refined (model_->getModelSize ());
120 model_->optimizeModelCoefficients (inliers.
indices, coeff, coeff_refined);
121 model_coefficients.
values.resize (coeff_refined.size ());
122 memcpy (model_coefficients.
values.data(), coeff_refined.data(), coeff_refined.size () * sizeof (
float));
124 model_->selectWithinDistance (coeff_refined, threshold_, inliers.
indices);
128 model_coefficients.
values.resize (coeff.size ());
129 memcpy (model_coefficients.
values.data(), coeff.data(), coeff.size () * sizeof (
float));
136 template <
typename Po
intT>
bool
147 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
153 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
159 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 ());
160 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
162 double min_radius, max_radius;
163 model_->getRadiusLimits (min_radius, max_radius);
164 if (radius_min_ != min_radius && radius_max_ != max_radius)
166 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
167 model_->setRadiusLimits (radius_min_, radius_max_);
173 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
176 double min_radius, max_radius;
178 if (radius_min_ != min_radius && radius_max_ != max_radius)
180 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
187 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
190 double min_radius, max_radius;
192 if (radius_min_ != min_radius && radius_max_ != max_radius)
194 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
201 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
204 double min_radius, max_radius;
206 if (radius_min_ != min_radius && radius_max_ != max_radius)
208 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
215 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
218 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->
getAxis () != axis_)
220 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
221 model_parallel->
setAxis (axis_);
223 if (eps_angle_ != 0.0 && model_parallel->
getEpsAngle () != eps_angle_)
225 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);
232 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
235 if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->
getAxis () != axis_)
237 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
238 model_perpendicular->
setAxis (axis_);
240 if (eps_angle_ != 0.0 && model_perpendicular->
getEpsAngle () != eps_angle_)
242 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);
249 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
252 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->
getAxis () != axis_)
254 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
255 model_parallel->
setAxis (axis_);
257 if (eps_angle_ != 0.0 && model_parallel->
getEpsAngle () != eps_angle_)
259 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);
266 PCL_DEBUG(
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_ELLIPSE3D\n", getClassName().c_str());
269 double min_radius, max_radius;
271 if (radius_min_ != min_radius && radius_max_ != max_radius) {
272 PCL_DEBUG(
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName().c_str(), radius_min_, radius_max_);
283 PCL_ERROR (
"[pcl::%s::initSACModel] Use SACSegmentationFromNormals for this model instead!\n", getClassName ().c_str ());
288 PCL_ERROR (
"[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
296 template <
typename Po
intT>
void
307 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
313 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
319 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
325 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
331 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
337 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
343 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
349 if (sac_->getProbability () != probability_)
351 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
352 sac_->setProbability (probability_);
354 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
356 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
357 sac_->setMaxIterations (max_iterations_);
359 if (samples_radius_ > 0.)
361 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
363 model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
365 if (sac_->getNumberOfThreads () != threads_)
367 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the number of threads to %i\n", getClassName ().c_str (), threads_);
368 sac_->setNumberOfThreads (threads_);
373 template <
typename Po
intT,
typename Po
intNT>
bool
376 if (!input_ || !normals_)
378 PCL_ERROR (
"[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
382 if (input_->size () != normals_->size ())
384 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 ());
396 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
402 double min_radius, max_radius;
404 if (radius_min_ != min_radius && radius_max_ != max_radius)
406 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
411 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
414 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->
getAxis () != axis_)
416 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
417 model_cylinder->
setAxis (axis_);
419 if (eps_angle_ != 0.0 && model_cylinder->
getEpsAngle () != eps_angle_)
421 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);
428 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
435 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
442 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
449 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
454 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
457 if (axis_ != Eigen::Vector3f::Zero () && model_normals->
getAxis () != axis_)
459 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
460 model_normals->
setAxis (axis_);
462 if (eps_angle_ != 0.0 && model_normals->
getEpsAngle () != eps_angle_)
464 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);
471 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
477 double min_angle, max_angle;
479 if (min_angle_ != min_angle && max_angle_ != max_angle)
481 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
487 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
490 if (axis_ != Eigen::Vector3f::Zero () && model_cone->
getAxis () != axis_)
492 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
495 if (eps_angle_ != 0.0 && model_cone->
getEpsAngle () != eps_angle_)
497 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);
504 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
509 double min_radius, max_radius;
511 if (radius_min_ != min_radius && radius_max_ != max_radius)
513 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
519 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
534 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>;
535 #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
SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
shared_ptr< SampleConsensusModelEllipse3D< PointT > > 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
constexpr int SAC_RRANSAC
std::vector< float > values