41 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/sample_consensus/model_types.h>
46 #include <pcl/common/common.h>
47 #include <pcl/common/distances.h>
64 template <
typename Po
intT,
typename Po
intNT>
80 typedef boost::shared_ptr<SampleConsensusModelCylinder>
Ptr;
89 , axis_ (
Eigen::Vector3f::Zero ())
101 const std::vector<int> &indices,
105 , axis_ (
Eigen::Vector3f::Zero ())
117 axis_ (
Eigen::Vector3f::Zero ()),
134 axis_ = source.axis_;
135 eps_angle_ = source.eps_angle_;
136 tmp_inliers_ = source.tmp_inliers_;
154 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
157 inline Eigen::Vector3f
168 Eigen::VectorXf &model_coefficients);
176 std::vector<double> &distances);
185 const double threshold,
186 std::vector<int> &inliers);
196 const double threshold);
206 const Eigen::VectorXf &model_coefficients,
207 Eigen::VectorXf &optimized_coefficients);
218 const Eigen::VectorXf &model_coefficients,
219 PointCloud &projected_points,
220 bool copy_data_fields =
true);
229 const Eigen::VectorXf &model_coefficients,
230 const double threshold);
242 pointToLineDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients);
252 const Eigen::Vector4f &line_pt,
253 const Eigen::Vector4f &line_dir,
254 Eigen::Vector4f &pt_proj)
256 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
258 pt_proj = line_pt + k * line_dir;
269 const Eigen::VectorXf &model_coefficients,
270 Eigen::Vector4f &pt_proj);
274 getName ()
const {
return (
"SampleConsensusModelCylinder"); }
281 isModelValid (
const Eigen::VectorXf &model_coefficients);
292 Eigen::Vector3f axis_;
298 const std::vector<int> *tmp_inliers_;
300 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
301 #pragma GCC diagnostic ignored "-Weffc++"
312 pcl::
Functor<float> (m_data_points), model_ (model) {}
320 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
322 Eigen::Vector4f line_pt (x[0], x[1], x[2], 0);
323 Eigen::Vector4f line_dir (x[3], x[4], x[5], 0);
325 for (
int i = 0; i < values (); ++i)
328 Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
329 model_->input_->points[(*model_->tmp_inliers_)[i]].y,
330 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
339 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
340 #pragma GCC diagnostic warning "-Weffc++"
345 #ifdef PCL_NO_PRECOMPILE
346 #include <pcl/sample_consensus/impl/sac_model_cylinder.hpp>
349 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_
SampleConsensusModel< PointT >::PointCloud PointCloud
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given cylinder model.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
std::string getName() const
Get a string representation of the name of this class.
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a cylinder direction.
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCylinder.
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the cylinder coefficients using the given inlier set and return them to the user...
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given cylinder model coefficients.
Base functor all the models that need non linear optimization must define their own one and implement...
SampleConsensusModelCylinder & operator=(const SampleConsensusModelCylinder &source)
Copy constructor.
boost::shared_ptr< SampleConsensusModelCylinder > Ptr
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid cylinder model, compute the model coefficients...
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
double pointToLineDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients)
Get the distance from a point to a line (represented by a point and a direction)
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CYLINDER).
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 projectPointToLine(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, Eigen::Vector4f &pt_proj)
Project a point onto a line given by a point and a direction vector.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
SampleConsensusModelCylinder(const SampleConsensusModelCylinder &source)
Copy constructor.
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. ...
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
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.
void projectPointToCylinder(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj)
Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, cylinder_radius_R)
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual ~SampleConsensusModelCylinder()
Empty destructor.
double getEpsAngle()
Get the angle epsilon (delta) threshold.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the cylinder model.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCylinder.