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_ void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
void setEpsAngle(const 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.
shared_ptr< SampleConsensusModelSphere< PointT > > Ptr
Defines functions, macros and traits for allocating and using memory.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator...
shared_ptr< SampleConsensusModelNormalSphere< PointT, PointNT > > Ptr
SampleConsensusModelLine defines a model for 3D line segmentation.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a line.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelParallelPlane defines a model for 3D plane segmentation using additional angular ...
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.
SACSegmentation represents the Nodelet segmentation class for Sample Consensus methods and models...
std::vector< float > values
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm...
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
MEstimatorSampleConsensus represents an implementation of the MSAC (M-estimator SAmple Consensus) alg...
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelNormalSphere defines a model for 3D sphere segmentation using additional surface ...
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelCircle3D defines a model for 3D circle segmentation.
RandomizedRandomSampleConsensus represents an implementation of the RRANSAC (Randomized RAndom SAmple...
SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane...
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
virtual bool initSACModel(const int model_type)
Initialize the Sample Consensus model and set its parameters.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
shared_ptr< SampleConsensusModelCylinder< PointT, PointNT > > Ptr
double getNormalDistanceWeight() const
Get the normal angular distance weight.
shared_ptr< SampleConsensusModelParallelPlane< PointT > > Ptr
shared_ptr< SampleConsensusModelCone< PointT, PointNT > > Ptr
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular co...
MaximumLikelihoodSampleConsensus represents an implementation of the MLESAC (Maximum Likelihood Estim...
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
shared_ptr< SampleConsensusModelNormalPlane< PointT, PointNT > > Ptr
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
SampleConsensusModelCone defines a model for 3D cone segmentation.
SampleConsensusModelStick defines a model for 3D stick segmentation.
shared_ptr< SampleConsensusModelPerpendicularPlane< PointT > > Ptr
shared_ptr< SampleConsensusModelParallelLine< PointT > > Ptr
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
shared_ptr< SampleConsensusModelCircle3D< 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 cylinder direction.
virtual void segment(PointIndices &inliers, ModelCoefficients &model_coefficients)
Base method for segmentation of a model in a PointCloud given by <setInputCloud (), setIndices ()>
bool initSACModel(const int model_type) override
Initialize the Sample Consensus model and set its parameters.
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 setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
virtual void initSAC(const int method_type)
Initialize the Sample Consensus method and set its parameters.
RandomSampleConsensus represents an implementation of the RANSAC (RANdom SAmple Consensus) algorithm...
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
shared_ptr< SampleConsensusModelNormalParallelPlane< 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...
SampleConsensusModelPlane defines a model for 3D plane segmentation.
double getEpsAngle() const
Get the angle epsilon (delta) threshold (in radians).
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
shared_ptr< SampleConsensusModelCircle2D< PointT > > Ptr
SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional ang...