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/common/common.h>
45 #include <pcl/common/distances.h>
64 template <
typename Po
intT,
typename Po
intNT>
81 typedef boost::shared_ptr<SampleConsensusModelCone>
Ptr;
90 , axis_ (
Eigen::Vector3f::Zero ())
92 , min_angle_ (-
std::numeric_limits<double>::max ())
93 , max_angle_ (
std::numeric_limits<double>::max ())
106 const std::vector<int> &indices,
110 , axis_ (
Eigen::Vector3f::Zero ())
112 , min_angle_ (-
std::numeric_limits<double>::max ())
113 , max_angle_ (
std::numeric_limits<double>::max ())
126 axis_ (), eps_angle_ (), min_angle_ (), max_angle_ ()
143 axis_ = source.axis_;
144 eps_angle_ = source.eps_angle_;
145 min_angle_ = source.min_angle_;
146 max_angle_ = source.max_angle_;
164 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
167 inline Eigen::Vector3f
178 min_angle_ = min_angle;
179 max_angle_ = max_angle;
189 min_angle = min_angle_;
190 max_angle = max_angle_;
201 Eigen::VectorXf &model_coefficients)
const;
209 std::vector<double> &distances)
const;
218 const double threshold,
219 std::vector<int> &inliers);
229 const double threshold)
const;
240 const Eigen::VectorXf &model_coefficients,
241 Eigen::VectorXf &optimized_coefficients)
const;
252 const Eigen::VectorXf &model_coefficients,
253 PointCloud &projected_points,
254 bool copy_data_fields =
true)
const;
263 const Eigen::VectorXf &model_coefficients,
264 const double threshold)
const;
279 pointToAxisDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients)
const;
285 isModelValid (
const Eigen::VectorXf &model_coefficients)
const;
296 Eigen::Vector3f axis_;
305 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
306 #pragma GCC diagnostic ignored "-Weffc++"
324 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
326 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
327 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
328 float opening_angle = x[6];
330 float apexdotdir = apex.dot (axis_dir);
331 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
333 for (
int i = 0; i < values (); ++i)
336 Eigen::Vector4f pt (model_->input_->points[
indices_[i]].x,
337 model_->input_->points[
indices_[i]].y,
338 model_->input_->points[
indices_[i]].z, 0);
341 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
342 Eigen::Vector4f pt_proj = apex + k * axis_dir;
345 Eigen::Vector4f height = apex-pt_proj;
346 float actual_cone_radius = tanf (opening_angle) * height.norm ();
356 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
357 #pragma GCC diagnostic warning "-Weffc++"
362 #ifdef PCL_NO_PRECOMPILE
363 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
366 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const
Create a new point cloud with inliers projected onto the cone model.
virtual ~SampleConsensusModelCone()
Empty destructor.
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
This file defines compatibility wrappers for low level I/O functions.
unsigned int model_size_
The number of coefficients in the model.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
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...
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the cone coefficients using the given inlier set and return them to the user...
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given cone model.
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CONE).
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
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
SampleConsensusModel< PointT >::PointCloud PointCloud
SampleConsensusModelCone defines a model for 3D cone segmentation.
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
Verify whether a subset of indices verifies the given cone model coefficients.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
double pointToAxisDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
Get the distance from a point to a line (represented by a point and a direction)
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...
boost::shared_ptr< SampleConsensusModelCone > Ptr
unsigned int sample_size_
The size of a sample from which the model is computed.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.