Point Cloud Library (PCL)  1.8.0
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/pcl_macros.h>
45 #include <pcl/common/common.h>
46 #include <pcl/common/distances.h>
47 #include <limits.h>
48 
49 namespace pcl
50 {
51  /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation.
52  * The model coefficients are defined as:
53  * <ul>
54  * <li><b>apex.x</b> : the X coordinate of cone's apex
55  * <li><b>apex.y</b> : the Y coordinate of cone's apex
56  * <li><b>apex.z</b> : the Z coordinate of cone's apex
57  * <li><b>axis_direction.x</b> : the X coordinate of the cone's axis direction
58  * <li><b>axis_direction.y</b> : the Y coordinate of the cone's axis direction
59  * <li><b>axis_direction.z</b> : the Z coordinate of the cone's axis direction
60  * <li><b>opening_angle</b> : the cone's opening angle
61  * </ul>
62  * \author Stefan Schrandt
63  * \ingroup sample_consensus
64  */
65  template <typename PointT, typename PointNT>
66  class SampleConsensusModelCone : public SampleConsensusModel<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
67  {
68  public:
77 
81 
82  typedef boost::shared_ptr<SampleConsensusModelCone> Ptr;
83 
84  /** \brief Constructor for base SampleConsensusModelCone.
85  * \param[in] cloud the input point cloud dataset
86  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
87  */
88  SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false)
89  : SampleConsensusModel<PointT> (cloud, random)
91  , axis_ (Eigen::Vector3f::Zero ())
92  , eps_angle_ (0)
93  , min_angle_ (-std::numeric_limits<double>::max ())
94  , max_angle_ (std::numeric_limits<double>::max ())
95  , tmp_inliers_ ()
96  {
97  model_name_ = "SampleConsensusModelCone";
98  sample_size_ = 3;
99  model_size_ = 7;
100  }
101 
102  /** \brief Constructor for base SampleConsensusModelCone.
103  * \param[in] cloud the input point cloud dataset
104  * \param[in] indices a vector of point indices to be used from \a cloud
105  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
106  */
107  SampleConsensusModelCone (const PointCloudConstPtr &cloud,
108  const std::vector<int> &indices,
109  bool random = false)
110  : SampleConsensusModel<PointT> (cloud, indices, random)
112  , axis_ (Eigen::Vector3f::Zero ())
113  , eps_angle_ (0)
114  , min_angle_ (-std::numeric_limits<double>::max ())
115  , max_angle_ (std::numeric_limits<double>::max ())
116  , tmp_inliers_ ()
117  {
118  model_name_ = "SampleConsensusModelCone";
119  sample_size_ = 3;
120  model_size_ = 7;
121  }
122 
123  /** \brief Copy constructor.
124  * \param[in] source the model to copy into this
125  */
129  axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
130  {
131  *this = source;
132  model_name_ = "SampleConsensusModelCone";
133  }
134 
135  /** \brief Empty destructor */
137 
138  /** \brief Copy constructor.
139  * \param[in] source the model to copy into this
140  */
143  {
146  axis_ = source.axis_;
147  eps_angle_ = source.eps_angle_;
148  min_angle_ = source.min_angle_;
149  max_angle_ = source.max_angle_;
150  tmp_inliers_ = source.tmp_inliers_;
151  return (*this);
152  }
153 
154  /** \brief Set the angle epsilon (delta) threshold.
155  * \param[in] ea the maximum allowed difference between the cone's axis and the given axis.
156  */
157  inline void
158  setEpsAngle (double ea) { eps_angle_ = ea; }
159 
160  /** \brief Get the angle epsilon (delta) threshold. */
161  inline double
162  getEpsAngle () const { return (eps_angle_); }
163 
164  /** \brief Set the axis along which we need to search for a cone direction.
165  * \param[in] ax the axis along which we need to search for a cone direction
166  */
167  inline void
168  setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
169 
170  /** \brief Get the axis along which we need to search for a cone direction. */
171  inline Eigen::Vector3f
172  getAxis () const { return (axis_); }
173 
174  /** \brief Set the minimum and maximum allowable opening angle for a cone model
175  * given from a user.
176  * \param[in] min_angle the minimum allowable opening angle of a cone model
177  * \param[in] max_angle the maximum allowable opening angle of a cone model
178  */
179  inline void
180  setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
181  {
182  min_angle_ = min_angle;
183  max_angle_ = max_angle;
184  }
185 
186  /** \brief Get the opening angle which we need minimum to validate a cone model.
187  * \param[out] min_angle the minimum allowable opening angle of a cone model
188  * \param[out] max_angle the maximum allowable opening angle of a cone model
189  */
190  inline void
191  getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
192  {
193  min_angle = min_angle_;
194  max_angle = max_angle_;
195  }
196 
197  /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients
198  * from these samples and store them in model_coefficients. The cone coefficients are: apex,
199  * axis_direction, opening_angle.
200  * \param[in] samples the point indices found as possible good candidates for creating a valid model
201  * \param[out] model_coefficients the resultant model coefficients
202  */
203  bool
204  computeModelCoefficients (const std::vector<int> &samples,
205  Eigen::VectorXf &model_coefficients);
206 
207  /** \brief Compute all distances from the cloud data to a given cone model.
208  * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
209  * \param[out] distances the resultant estimated distances
210  */
211  void
212  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
213  std::vector<double> &distances);
214 
215  /** \brief Select all the points which respect the given model coefficients as inliers.
216  * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
217  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
218  * \param[out] inliers the resultant model inliers
219  */
220  void
221  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
222  const double threshold,
223  std::vector<int> &inliers);
224 
225  /** \brief Count all the points which respect the given model coefficients as inliers.
226  *
227  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
228  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
229  * \return the resultant number of inliers
230  */
231  virtual int
232  countWithinDistance (const Eigen::VectorXf &model_coefficients,
233  const double threshold);
234 
235 
236  /** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
237  * @note: these are the coefficients of the cone model after refinement (e.g. after SVD)
238  * \param[in] inliers the data inliers found as supporting the model
239  * \param[in] model_coefficients the initial guess for the optimization
240  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
241  */
242  void
243  optimizeModelCoefficients (const std::vector<int> &inliers,
244  const Eigen::VectorXf &model_coefficients,
245  Eigen::VectorXf &optimized_coefficients);
246 
247 
248  /** \brief Create a new point cloud with inliers projected onto the cone model.
249  * \param[in] inliers the data inliers that we want to project on the cone model
250  * \param[in] model_coefficients the coefficients of a cone model
251  * \param[out] projected_points the resultant projected points
252  * \param[in] copy_data_fields set to true if we need to copy the other data fields
253  */
254  void
255  projectPoints (const std::vector<int> &inliers,
256  const Eigen::VectorXf &model_coefficients,
257  PointCloud &projected_points,
258  bool copy_data_fields = true);
259 
260  /** \brief Verify whether a subset of indices verifies the given cone model coefficients.
261  * \param[in] indices the data indices that need to be tested against the cone model
262  * \param[in] model_coefficients the cone model coefficients
263  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
264  */
265  bool
266  doSamplesVerifyModel (const std::set<int> &indices,
267  const Eigen::VectorXf &model_coefficients,
268  const double threshold);
269 
270  /** \brief Return an unique id for this model (SACMODEL_CONE). */
271  inline pcl::SacModel
272  getModelType () const { return (SACMODEL_CONE); }
273 
274  protected:
277 
278  /** \brief Get the distance from a point to a line (represented by a point and a direction)
279  * \param[in] pt a point
280  * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
281  */
282  double
283  pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients);
284 
285  /** \brief Get a string representation of the name of this class. */
286  PCL_DEPRECATED ("[pcl::SampleConsensusModelCone::getName] getName is deprecated. Please use getClassName instead.")
287  std::string
288  getName () const { return (model_name_); }
289 
290  protected:
291  /** \brief Check whether a model is valid given the user constraints.
292  * \param[in] model_coefficients the set of model coefficients
293  */
294  virtual bool
295  isModelValid (const Eigen::VectorXf &model_coefficients);
296 
297  /** \brief Check if a sample of indices results in a good sample of points
298  * indices. Pure virtual.
299  * \param[in] samples the resultant index samples
300  */
301  bool
302  isSampleGood (const std::vector<int> &samples) const;
303 
304  private:
305  /** \brief The axis along which we need to search for a plane perpendicular to. */
306  Eigen::Vector3f axis_;
307 
308  /** \brief The maximum allowed difference between the plane normal and the given axis. */
309  double eps_angle_;
310 
311  /** \brief The minimum and maximum allowed opening angles of valid cone model. */
312  double min_angle_;
313  double max_angle_;
314 
315  /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */
316  const std::vector<int> *tmp_inliers_;
317 
318 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
319 #pragma GCC diagnostic ignored "-Weffc++"
320 #endif
321  /** \brief Functor for the optimization function */
322  struct OptimizationFunctor : pcl::Functor<float>
323  {
324  /** Functor constructor
325  * \param[in] m_data_points the number of data points to evaluate
326  * \param[in] estimator pointer to the estimator object
327  * \param[in] distance distance computation function pointer
328  */
329  OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCone<PointT, PointNT> *model) :
330  pcl::Functor<float> (m_data_points), model_ (model) {}
331 
332  /** Cost function to be minimized
333  * \param[in] x variables array
334  * \param[out] fvec resultant functions evaluations
335  * \return 0
336  */
337  int
338  operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
339  {
340  Eigen::Vector4f apex (x[0], x[1], x[2], 0);
341  Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
342  float opening_angle = x[6];
343 
344  float apexdotdir = apex.dot (axis_dir);
345  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
346 
347  for (int i = 0; i < values (); ++i)
348  {
349  // dist = f - r
350  Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
351  model_->input_->points[(*model_->tmp_inliers_)[i]].y,
352  model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
353 
354  // Calculate the point's projection on the cone axis
355  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
356  Eigen::Vector4f pt_proj = apex + k * axis_dir;
357 
358  // Calculate the actual radius of the cone at the level of the projected point
359  Eigen::Vector4f height = apex-pt_proj;
360  float actual_cone_radius = tanf (opening_angle) * height.norm ();
361 
362  fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius);
363  }
364  return (0);
365  }
366 
368  };
369 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
370 #pragma GCC diagnostic warning "-Weffc++"
371 #endif
372  };
373 }
374 
375 #ifdef PCL_NO_PRECOMPILE
376 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
377 #endif
378 
379 #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 minimum to validate a cone model.
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
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...
Definition: sac_model.h:653
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.
Definition: bfgs.h:10
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.
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
std::string getName() const
Get a string representation of the name of this class.
SampleConsensusModel< PointT >::PointCloud PointCloud
virtual 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...
Definition: sac_model.h:591
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SacModel
Definition: model_types.h:48
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
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
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572