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,
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...
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.