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> 74 template <
typename Po
intT>
void 78 inliers.
header = model_coefficients.
header = input_->header;
87 if (!initSACModel (model_type_))
89 PCL_ERROR (
"[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
95 initSAC (method_type_);
97 if (!sac_->computeModel (0))
99 PCL_ERROR (
"[pcl::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
101 inliers.
indices.clear (); model_coefficients.
values.clear ();
106 sac_->getInliers (inliers.
indices);
109 Eigen::VectorXf coeff;
110 sac_->getModelCoefficients (coeff);
113 if (optimize_coefficients_)
115 Eigen::VectorXf coeff_refined;
116 model_->optimizeModelCoefficients (inliers.
indices, coeff, coeff_refined);
117 model_coefficients.
values.resize (coeff_refined.size ());
118 memcpy (&model_coefficients.
values[0], &coeff_refined[0], coeff_refined.size () *
sizeof (float));
120 model_->selectWithinDistance (coeff_refined, threshold_, inliers.
indices);
124 model_coefficients.
values.resize (coeff.size ());
125 memcpy (&model_coefficients.
values[0], &coeff[0], coeff.size () *
sizeof (float));
132 template <
typename Po
intT>
bool 143 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
149 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
155 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_STICK\n", getClassName ().c_str ());
157 double min_radius, max_radius;
158 model_->getRadiusLimits (min_radius, max_radius);
159 if (radius_min_ != min_radius && radius_max_ != max_radius)
161 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
162 model_->setRadiusLimits (radius_min_, radius_max_);
168 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
171 double min_radius, max_radius;
173 if (radius_min_ != min_radius && radius_max_ != max_radius)
175 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
182 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ());
185 double min_radius, max_radius;
187 if (radius_min_ != min_radius && radius_max_ != max_radius)
189 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
196 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
199 double min_radius, max_radius;
201 if (radius_min_ != min_radius && radius_max_ != max_radius)
203 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
210 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
213 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->
getAxis () != axis_)
215 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
216 model_parallel->
setAxis (axis_);
218 if (eps_angle_ != 0.0 && model_parallel->
getEpsAngle () != eps_angle_)
220 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);
227 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
230 if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->
getAxis () != axis_)
232 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
233 model_perpendicular->
setAxis (axis_);
235 if (eps_angle_ != 0.0 && model_perpendicular->
getEpsAngle () != eps_angle_)
237 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);
244 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
247 if (axis_ != Eigen::Vector3f::Zero () && model_parallel->
getAxis () != axis_)
249 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
250 model_parallel->
setAxis (axis_);
252 if (eps_angle_ != 0.0 && model_parallel->
getEpsAngle () != eps_angle_)
254 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);
261 PCL_ERROR (
"[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
269 template <
typename Po
intT>
void 280 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
286 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f\n", getClassName ().c_str (), threshold_);
292 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
298 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
304 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
310 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
316 PCL_DEBUG (
"[pcl::%s::initSAC] Using a method of type: SAC_PROSAC with a model threshold of %f\n", getClassName ().c_str (), threshold_);
322 if (sac_->getProbability () != probability_)
324 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the desired probability to %f\n", getClassName ().c_str (), probability_);
325 sac_->setProbability (probability_);
327 if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
329 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum number of iterations to %d\n", getClassName ().c_str (), max_iterations_);
330 sac_->setMaxIterations (max_iterations_);
332 if (samples_radius_ > 0.)
334 PCL_DEBUG (
"[pcl::%s::initSAC] Setting the maximum sample radius to %f\n", getClassName ().c_str (), samples_radius_);
336 model_->setSamplesMaxDist (samples_radius_, samples_radius_search_);
341 template <
typename Po
intT,
typename Po
intNT>
bool 344 if (!input_ || !normals_)
346 PCL_ERROR (
"[pcl::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ());
350 if (input_->points.size () != normals_->points.size ())
352 PCL_ERROR (
"[pcl::%s::initSACModel] The number of points inthe input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ());
364 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
370 double min_radius, max_radius;
372 if (radius_min_ != min_radius && radius_max_ != max_radius)
374 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
379 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
382 if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->
getAxis () != axis_)
384 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
385 model_cylinder->
setAxis (axis_);
387 if (eps_angle_ != 0.0 && model_cylinder->
getEpsAngle () != eps_angle_)
389 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);
396 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
403 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
410 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
417 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
422 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_);
425 if (axis_ != Eigen::Vector3f::Zero () && model_normals->
getAxis () != axis_)
427 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
428 model_normals->
setAxis (axis_);
430 if (eps_angle_ != 0.0 && model_normals->
getEpsAngle () != eps_angle_)
432 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);
439 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
445 double min_angle, max_angle;
447 if (min_angle_ != min_angle && max_angle_ != max_angle)
449 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_);
455 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
458 if (axis_ != Eigen::Vector3f::Zero () && model_cone->
getAxis () != axis_)
460 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
463 if (eps_angle_ != 0.0 && model_cone->
getEpsAngle () != eps_angle_)
465 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);
472 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
477 double min_radius, max_radius;
479 if (radius_min_ != min_radius && radius_max_ != max_radius)
481 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_);
487 PCL_DEBUG (
"[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_);
502 #define PCL_INSTANTIATE_SACSegmentation(T) template class PCL_EXPORTS pcl::SACSegmentation<T>; 503 #define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class PCL_EXPORTS pcl::SACSegmentationFromNormals<T,NT>; 505 #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.
double getEpsAngle()
Get the angle epsilon (delta) threshold.
boost::shared_ptr< SampleConsensusModelCircle2D > Ptr
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.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
RandomizedMEstimatorSampleConsensus represents an implementation of the RMSAC (Randomized M-estimator...
SampleConsensusModelLine defines a model for 3D line segmentation.
boost::shared_ptr< SampleConsensusModelCircle3D< PointT > > Ptr
Eigen::Vector3f getAxis()
Get 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 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
boost::shared_ptr< SampleConsensusModelSphere > Ptr
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.
std::vector< int > indices
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 ...
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a plane perpendicular to.
double getDistanceFromOrigin()
Get the distance of the plane from the origin.
boost::shared_ptr< SampleConsensusModelCylinder > Ptr
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a plane perpendicular to.
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.
boost::shared_ptr< SampleConsensusModelNormalPlane > Ptr
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
boost::shared_ptr< SampleConsensusModelNormalParallelPlane > Ptr
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...
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
double getEpsAngle()
Get the angle epsilon (delta) threshold.
double getEpsAngle()
Get the angle epsilon (delta) threshold.
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.
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a plane perpendicular to.
SampleConsensusModelStick defines a model for 3D stick segmentation.
boost::shared_ptr< SampleConsensusModelPerpendicularPlane > Ptr
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a line.
boost::shared_ptr< SampleConsensusModelNormalSphere > Ptr
virtual void segment(PointIndices &inliers, ModelCoefficients &model_coefficients)
Base method for segmentation of a model in a PointCloud given by <setInputCloud (),...
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...
double getNormalDistanceWeight()
Get the normal angular distance weight.
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()
Get the angle epsilon (delta) threshold.
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).
boost::shared_ptr< SampleConsensusModelParallelLine > Ptr
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
boost::shared_ptr< SampleConsensusModelParallelPlane > Ptr
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
boost::shared_ptr< SampleConsensusModelCone > Ptr
virtual bool initSACModel(const int model_type)
Initialize the Sample Consensus model and set its parameters.
SampleConsensusModelPerpendicularPlane defines a model for 3D plane segmentation using additional ang...