39 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_ 40 #define PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_ 42 #include <pcl/sample_consensus/sac_model.h> 43 #include <pcl/sample_consensus/model_types.h> 44 #include <pcl/pcl_macros.h> 65 template <
typename Po
intT,
typename Po
intNT>
82 typedef boost::shared_ptr<SampleConsensusModelCone>
Ptr;
91 , axis_ (
Eigen::Vector3f::Zero ())
93 , min_angle_ (-
std::numeric_limits<double>::max ())
94 , max_angle_ (
std::numeric_limits<double>::max ())
108 const std::vector<int> &indices,
112 , axis_ (
Eigen::Vector3f::Zero ())
114 , min_angle_ (-
std::numeric_limits<double>::max ())
115 , max_angle_ (
std::numeric_limits<double>::max ())
129 axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
146 axis_ = source.axis_;
147 eps_angle_ = source.eps_angle_;
148 min_angle_ = source.min_angle_;
149 max_angle_ = source.max_angle_;
150 tmp_inliers_ = source.tmp_inliers_;
168 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
171 inline Eigen::Vector3f
182 min_angle_ = min_angle;
183 max_angle_ = max_angle;
193 min_angle = min_angle_;
194 max_angle = max_angle_;
205 Eigen::VectorXf &model_coefficients);
213 std::vector<double> &distances);
222 const double threshold,
223 std::vector<int> &inliers);
233 const double threshold);
244 const Eigen::VectorXf &model_coefficients,
245 Eigen::VectorXf &optimized_coefficients);
256 const Eigen::VectorXf &model_coefficients,
258 bool copy_data_fields =
true);
267 const Eigen::VectorXf &model_coefficients,
268 const double threshold);
283 pointToAxisDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients);
286 PCL_DEPRECATED (
"[pcl::SampleConsensusModelCone::getName] getName is deprecated. Please use getClassName instead.")
295 isModelValid (
const Eigen::VectorXf &model_coefficients);
306 Eigen::Vector3f axis_;
316 const std::vector<int> *tmp_inliers_;
318 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 319 #pragma GCC diagnostic ignored "-Weffc++" 330 pcl::
Functor<float> (m_data_points), model_ (model) {}
338 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const 340 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
341 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
342 float opening_angle = x[6];
344 float apexdotdir = apex.dot (axis_dir);
345 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
347 for (
int i = 0; i < values (); ++i)
350 Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
351 model_->input_->points[(*model_->tmp_inliers_)[i]].y,
352 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
355 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
356 Eigen::Vector4f pt_proj = apex + k * axis_dir;
359 Eigen::Vector4f height = apex-pt_proj;
360 float actual_cone_radius = tanf (opening_angle) * height.norm ();
369 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 370 #pragma GCC diagnostic warning "-Weffc++" 375 #ifdef PCL_NO_PRECOMPILE 376 #include <pcl/sample_consensus/impl/sac_model_cone.hpp> 379 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_ bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
virtual ~SampleConsensusModelCone()
Empty destructor.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
unsigned int model_size_
The number of coefficients in the model.
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
Base functor all the models that need non linear optimization must define their own one and implement...
double pointToAxisDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients)
Get the distance from a point to a line (represented by a point and a direction)
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the cone coefficients using the given inlier set and return them to the user...
Define standard C methods to do distance calculations.
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given cone model coefficients.
Define standard C methods and C++ classes that are common to all methods.
SampleConsensusModel represents the base model class.
double sqrPointToLineDistance(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
Get the square distance from a point to a line (represented by a point and a direction) ...
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
std::string model_name_
The model name.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
SampleConsensusModel< PointT >::PointCloud PointCloud
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given cone model.
SampleConsensusModelCone defines a model for 3D cone segmentation.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the cone model.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
std::string getName() const
Get a string representation of the name of this class.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CONE).
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCone.
SampleConsensusModelCone & operator=(const SampleConsensusModelCone &source)
Copy constructor.
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...
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
boost::shared_ptr< SampleConsensusModelCone > Ptr
unsigned int sample_size_
The size of a sample from which the model is computed.