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