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_
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.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
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.
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a cylinder direction.
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...
bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
boost::shared_ptr< SampleConsensusModelCylinder > Ptr
SampleConsensusModel< PointT >::PointCloud PointCloud
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)
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
SampleConsensusModelCylinder & operator=(const SampleConsensusModelCylinder &source)
Copy constructor.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
std::string getName() const
Get a string representation of the name of this class.
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CYLINDER).
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)
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.
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCylinder.
SampleConsensusModelCylinder(const SampleConsensusModelCylinder &source)
Copy constructor.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
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...
SampleConsensusModel represents the base model class.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCylinder.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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) ...
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
double getEpsAngle()
Get the angle epsilon (delta) threshold.
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
virtual ~SampleConsensusModelCylinder()
Empty destructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Base functor all the models that need non linear optimization must define their own one and implement...
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given cylinder model.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
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.