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