Point Cloud Library (PCL)
1.11.1-dev
|
41 #include <pcl/sample_consensus/sac_model.h>
42 #include <pcl/sample_consensus/model_types.h>
61 template <
typename Po
intT,
typename Po
intNT>
78 using Ptr = shared_ptr<SampleConsensusModelCone<PointT, PointNT> >;
79 using ConstPtr = shared_ptr<const SampleConsensusModelCone<PointT, PointNT>>;
88 , axis_ (
Eigen::Vector3f::Zero ())
90 , min_angle_ (-std::numeric_limits<double>::max ())
91 , max_angle_ (std::numeric_limits<double>::max ())
108 , axis_ (
Eigen::Vector3f::Zero ())
110 , min_angle_ (-std::numeric_limits<double>::max ())
111 , max_angle_ (std::numeric_limits<double>::max ())
124 eps_angle_ (), min_angle_ (), max_angle_ ()
141 axis_ = source.axis_;
142 eps_angle_ = source.eps_angle_;
143 min_angle_ = source.min_angle_;
144 max_angle_ = source.max_angle_;
162 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
165 inline Eigen::Vector3f
176 min_angle_ = min_angle;
177 max_angle_ = max_angle;
187 min_angle = min_angle_;
188 max_angle = max_angle_;
199 Eigen::VectorXf &model_coefficients)
const override;
207 std::vector<double> &distances)
const override;
216 const double threshold,
227 const double threshold)
const override;
238 const Eigen::VectorXf &model_coefficients,
239 Eigen::VectorXf &optimized_coefficients)
const override;
250 const Eigen::VectorXf &model_coefficients,
252 bool copy_data_fields =
true)
const override;
261 const Eigen::VectorXf &model_coefficients,
262 const double threshold)
const override;
277 pointToAxisDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients)
const;
283 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
294 Eigen::Vector3f axis_;
319 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
321 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
322 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
323 float opening_angle = x[6];
325 float apexdotdir = apex.dot (axis_dir);
326 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
328 for (
int i = 0; i < values (); ++i)
331 Eigen::Vector4f pt = (*model_->input_)[
indices_[i]].getVector4fMap();
335 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
336 Eigen::Vector4f pt_proj = apex + k * axis_dir;
339 Eigen::Vector4f height = apex-pt_proj;
340 float actual_cone_radius = tanf (opening_angle) * height.norm ();
353 #ifdef PCL_NO_PRECOMPILE
354 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
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.
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.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
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 getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CONE).
~SampleConsensusModelCone()
Empty destructor.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
unsigned int sample_size_
The size of a sample from which the model is computed.
unsigned int model_size_
The number of coefficients in the model.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 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 override
Compute all distances from the cloud data to a given cone model.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Base functor all the models that need non linear optimization must define their own one and implement...
IndicesPtr indices_
A pointer to the vector of point indices to use.
shared_ptr< const SampleConsensusModel< pcl::PointXYZRGB > > ConstPtr
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModelCone defines a model for 3D cone segmentation.
shared_ptr< SampleConsensusModel< pcl::PointXYZRGB > > Ptr
std::string model_name_
The model name.
IndicesAllocator<> Indices
Type used for indices in PCL.
typename PointCloud::ConstPtr PointCloudConstPtr
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
typename PointCloud::Ptr PointCloudPtr
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
SampleConsensusModel represents the base model class.
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.
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...
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 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.
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.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelCone.