Point Cloud Library (PCL)  1.8.1
sac_model.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_H_
39 #define PCL_CUDA_SAMPLE_CONSENSUS_MODEL_H_
40 
41 #include <float.h>
42 #include <thrust/sequence.h>
43 #include <thrust/count.h>
44 #include <thrust/remove.h>
45 #include <pcl/cuda/point_cloud.h>
46 #include <thrust/random/linear_congruential_engine.h>
47 
48 #include <pcl/pcl_exports.h>
49 
50 namespace pcl
51 {
52  namespace cuda
53  {
54  // Forward declaration
55  //template<class T> class ProgressiveSampleConsensus;
56 
57  /** \brief Check if a certain tuple is a point inlier. */
59  {
60  template <typename Tuple> __inline__ __host__ __device__ int
61  operator () (const Tuple &t);
62  };
63 
64  /** \brief Check if a certain tuple is a point inlier. */
65  struct isInlier
66  {
67  __inline__ __host__ __device__ bool
68  operator()(int x) { return (x != -1); }
69  };
70 
71  struct isNaNPoint
72  {
73  __inline__ __host__ __device__ bool
75  {
76 #ifdef __CUDACC__
77  return (isnan (pt.x) | isnan (pt.y) | isnan (pt.z)) == 1;
78 #else
79  return (pcl_isnan (pt.x) | pcl_isnan (pt.y) | pcl_isnan (pt.z)) == 1;
80 #endif
81  }
82  };
83 
84  /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit from
85  * this class.
86  */
87  template <template <typename> class Storage>
89  {
90  public:
92  typedef typename PointCloud::Ptr PointCloudPtr;
94 
95  typedef boost::shared_ptr<SampleConsensusModel> Ptr;
96  typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;
97 
98  typedef typename Storage<int>::type Indices;
99  typedef boost::shared_ptr<typename Storage<int>::type> IndicesPtr;
100  typedef boost::shared_ptr<const typename Storage<int>::type> IndicesConstPtr;
101 
102  typedef typename Storage<float>::type Coefficients;
103  typedef boost::shared_ptr <Coefficients> CoefficientsPtr;
104  typedef boost::shared_ptr <const Coefficients> CoefficientsConstPtr;
105 
106  typedef typename Storage<float4>::type Hypotheses;
107  //TODO: should be vector<int> instead of int. but currently, only 1point plane model supports this
108  typedef typename Storage<int>::type Samples;
109 
110  private:
111  /** \brief Empty constructor for base SampleConsensusModel. */
112  SampleConsensusModel () : radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
113  {};
114 
115  public:
116  /** \brief Constructor for base SampleConsensusModel.
117  * \param cloud the input point cloud dataset
118  */
119  SampleConsensusModel (const PointCloudConstPtr &cloud) :
120  radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
121  {
122  // Sets the input cloud and creates a vector of "fake" indices
123  setInputCloud (cloud);
124  }
125 
126  /* \brief Constructor for base SampleConsensusModel.
127  * \param cloud the input point cloud dataset
128  * \param indices a vector of point indices to be used from \a cloud
129  */
130  /* SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector<int> &indices) :
131  input_ (cloud),
132  indices_ (boost::make_shared <std::vector<int> > (indices)),
133  radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX)
134 
135  {
136  if (indices_->size () > input_->points.size ())
137  {
138  ROS_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!", (unsigned long) indices_->size (), (unsigned long) input_->points.size ());
139  indices_->clear ();
140  }
141  };*/
142 
143  /** \brief Destructor for base SampleConsensusModel. */
144  virtual ~SampleConsensusModel () {};
145 
146  /** \brief Get a set of random data samples and return them as point
147  * indices. Pure virtual.
148  * \param iterations the internal number of iterations used by SAC methods
149  * \param samples the resultant model samples, <b>stored on the device</b>
150  */
151  virtual void
152  getSamples (int &iterations, Indices &samples) = 0;
153 
154  /** \brief Check whether the given index samples can form a valid model,
155  * compute the model coefficients from these samples and store them
156  * in model_coefficients. Pure virtual.
157  * \param samples the point indices found as possible good candidates
158  * for creating a valid model, <b>stored on the device</b>
159  * \param model_coefficients the computed model coefficients
160  */
161  virtual bool
162  computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients) = 0;
163 
164  virtual bool
165  generateModelHypotheses (Hypotheses &h, int max_iterations) = 0;
166 
167  virtual bool
168  generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations) = 0;
169 
170  virtual bool
171  isSampleInlier (IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
172  {return ((*inliers_stencil)[samples[i]] != -1);};
173 
174  /* \brief Recompute the model coefficients using the given inlier set
175  * and return them to the user. Pure virtual.
176  *
177  * @note: these are the coefficients of the model after refinement
178  * (e.g., after a least-squares optimization)
179  *
180  * \param inliers the data inliers supporting the model
181  * \param model_coefficients the initial guess for the model coefficients
182  * \param optimized_coefficients the resultant recomputed coefficients
183  * after non-linear optimization
184  */
185  // virtual void
186  // optimizeModelCoefficients (const std::vector<int> &inliers,
187  // const Eigen::VectorXf &model_coefficients,
188  // Eigen::VectorXf &optimized_coefficients) = 0;
189 
190  /* \brief Compute all distances from the cloud data to a given model. Pure virtual.
191  * \param model_coefficients the coefficients of a model that we need to
192  * compute distances to
193  * \param distances the resultant estimated distances
194  */
195  // virtual void
196  // getDistancesToModel (const Eigen::VectorXf &model_coefficients,
197  // std::vector<float> &distances) = 0;
198 
199  /** \brief Select all the points which respect the given model
200  * coefficients as inliers. Pure virtual.
201  *
202  * \param model_coefficients the coefficients of a model that we need to
203  * compute distances to
204  * \param threshold a maximum admissible distance threshold for
205  * determining the inliers from the outliers
206  * \param inliers the resultant model inliers
207  * \param inliers_stencil
208  */
209  virtual int
210  selectWithinDistance (const Coefficients &model_coefficients,
211  float threshold,
212  IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
213  virtual int
214  selectWithinDistance (const Hypotheses &h, int idx,
215  float threshold,
216  IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
217  virtual int
218  selectWithinDistance (Hypotheses &h, int idx,
219  float threshold,
220  IndicesPtr &inliers_stencil,
221  float3 &centroid) = 0;
222 
223  virtual int
224  countWithinDistance (const Coefficients &model_coefficients, float threshold) = 0;
225 
226  virtual int
227  countWithinDistance (const Hypotheses &h, int idx, float threshold) = 0;
228 
229  int
230  deleteIndices (const IndicesPtr &indices_stencil );
231  int
232  deleteIndices (const Hypotheses &h, int idx, IndicesPtr &inliers, const IndicesPtr &inliers_delete);
233 
234  /* \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
235  * \param inliers the data inliers that we want to project on the model
236  * \param model_coefficients the coefficients of a model
237  * \param projected_points the resultant projected points
238  * \param copy_data_fields set to true (default) if we want the \a
239  * projected_points cloud to be an exact copy of the input dataset minus
240  * the point projections on the plane model
241  */
242  // virtual void
243  // projectPoints (const std::vector<int> &inliers,
244  // const Eigen::VectorXf &model_coefficients,
245  // PointCloud &projected_points,
246  // bool copy_data_fields = true) = 0;
247 
248  /* \brief Verify whether a subset of indices verifies a given set of
249  * model coefficients. Pure virtual.
250  *
251  * \param indices the data indices that need to be tested against the model
252  * \param model_coefficients the set of model coefficients
253  * \param threshold a maximum admissible distance threshold for
254  * determining the inliers from the outliers
255  */
256  // virtual bool
257  // doSamplesVerifyModel (const std::set<int> &indices,
258  // const Eigen::VectorXf &model_coefficients,
259  // float threshold) = 0;
260 
261  /** \brief Provide a pointer to the input dataset
262  * \param cloud the const boost shared pointer to a PointCloud message
263  */
264  virtual void
265  setInputCloud (const PointCloudConstPtr &cloud);
266 
267  /** \brief Get a pointer to the input point cloud dataset. */
268  inline PointCloudConstPtr
269  getInputCloud () const { return (input_); }
270 
271  /* \brief Provide a pointer to the vector of indices that represents the input data.
272  * \param indices a pointer to the vector of indices that represents the input data.
273  */
274  // inline void
275  // setIndices (const IndicesPtr &indices) { indices_ = indices; }
276 
277  /* \brief Provide the vector of indices that represents the input data.
278  * \param indices the vector of indices that represents the input data.
279  */
280  // inline void
281  // setIndices (std::vector<int> &indices)
282  // {
283  // indices_ = boost::make_shared <std::vector<int> > (indices);
284  // }
285 
286  /** \brief Get a pointer to the vector of indices used. */
287  inline IndicesPtr
288  getIndices () const
289  {
290  if (nr_indices_in_stencil_ != indices_->size())
291  {
292  typename Indices::iterator last = thrust::remove_copy (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), -1);
293  indices_->erase (last, indices_->end ());
294  }
295 
296  return (indices_);
297  }
298 
299  /* \brief Return an unique id for each type of model employed. */
300  // virtual SacModel
301  // getModelType () const = 0;
302 
303  /* \brief Return the size of a sample from which a model is computed */
304  // inline unsigned int
305  // getSampleSize () const { return SAC_SAMPLE_SIZE.at (getModelType ()); }
306 
307  /** \brief Set the minimum and maximum allowable radius limits for the
308  * model (applicable to models that estimate a radius)
309  * \param min_radius the minimum radius model
310  * \param max_radius the maximum radius model
311  * \todo change this to set limits on the entire model
312  */
313  inline void
314  setRadiusLimits (float min_radius, float max_radius)
315  {
316  radius_min_ = min_radius;
317  radius_max_ = max_radius;
318  }
319 
320  /** \brief Get the minimum and maximum allowable radius limits for the
321  * model as set by the user.
322  *
323  * \param min_radius the resultant minimum radius model
324  * \param max_radius the resultant maximum radius model
325  */
326  inline void
327  getRadiusLimits (float &min_radius, float &max_radius)
328  {
329  min_radius = radius_min_;
330  max_radius = radius_max_;
331  }
332 
333  // friend class ProgressiveSampleConsensus<PointT>;
334 
335  inline boost::shared_ptr<typename Storage<float4>::type>
336  getNormals () { return (normals_); }
337 
338  inline
339  void setNormals (boost::shared_ptr<typename Storage<float4>::type> normals) { normals_ = normals; }
340 
341 
342  protected:
343  /* \brief Check whether a model is valid given the user constraints.
344  * \param model_coefficients the set of model coefficients
345  */
346  // virtual inline bool
347  // isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
348 
349  /** \brief A boost shared pointer to the point cloud data array. */
350  PointCloudConstPtr input_;
351  boost::shared_ptr<typename Storage<float4>::type> normals_;
352 
353  /** \brief A pointer to the vector of point indices to use. */
354  IndicesPtr indices_;
355  /** \brief A pointer to the vector of point indices (stencil) to use. */
356  IndicesPtr indices_stencil_;
357  /** \brief number of indices left in indices_stencil_ */
359 
360  /** \brief The minimum and maximum radius limits for the model.
361  * Applicable to all models that estimate a radius.
362  */
363  float radius_min_, radius_max_;
364 
365  /** \brief Linear-Congruent random number generator engine. */
366  thrust::minstd_rand rngl_;
367  };
368 
369  /* \brief @b SampleConsensusModelFromNormals represents the base model class
370  * for models that require the use of surface normals for estimation.
371  */
372  // template <typename PointT, typename PointNT>
373  // class SampleConsensusModelFromNormals
374  // {
375  // public:
376  // typedef typename pcl::PointCloud<PointNT>::ConstPtr PointCloudNConstPtr;
377  // typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudNPtr;
378  //
379  // typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
380  // typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;
381  //
382  // /* \brief Empty constructor for base SampleConsensusModelFromNormals. */
383  // SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0) {};
384  //
385  // /* \brief Set the normal angular distance weight.
386  // * \param w the relative weight (between 0 and 1) to give to the angular
387  // * distance (0 to pi/2) between point normals and the plane normal.
388  // * (The Euclidean distance will have weight 1-w.)
389  // */
390  // inline void
391  // setNormalDistanceWeight (float w) { normal_distance_weight_ = w; }
392  //
393  // /* \brief Get the normal angular distance weight. */
394  // inline float
395  // getNormalDistanceWeight () { return (normal_distance_weight_); }
396  //
397  // /* \brief Provide a pointer to the input dataset that contains the point
398  // * normals of the XYZ dataset.
399  // *
400  // * \param normals the const boost shared pointer to a PointCloud message
401  // */
402  // inline void
403  // setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
404  //
405  // /* \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
406  // inline PointCloudNConstPtr
407  // getInputNormals () { return (normals_); }
408  //
409  // protected:
410  // /* \brief The relative weight (between 0 and 1) to give to the angular
411  // * distance (0 to pi/2) between point normals and the plane normal.
412  // */
413  // float normal_distance_weight_;
414  //
415  // /* \brief A pointer to the input dataset that contains the point normals
416  // * of the XYZ dataset.
417  // */
418  // PointCloudNConstPtr normals_;
419  // };
420  } // namespace_
421 } // namespace_
422 
423 #endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_H_
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:350
SampleConsensusModel represents the base model class.
Definition: sac_model.h:88
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: sac_model.h:288
boost::shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:99
virtual bool isSampleInlier(IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
Definition: sac_model.h:171
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:133
void setRadiusLimits(float min_radius, float max_radius)
Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate...
Definition: sac_model.h:314
__inline__ __host__ __device__ bool operator()(int x)
Definition: sac_model.h:68
Storage< int >::type Samples
Definition: sac_model.h:108
Check if a certain tuple is a point inlier.
Definition: sac_model.h:58
boost::shared_ptr< const typename Storage< int >::type > IndicesConstPtr
Definition: sac_model.h:100
Storage< float >::type Coefficients
Definition: sac_model.h:102
Storage< int >::type Indices
Definition: sac_model.h:98
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:354
Storage< float4 >::type Hypotheses
Definition: sac_model.h:106
PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:93
boost::shared_ptr< SampleConsensusModel > Ptr
Definition: sac_model.h:95
SampleConsensusModel(const PointCloudConstPtr &cloud)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:119
Check if a certain tuple is a point inlier.
Definition: predicate.h:60
boost::shared_ptr< typename Storage< float4 >::type > getNormals()
Definition: sac_model.h:336
boost::shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:201
boost::shared_ptr< Coefficients > CoefficientsPtr
Definition: sac_model.h:103
boost::shared_ptr< const SampleConsensusModel > ConstPtr
Definition: sac_model.h:96
__inline__ __host__ __device__ int operator()(const Tuple &t)
thrust::minstd_rand rngl_
Linear-Congruent random number generator engine.
Definition: sac_model.h:366
float radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:363
boost::shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:202
PointCloudAOS< Storage > PointCloud
Definition: sac_model.h:91
IndicesPtr indices_stencil_
A pointer to the vector of point indices (stencil) to use.
Definition: sac_model.h:356
Default point xyz-rgb structure.
Definition: point_types.h:49
unsigned int nr_indices_in_stencil_
number of indices left in indices_stencil_
Definition: sac_model.h:358
void setNormals(boost::shared_ptr< typename Storage< float4 >::type > normals)
Definition: sac_model.h:339
boost::shared_ptr< typename Storage< float4 >::type > normals_
Definition: sac_model.h:351
boost::shared_ptr< const Coefficients > CoefficientsConstPtr
Definition: sac_model.h:104
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: sac_model.h:269
PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:92
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
Definition: sac_model.h:144
void getRadiusLimits(float &min_radius, float &max_radius)
Get the minimum and maximum allowable radius limits for the model as set by the user.
Definition: sac_model.h:327