Point Cloud Library (PCL)  1.7.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/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:
75 
79 
80  typedef boost::shared_ptr<SampleConsensusModelCone> Ptr;
81 
82  /** \brief Constructor for base SampleConsensusModelCone.
83  * \param[in] cloud the input point cloud dataset
84  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
85  */
86  SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false)
87  : SampleConsensusModel<PointT> (cloud, random)
89  , axis_ (Eigen::Vector3f::Zero ())
90  , eps_angle_ (0)
91  , min_angle_ (-std::numeric_limits<double>::max ())
92  , max_angle_ (std::numeric_limits<double>::max ())
93  , tmp_inliers_ ()
94  {
95  }
96 
97  /** \brief Constructor for base SampleConsensusModelCone.
98  * \param[in] cloud the input point cloud dataset
99  * \param[in] indices a vector of point indices to be used from \a cloud
100  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
101  */
103  const std::vector<int> &indices,
104  bool random = false)
105  : SampleConsensusModel<PointT> (cloud, indices, random)
107  , axis_ (Eigen::Vector3f::Zero ())
108  , eps_angle_ (0)
109  , min_angle_ (-std::numeric_limits<double>::max ())
110  , max_angle_ (std::numeric_limits<double>::max ())
111  , tmp_inliers_ ()
112  {
113  }
114 
115  /** \brief Copy constructor.
116  * \param[in] source the model to copy into this
117  */
121  axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
122  {
123  *this = source;
124  }
125 
126  /** \brief Empty destructor */
128 
129  /** \brief Copy constructor.
130  * \param[in] source the model to copy into this
131  */
134  {
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_;
141  return (*this);
142  }
143 
144  /** \brief Set the angle epsilon (delta) threshold.
145  * \param[in] ea the maximum allowed difference between the cone's axis and the given axis.
146  */
147  inline void
148  setEpsAngle (double ea) { eps_angle_ = ea; }
149 
150  /** \brief Get the angle epsilon (delta) threshold. */
151  inline double
152  getEpsAngle () const { return (eps_angle_); }
153 
154  /** \brief Set the axis along which we need to search for a cone direction.
155  * \param[in] ax the axis along which we need to search for a cone direction
156  */
157  inline void
158  setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
159 
160  /** \brief Get the axis along which we need to search for a cone direction. */
161  inline Eigen::Vector3f
162  getAxis () const { return (axis_); }
163 
164  /** \brief Set the minimum and maximum allowable opening angle for a cone model
165  * given from a user.
166  * \param[in] min_angle the minimum allwoable opening angle of a cone model
167  * \param[in] max_angle the maximum allwoable opening angle of a cone model
168  */
169  inline void
170  setMinMaxOpeningAngle (const double &min_angle, const double &max_angle)
171  {
172  min_angle_ = min_angle;
173  max_angle_ = max_angle;
174  }
175 
176  /** \brief Get the opening angle which we need minumum to validate a cone model.
177  * \param[out] min_angle the minimum allwoable opening angle of a cone model
178  * \param[out] max_angle the maximum allwoable opening angle of a cone model
179  */
180  inline void
181  getMinMaxOpeningAngle (double &min_angle, double &max_angle) const
182  {
183  min_angle = min_angle_;
184  max_angle = max_angle_;
185  }
186 
187  /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients
188  * from these samples and store them in model_coefficients. The cone coefficients are: apex,
189  * axis_direction, opening_angle.
190  * \param[in] samples the point indices found as possible good candidates for creating a valid model
191  * \param[out] model_coefficients the resultant model coefficients
192  */
193  bool
194  computeModelCoefficients (const std::vector<int> &samples,
195  Eigen::VectorXf &model_coefficients);
196 
197  /** \brief Compute all distances from the cloud data to a given cone model.
198  * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
199  * \param[out] distances the resultant estimated distances
200  */
201  void
202  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
203  std::vector<double> &distances);
204 
205  /** \brief Select all the points which respect the given model coefficients as inliers.
206  * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to
207  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
208  * \param[out] inliers the resultant model inliers
209  */
210  void
211  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
212  const double threshold,
213  std::vector<int> &inliers);
214 
215  /** \brief Count all the points which respect the given model coefficients as inliers.
216  *
217  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
218  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
219  * \return the resultant number of inliers
220  */
221  virtual int
222  countWithinDistance (const Eigen::VectorXf &model_coefficients,
223  const double threshold);
224 
225 
226  /** \brief Recompute the cone coefficients using the given inlier set and return them to the user.
227  * @note: these are the coefficients of the cone model after refinement (eg. after SVD)
228  * \param[in] inliers the data inliers found as supporting the model
229  * \param[in] model_coefficients the initial guess for the optimization
230  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
231  */
232  void
233  optimizeModelCoefficients (const std::vector<int> &inliers,
234  const Eigen::VectorXf &model_coefficients,
235  Eigen::VectorXf &optimized_coefficients);
236 
237 
238  /** \brief Create a new point cloud with inliers projected onto the cone model.
239  * \param[in] inliers the data inliers that we want to project on the cone model
240  * \param[in] model_coefficients the coefficients of a cone model
241  * \param[out] projected_points the resultant projected points
242  * \param[in] copy_data_fields set to true if we need to copy the other data fields
243  */
244  void
245  projectPoints (const std::vector<int> &inliers,
246  const Eigen::VectorXf &model_coefficients,
247  PointCloud &projected_points,
248  bool copy_data_fields = true);
249 
250  /** \brief Verify whether a subset of indices verifies the given cone model coefficients.
251  * \param[in] indices the data indices that need to be tested against the cone model
252  * \param[in] model_coefficients the cone model coefficients
253  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
254  */
255  bool
256  doSamplesVerifyModel (const std::set<int> &indices,
257  const Eigen::VectorXf &model_coefficients,
258  const double threshold);
259 
260  /** \brief Return an unique id for this model (SACMODEL_CONE). */
261  inline pcl::SacModel
262  getModelType () const { return (SACMODEL_CONE); }
263 
264  protected:
265  /** \brief Get the distance from a point to a line (represented by a point and a direction)
266  * \param[in] pt a point
267  * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
268  */
269  double
270  pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients);
271 
272  /** \brief Get a string representation of the name of this class. */
273  std::string
274  getName () const { return ("SampleConsensusModelCone"); }
275 
276  protected:
277  /** \brief Check whether a model is valid given the user constraints.
278  * \param[in] model_coefficients the set of model coefficients
279  */
280  bool
281  isModelValid (const Eigen::VectorXf &model_coefficients);
282 
283  /** \brief Check if a sample of indices results in a good sample of points
284  * indices. Pure virtual.
285  * \param[in] samples the resultant index samples
286  */
287  bool
288  isSampleGood (const std::vector<int> &samples) const;
289 
290  private:
291  /** \brief The axis along which we need to search for a plane perpendicular to. */
292  Eigen::Vector3f axis_;
293 
294  /** \brief The maximum allowed difference between the plane normal and the given axis. */
295  double eps_angle_;
296 
297  /** \brief The minimum and maximum allowed opening angles of valid cone model. */
298  double min_angle_;
299  double max_angle_;
300 
301  /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */
302  const std::vector<int> *tmp_inliers_;
303 
304 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
305 #pragma GCC diagnostic ignored "-Weffc++"
306 #endif
307  /** \brief Functor for the optimization function */
308  struct OptimizationFunctor : pcl::Functor<float>
309  {
310  /** Functor constructor
311  * \param[in] m_data_points the number of data points to evaluate
312  * \param[in] estimator pointer to the estimator object
313  * \param[in] distance distance computation function pointer
314  */
315  OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCone<PointT, PointNT> *model) :
316  pcl::Functor<float> (m_data_points), model_ (model) {}
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[(*model_->tmp_inliers_)[i]].x,
337  model_->input_->points[(*model_->tmp_inliers_)[i]].y,
338  model_->input_->points[(*model_->tmp_inliers_)[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  };
355 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
356 #pragma GCC diagnostic warning "-Weffc++"
357 #endif
358  };
359 }
360 
361 #ifdef PCL_NO_PRECOMPILE
362 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
363 #endif
364 
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...
Definition: sac_model.h:618
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.
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.
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
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:556
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