41 #include <pcl/sample_consensus/sac_model.h> 42 #include <pcl/sample_consensus/model_types.h> 63 template <
typename Po
intT,
typename Po
intNT>
80 using Ptr = shared_ptr<SampleConsensusModelCone<PointT, PointNT> >;
81 using ConstPtr = shared_ptr<const SampleConsensusModelCone<PointT, PointNT>>;
90 , axis_ (
Eigen::Vector3f::Zero ())
92 , min_angle_ (-std::numeric_limits<double>::max ())
93 , max_angle_ (std::numeric_limits<double>::max ())
110 , axis_ (
Eigen::Vector3f::Zero ())
112 , min_angle_ (-std::numeric_limits<double>::max ())
113 , max_angle_ (std::numeric_limits<double>::max ())
126 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 override;
209 std::vector<double> &distances)
const override;
218 const double threshold,
229 const double threshold)
const override;
240 const Eigen::VectorXf &model_coefficients,
241 Eigen::VectorXf &optimized_coefficients)
const override;
252 const Eigen::VectorXf &model_coefficients,
254 bool copy_data_fields =
true)
const override;
263 const Eigen::VectorXf &model_coefficients,
264 const double threshold)
const override;
279 pointToAxisDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients)
const;
285 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
296 Eigen::Vector3f axis_;
321 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const 323 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
324 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
325 float opening_angle = x[6];
327 float apexdotdir = apex.dot (axis_dir);
328 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
330 for (
int i = 0; i < values (); ++i)
333 Eigen::Vector4f pt = (*model_->input_)[
indices_[i]].getVector4fMap();
337 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
338 Eigen::Vector4f pt_proj = apex + k * axis_dir;
341 Eigen::Vector4f height = apex-pt_proj;
342 float actual_cone_radius = tanf (opening_angle) * height.norm ();
355 #ifdef PCL_NO_PRECOMPILE 356 #include <pcl/sample_consensus/impl/sac_model_cone.hpp> std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelCone.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
~SampleConsensusModelCone()
Empty destructor.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CONE).
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
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...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cone model.
typename PointCloud::Ptr PointCloudPtr
Define standard C methods to do distance calculations.
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) ...
std::string model_name_
The model name.
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the cone model.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
SampleConsensusModelCone defines a model for 3D cone segmentation.
typename PointCloud::ConstPtr PointCloudConstPtr
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)
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
IndicesAllocator<> Indices
Type used for indices in PCL.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
shared_ptr< const SampleConsensusModel< pcl::PointXYZRGB > > ConstPtr
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
IndicesPtr indices_
A pointer to the vector of point indices to use.
A point structure representing Euclidean xyz coordinates, and the RGB color.
shared_ptr< SampleConsensusModel< pcl::PointXYZRGB > > Ptr
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
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 doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given cone model coefficients.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
unsigned int sample_size_
The size of a sample from which the model is computed.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cone coefficients using the given inlier set and return them to the user...