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> 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.
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.
This file defines compatibility wrappers for low level I/O functions.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
unsigned int model_size_
The number of coefficients in the model.
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...
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.
Define standard C methods to do distance calculations.
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.
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
SampleConsensusModelCone defines a model for 3D cone segmentation.
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)
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given cone model.
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.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
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...
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...
boost::shared_ptr< SampleConsensusModelCone > Ptr
unsigned int sample_size_
The size of a sample from which the model is computed.