39 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
40 #define PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
42 #include <pcl/sample_consensus/sac_model.h>
43 #include <pcl/sample_consensus/model_types.h>
44 #include <pcl/common/common.h>
45 #include <pcl/common/distances.h>
64 template <
typename Po
intT,
typename Po
intNT>
80 typedef boost::shared_ptr<SampleConsensusModelCone>
Ptr;
89 , axis_ (
Eigen::Vector3f::Zero ())
91 , min_angle_ (-
std::numeric_limits<double>::max ())
92 , max_angle_ (
std::numeric_limits<double>::max ())
103 const std::vector<int> &indices,
107 , axis_ (
Eigen::Vector3f::Zero ())
109 , min_angle_ (-
std::numeric_limits<double>::max ())
110 , max_angle_ (
std::numeric_limits<double>::max ())
121 axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
136 axis_ = source.axis_;
137 eps_angle_ = source.eps_angle_;
138 min_angle_ = source.min_angle_;
139 max_angle_ = source.max_angle_;
140 tmp_inliers_ = source.tmp_inliers_;
158 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
161 inline Eigen::Vector3f
172 min_angle_ = min_angle;
173 max_angle_ = max_angle;
183 min_angle = min_angle_;
184 max_angle = max_angle_;
195 Eigen::VectorXf &model_coefficients);
203 std::vector<double> &distances);
212 const double threshold,
213 std::vector<int> &inliers);
223 const double threshold);
234 const Eigen::VectorXf &model_coefficients,
235 Eigen::VectorXf &optimized_coefficients);
246 const Eigen::VectorXf &model_coefficients,
247 PointCloud &projected_points,
248 bool copy_data_fields =
true);
257 const Eigen::VectorXf &model_coefficients,
258 const double threshold);
270 pointToAxisDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients);
274 getName ()
const {
return (
"SampleConsensusModelCone"); }
281 isModelValid (
const Eigen::VectorXf &model_coefficients);
292 Eigen::Vector3f axis_;
302 const std::vector<int> *tmp_inliers_;
304 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
305 #pragma GCC diagnostic ignored "-Weffc++"
316 pcl::
Functor<float> (m_data_points), model_ (model) {}
324 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
326 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
327 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
328 float opening_angle = x[6];
330 float apexdotdir = apex.dot (axis_dir);
331 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
333 for (
int i = 0; i < values (); ++i)
336 Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
337 model_->input_->points[(*model_->tmp_inliers_)[i]].y,
338 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
341 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
342 Eigen::Vector4f pt_proj = apex + k * axis_dir;
345 Eigen::Vector4f height = apex-pt_proj;
346 float actual_cone_radius = tanf (opening_angle) * height.norm ();
355 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
356 #pragma GCC diagnostic warning "-Weffc++"
361 #ifdef PCL_NO_PRECOMPILE
362 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
365 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
SampleConsensusModel< PointT >::PointCloud PointCloud
pcl::PointCloud< PointT >::Ptr PointCloudPtr
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the cone coefficients using the given inlier set and return them to the user...
double pointToAxisDistance(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)
std::string getName() const
Get a string representation of the name of this class.
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minumum to validate a cone model.
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
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.
virtual ~SampleConsensusModelCone()
Empty destructor.
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CONE).
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
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 cone model.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cone direction.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
void setEpsAngle(double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
SampleConsensusModel represents the base model class.
bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
boost::shared_ptr< SampleConsensusModelCone > Ptr
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 & operator=(const SampleConsensusModelCone &source)
Copy constructor.
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) ...
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given cone model coefficients.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCone.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SampleConsensusModelCone defines a model for 3D cone segmentation.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
Base functor all the models that need non linear optimization must define their own one and implement...
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given cone model.