43#include <pcl/sample_consensus/sac_model.h>
44#include <pcl/sample_consensus/model_types.h>
62 template <
typename Po
intT,
typename Po
intNT>
79 using Ptr = shared_ptr<SampleConsensusModelCylinder<PointT, PointNT> >;
80 using ConstPtr = shared_ptr<const SampleConsensusModelCylinder<PointT, PointNT>>;
89 , axis_ (
Eigen::Vector3f::Zero ())
107 , axis_ (
Eigen::Vector3f::Zero ())
121 axis_ (
Eigen::Vector3f::Zero ()),
139 axis_ = source.axis_;
140 eps_angle_ = source.eps_angle_;
158 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
161 inline Eigen::Vector3f
172 Eigen::VectorXf &model_coefficients)
const override;
180 std::vector<double> &distances)
const override;
189 const double threshold,
200 const double threshold)
const override;
210 const Eigen::VectorXf &model_coefficients,
211 Eigen::VectorXf &optimized_coefficients)
const override;
222 const Eigen::VectorXf &model_coefficients,
224 bool copy_data_fields =
true)
const override;
233 const Eigen::VectorXf &model_coefficients,
234 const double threshold)
const override;
249 pointToLineDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients)
const;
259 const Eigen::Vector4f &line_pt,
260 const Eigen::Vector4f &line_dir,
261 Eigen::Vector4f &pt_proj)
const
263 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
265 pt_proj = line_pt + k * line_dir;
276 const Eigen::VectorXf &model_coefficients,
277 Eigen::Vector4f &pt_proj)
const;
283 isModelValid (
const Eigen::VectorXf &model_coefficients)
const override;
294 Eigen::Vector3f axis_;
307 pcl::
Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
315 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
317 Eigen::Vector4f line_pt (x[0], x[1], x[2], 0);
318 Eigen::Vector4f line_dir (x[3], x[4], x[5], 0);
320 for (
int i = 0; i <
values (); ++i)
323 Eigen::Vector4f pt = (*model_->input_)[indices_[i]].getVector4fMap();
337#ifdef PCL_NO_PRECOMPILE
338#include <pcl/sample_consensus/impl/sac_model_cylinder.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelCylinder.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_CYLINDER).
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cylinder model.
SampleConsensusModelCylinder(const SampleConsensusModelCylinder &source)
Copy constructor.
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 cylinder model.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void projectPointToLine(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, Eigen::Vector4f &pt_proj) const
Project a point onto a line given by a point and a direction vector.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cylinder coefficients using the given inlier set and return them to the user.
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 projectPointToCylinder(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const
Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction,...
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.
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCylinder.
~SampleConsensusModelCylinder()
Empty destructor.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelCylinder & operator=(const SampleConsensusModelCylinder &source)
Copy constructor.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
shared_ptr< const SampleConsensusModelCylinder< PointT, PointNT > > ConstPtr
typename SampleConsensusModel< PointT >::PointCloud PointCloud
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cylinder direction.
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 cylinder model coefficients.
double pointToLineDistance(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)
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cylinder model, compute the model coefficients...
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
shared_ptr< SampleConsensusModelCylinder< PointT, PointNT > > Ptr
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
SampleConsensusModel represents the base model class.
double radius_min_
The minimum and maximum radius limits for the model.
unsigned int sample_size_
The size of a sample from which the model is computed.
typename PointCloud::ConstPtr PointCloudConstPtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
std::string model_name_
The model name.
unsigned int model_size_
The number of coefficients in the model.
typename PointCloud::Ptr PointCloudPtr
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Define standard C methods to do distance calculations.
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)
IndicesAllocator<> Indices
Type used for indices in PCL.
Base functor all the models that need non linear optimization must define their own one and implement...
int values() const
Get the number of values.
A point structure representing Euclidean xyz coordinates, and the RGB color.