Point Cloud Library (PCL)  1.9.1
sac_model_cone.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
40 #define PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
41 
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>
46 #include <limits.h>
47 
48 namespace pcl
49 {
50  /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation.
51  * The model coefficients are defined as:
52  * <ul>
53  * <li><b>apex.x</b> : the X coordinate of cone's apex
54  * <li><b>apex.y</b> : the Y coordinate of cone's apex
55  * <li><b>apex.z</b> : the Z coordinate of cone's apex
56  * <li><b>axis_direction.x</b> : the X coordinate of the cone's axis direction
57  * <li><b>axis_direction.y</b> : the Y coordinate of the cone's axis direction
58  * <li><b>axis_direction.z</b> : the Z coordinate of the cone's axis direction
59  * <li><b>opening_angle</b> : the cone's opening angle
60  * </ul>
61  * \author Stefan Schrandt
62  * \ingroup sample_consensus
63  */
64  template <typename PointT, typename PointNT>
65  class SampleConsensusModelCone : public SampleConsensusModel<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
66  {
67  public:
76 
80 
81  typedef boost::shared_ptr<SampleConsensusModelCone> Ptr;
82 
83  /** \brief Constructor for base SampleConsensusModelCone.
84  * \param[in] cloud the input point cloud dataset
85  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
86  */
87  SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false)
88  : SampleConsensusModel<PointT> (cloud, random)
90  , axis_ (Eigen::Vector3f::Zero ())
91  , eps_angle_ (0)
92  , min_angle_ (-std::numeric_limits<double>::max ())
93  , max_angle_ (std::numeric_limits<double>::max ())
94  {
95  model_name_ = "SampleConsensusModelCone";
96  sample_size_ = 3;
97  model_size_ = 7;
98  }
99 
100  /** \brief Constructor for base SampleConsensusModelCone.
101  * \param[in] cloud the input point cloud dataset
102  * \param[in] indices a vector of point indices to be used from \a cloud
103  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
104  */
106  const std::vector<int> &indices,
107  bool random = false)
108  : SampleConsensusModel<PointT> (cloud, indices, random)
110  , axis_ (Eigen::Vector3f::Zero ())
111  , eps_angle_ (0)
112  , min_angle_ (-std::numeric_limits<double>::max ())
113  , max_angle_ (std::numeric_limits<double>::max ())
114  {
115  model_name_ = "SampleConsensusModelCone";
116  sample_size_ = 3;
117  model_size_ = 7;
118  }
119 
120  /** \brief Copy constructor.
121  * \param[in] source the model to copy into this
122  */
126  axis_ (), eps_angle_ (), min_angle_ (), max_angle_ ()
127  {
128  *this = source;
129  model_name_ = "SampleConsensusModelCone";
130  }
131 
132  /** \brief Empty destructor */
134 
135  /** \brief Copy constructor.
136  * \param[in] source the model to copy into this
137  */
140  {
143  axis_ = source.axis_;
144  eps_angle_ = source.eps_angle_;
145  min_angle_ = source.min_angle_;
146  max_angle_ = source.max_angle_;
147  return (*this);
148  }
149 
150  /** \brief Set the angle epsilon (delta) threshold.
151  * \param[in] ea the maximum allowed difference between the cone's axis and the given axis.
152  */
153  inline void
154  setEpsAngle (double ea) { eps_angle_ = ea; }
155 
156  /** \brief Get the angle epsilon (delta) threshold. */
157  inline double
158  getEpsAngle () const { return (eps_angle_); }
159 
160  /** \brief Set the axis along which we need to search for a cone direction.
161  * \param[in] ax the axis along which we need to search for a cone direction
162  */
163  inline void
164  setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
165 
166  /** \brief Get the axis along which we need to search for a cone direction. */
167  inline Eigen::Vector3f
168  getAxis () const { return (axis_); }
169 
170  /** \brief Set the minimum and maximum allowable opening angle for a cone model
171  * given from a user.
172  * \param[in] min_angle the minimum allowable opening angle of a cone model
173  * \param[in] max_angle the maximum allowable opening angle of a cone model
174  */
175  inline void
176  setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
177  {
178  min_angle_ = min_angle;
179  max_angle_ = max_angle;
180  }
181 
182  /** \brief Get the opening angle which we need minimum to validate a cone model.
183  * \param[out] min_angle the minimum allowable opening angle of a cone model
184  * \param[out] max_angle the maximum allowable opening angle of a cone model
185  */
186  inline void
187  getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
188  {
189  min_angle = min_angle_;
190  max_angle = max_angle_;
191  }
192 
193  /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients
194  * from these samples and store them in model_coefficients. The cone coefficients are: apex,
195  * axis_direction, opening_angle.
196  * \param[in] samples the point indices found as possible good candidates for creating a valid model
197  * \param[out] model_coefficients the resultant model coefficients
198  */
199  bool
200  computeModelCoefficients (const std::vector<int> &samples,
201  Eigen::VectorXf &model_coefficients) const;
202 
203  /** \brief Compute all distances from the cloud data to a given cone model.
204  * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
205  * \param[out] distances the resultant estimated distances
206  */
207  void
208  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
209  std::vector<double> &distances) const;
210 
211  /** \brief Select all the points which respect the given model coefficients as inliers.
212  * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
213  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
214  * \param[out] inliers the resultant model inliers
215  */
216  void
217  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
218  const double threshold,
219  std::vector<int> &inliers);
220 
221  /** \brief Count all the points which respect the given model coefficients as inliers.
222  *
223  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
224  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
225  * \return the resultant number of inliers
226  */
227  virtual int
228  countWithinDistance (const Eigen::VectorXf &model_coefficients,
229  const double threshold) const;
230 
231 
232  /** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
233  * @note: these are the coefficients of the cone model after refinement (e.g. after SVD)
234  * \param[in] inliers the data inliers found as supporting the model
235  * \param[in] model_coefficients the initial guess for the optimization
236  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
237  */
238  void
239  optimizeModelCoefficients (const std::vector<int> &inliers,
240  const Eigen::VectorXf &model_coefficients,
241  Eigen::VectorXf &optimized_coefficients) const;
242 
243 
244  /** \brief Create a new point cloud with inliers projected onto the cone model.
245  * \param[in] inliers the data inliers that we want to project on the cone model
246  * \param[in] model_coefficients the coefficients of a cone model
247  * \param[out] projected_points the resultant projected points
248  * \param[in] copy_data_fields set to true if we need to copy the other data fields
249  */
250  void
251  projectPoints (const std::vector<int> &inliers,
252  const Eigen::VectorXf &model_coefficients,
253  PointCloud &projected_points,
254  bool copy_data_fields = true) const;
255 
256  /** \brief Verify whether a subset of indices verifies the given cone model coefficients.
257  * \param[in] indices the data indices that need to be tested against the cone model
258  * \param[in] model_coefficients the cone model coefficients
259  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
260  */
261  bool
262  doSamplesVerifyModel (const std::set<int> &indices,
263  const Eigen::VectorXf &model_coefficients,
264  const double threshold) const;
265 
266  /** \brief Return an unique id for this model (SACMODEL_CONE). */
267  inline pcl::SacModel
268  getModelType () const { return (SACMODEL_CONE); }
269 
270  protected:
273 
274  /** \brief Get the distance from a point to a line (represented by a point and a direction)
275  * \param[in] pt a point
276  * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
277  */
278  double
279  pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const;
280 
281  /** \brief Check whether a model is valid given the user constraints.
282  * \param[in] model_coefficients the set of model coefficients
283  */
284  virtual bool
285  isModelValid (const Eigen::VectorXf &model_coefficients) const;
286 
287  /** \brief Check if a sample of indices results in a good sample of points
288  * indices. Pure virtual.
289  * \param[in] samples the resultant index samples
290  */
291  bool
292  isSampleGood (const std::vector<int> &samples) const;
293 
294  private:
295  /** \brief The axis along which we need to search for a plane perpendicular to. */
296  Eigen::Vector3f axis_;
297 
298  /** \brief The maximum allowed difference between the plane normal and the given axis. */
299  double eps_angle_;
300 
301  /** \brief The minimum and maximum allowed opening angles of valid cone model. */
302  double min_angle_;
303  double max_angle_;
304 
305 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
306 #pragma GCC diagnostic ignored "-Weffc++"
307 #endif
308  /** \brief Functor for the optimization function */
309  struct OptimizationFunctor : pcl::Functor<float>
310  {
311  /** Functor constructor
312  * \param[in] indices the indices of data points to evaluate
313  * \param[in] estimator pointer to the estimator object
314  */
315  OptimizationFunctor (const pcl::SampleConsensusModelCone<PointT, PointNT> *model, const std::vector<int>& indices) :
316  pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
317 
318  /** Cost function to be minimized
319  * \param[in] x variables array
320  * \param[out] fvec resultant functions evaluations
321  * \return 0
322  */
323  int
324  operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
325  {
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];
329 
330  float apexdotdir = apex.dot (axis_dir);
331  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
332 
333  for (int i = 0; i < values (); ++i)
334  {
335  // dist = f - r
336  Eigen::Vector4f pt (model_->input_->points[indices_[i]].x,
337  model_->input_->points[indices_[i]].y,
338  model_->input_->points[indices_[i]].z, 0);
339 
340  // Calculate the point's projection on the cone axis
341  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
342  Eigen::Vector4f pt_proj = apex + k * axis_dir;
343 
344  // Calculate the actual radius of the cone at the level of the projected point
345  Eigen::Vector4f height = apex-pt_proj;
346  float actual_cone_radius = tanf (opening_angle) * height.norm ();
347 
348  fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius);
349  }
350  return (0);
351  }
352 
354  const std::vector<int> &indices_;
355  };
356 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
357 #pragma GCC diagnostic warning "-Weffc++"
358 #endif
359  };
360 }
361 
362 #ifdef PCL_NO_PRECOMPILE
363 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
364 #endif
365 
366 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const
Create a new point cloud with inliers projected onto the cone model.
void getMinMaxOpeningAngle(double &min_angle, double &max_angle) const
Get the opening angle which we need minimum to validate a cone model.
virtual ~SampleConsensusModelCone()
Empty destructor.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the cone coefficients using the given inlier set and return them to the user.
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...
Definition: sac_model.h:653
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:540
Definition: bfgs.h:10
Define standard C methods to do distance calculations.
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
Verify whether a subset of indices verifies the given cone model coefficients.
Define standard C methods and C++ classes that are common to all methods.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
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)
Definition: distances.h:69
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.
std::string model_name_
The model name.
Definition: sac_model.h:534
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
SampleConsensusModel< PointT >::PointCloud PointCloud
SampleConsensusModelCone defines a model for 3D cone segmentation.
double pointToAxisDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
Get the distance from a point to a line (represented by a point and a direction)
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given cone model.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:591
PointCloud represents the base class in PCL for storing collections of 3D points.
SacModel
Definition: model_types.h:46
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a cone direction.
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
SampleConsensusModelCone(const SampleConsensusModelCone &source)
Copy constructor.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModelCone(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCone.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CONE).
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.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
boost::shared_ptr< SampleConsensusModelCone > Ptr
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572