Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
39 
40 #include <float.h>
41 #include <thrust/sequence.h>
42 #include <thrust/count.h>
43 #include <thrust/remove.h>
44 #include <pcl/cuda/point_cloud.h>
45 #include <thrust/random/linear_congruential_engine.h>
46 
47 #include <pcl/pcl_exports.h>
48 
49 namespace pcl
50 {
51  namespace cuda
52  {
53  // Forward declaration
54  //template<class T> class ProgressiveSampleConsensus;
55 
56  /** \brief Check if a certain tuple is a point inlier. */
58  {
59  template <typename Tuple> __inline__ __host__ __device__ int
60  operator () (const Tuple &t);
61  };
62 
63  /** \brief Check if a certain tuple is a point inlier. */
64  struct isInlier
65  {
66  __inline__ __host__ __device__ bool
67  operator()(int x) { return (x != -1); }
68  };
69 
70  struct isNaNPoint
71  {
72  __inline__ __host__ __device__ bool
74  {
75 #ifdef __CUDACC__
76  return (isnan (pt.x) | isnan (pt.y) | isnan (pt.z)) == 1;
77 #else
78  return (std::isnan (pt.x) | std::isnan (pt.y) | std::isnan (pt.z)) == 1;
79 #endif
80  }
81  };
82 
83  /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit from
84  * this class.
85  */
86  template <template <typename> class Storage>
88  {
89  public:
91  using PointCloudPtr = typename PointCloud::Ptr;
93 
94  using Ptr = boost::shared_ptr<SampleConsensusModel>;
95  using ConstPtr = boost::shared_ptr<const SampleConsensusModel>;
96 
97  using Indices = typename Storage<int>::type;
98  using IndicesPtr = boost::shared_ptr<typename Storage<int>::type>;
99  using IndicesConstPtr = boost::shared_ptr<const typename Storage<int>::type>;
100 
101  using Coefficients = typename Storage<float>::type;
102  using CoefficientsPtr = boost::shared_ptr <Coefficients>;
103  using CoefficientsConstPtr = boost::shared_ptr <const Coefficients>;
104 
105  using Hypotheses = typename Storage<float4>::type;
106  //TODO: should be std::vector<int> instead of int. but currently, only 1point plane model supports this
107  using Samples = typename Storage<int>::type;
108 
109  private:
110  /** \brief Empty constructor for base SampleConsensusModel. */
111  SampleConsensusModel () : radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
112  {};
113 
114  public:
115  /** \brief Constructor for base SampleConsensusModel.
116  * \param cloud the input point cloud dataset
117  */
119  radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
120  {
121  // Sets the input cloud and creates a vector of "fake" indices
122  setInputCloud (cloud);
123  }
124 
125  /* \brief Constructor for base SampleConsensusModel.
126  * \param cloud the input point cloud dataset
127  * \param indices a vector of point indices to be used from \a cloud
128  */
129  /* SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector<int> &indices) :
130  input_ (cloud),
131  indices_ (boost::make_shared <std::vector<int> > (indices)),
132  radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX)
133 
134  {
135  if (indices_->size () > input_->points.size ())
136  {
137  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 ());
138  indices_->clear ();
139  }
140  };*/
141 
142  /** \brief Destructor for base SampleConsensusModel. */
143  virtual ~SampleConsensusModel () {};
144 
145  /** \brief Get a set of random data samples and return them as point
146  * indices. Pure virtual.
147  * \param iterations the internal number of iterations used by SAC methods
148  * \param samples the resultant model samples, <b>stored on the device</b>
149  */
150  virtual void
151  getSamples (int &iterations, Indices &samples) = 0;
152 
153  /** \brief Check whether the given index samples can form a valid model,
154  * compute the model coefficients from these samples and store them
155  * in model_coefficients. Pure virtual.
156  * \param samples the point indices found as possible good candidates
157  * for creating a valid model, <b>stored on the device</b>
158  * \param model_coefficients the computed model coefficients
159  */
160  virtual bool
161  computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients) = 0;
162 
163  virtual bool
164  generateModelHypotheses (Hypotheses &h, int max_iterations) = 0;
165 
166  virtual bool
167  generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations) = 0;
168 
169  virtual bool
170  isSampleInlier (IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
171  {return ((*inliers_stencil)[samples[i]] != -1);};
172 
173  /* \brief Recompute the model coefficients using the given inlier set
174  * and return them to the user. Pure virtual.
175  *
176  * @note: these are the coefficients of the model after refinement
177  * (e.g., after a least-squares optimization)
178  *
179  * \param inliers the data inliers supporting the model
180  * \param model_coefficients the initial guess for the model coefficients
181  * \param optimized_coefficients the resultant recomputed coefficients
182  * after non-linear optimization
183  */
184  // virtual void
185  // optimizeModelCoefficients (const std::vector<int> &inliers,
186  // const Eigen::VectorXf &model_coefficients,
187  // Eigen::VectorXf &optimized_coefficients) = 0;
188 
189  /* \brief Compute all distances from the cloud data to a given model. Pure virtual.
190  * \param model_coefficients the coefficients of a model that we need to
191  * compute distances to
192  * \param distances the resultant estimated distances
193  */
194  // virtual void
195  // getDistancesToModel (const Eigen::VectorXf &model_coefficients,
196  // std::vector<float> &distances) = 0;
197 
198  /** \brief Select all the points which respect the given model
199  * coefficients as inliers. Pure virtual.
200  *
201  * \param model_coefficients the coefficients of a model that we need to
202  * compute distances to
203  * \param threshold a maximum admissible distance threshold for
204  * determining the inliers from the outliers
205  * \param inliers the resultant model inliers
206  * \param inliers_stencil
207  */
208  virtual int
209  selectWithinDistance (const Coefficients &model_coefficients,
210  float threshold,
211  IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
212  virtual int
213  selectWithinDistance (const Hypotheses &h, int idx,
214  float threshold,
215  IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
216  virtual int
217  selectWithinDistance (Hypotheses &h, int idx,
218  float threshold,
219  IndicesPtr &inliers_stencil,
220  float3 &centroid) = 0;
221 
222  virtual int
223  countWithinDistance (const Coefficients &model_coefficients, float threshold) = 0;
224 
225  virtual int
226  countWithinDistance (const Hypotheses &h, int idx, float threshold) = 0;
227 
228  int
229  deleteIndices (const IndicesPtr &indices_stencil );
230  int
231  deleteIndices (const Hypotheses &h, int idx, IndicesPtr &inliers, const IndicesPtr &inliers_delete);
232 
233  /* \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
234  * \param inliers the data inliers that we want to project on the model
235  * \param model_coefficients the coefficients of a model
236  * \param projected_points the resultant projected points
237  * \param copy_data_fields set to true (default) if we want the \a
238  * projected_points cloud to be an exact copy of the input dataset minus
239  * the point projections on the plane model
240  */
241  // virtual void
242  // projectPoints (const std::vector<int> &inliers,
243  // const Eigen::VectorXf &model_coefficients,
244  // PointCloud &projected_points,
245  // bool copy_data_fields = true) = 0;
246 
247  /* \brief Verify whether a subset of indices verifies a given set of
248  * model coefficients. Pure virtual.
249  *
250  * \param indices the data indices that need to be tested against the model
251  * \param model_coefficients the set of model coefficients
252  * \param threshold a maximum admissible distance threshold for
253  * determining the inliers from the outliers
254  */
255  // virtual bool
256  // doSamplesVerifyModel (const std::set<int> &indices,
257  // const Eigen::VectorXf &model_coefficients,
258  // float threshold) = 0;
259 
260  /** \brief Provide a pointer to the input dataset
261  * \param cloud the const boost shared pointer to a PointCloud message
262  */
263  virtual void
264  setInputCloud (const PointCloudConstPtr &cloud);
265 
266  /** \brief Get a pointer to the input point cloud dataset. */
267  inline PointCloudConstPtr
268  getInputCloud () const { return (input_); }
269 
270  /* \brief Provide a pointer to the vector of indices that represents the input data.
271  * \param indices a pointer to the vector of indices that represents the input data.
272  */
273  // inline void
274  // setIndices (const IndicesPtr &indices) { indices_ = indices; }
275 
276  /* \brief Provide the vector of indices that represents the input data.
277  * \param indices the vector of indices that represents the input data.
278  */
279  // inline void
280  // setIndices (std::vector<int> &indices)
281  // {
282  // indices_ = boost::make_shared <std::vector<int> > (indices);
283  // }
284 
285  /** \brief Get a pointer to the vector of indices used. */
286  inline IndicesPtr
287  getIndices () const
288  {
289  if (nr_indices_in_stencil_ != indices_->size())
290  {
291  typename Indices::iterator last = thrust::remove_copy (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), -1);
292  indices_->erase (last, indices_->end ());
293  }
294 
295  return (indices_);
296  }
297 
298  /* \brief Return an unique id for each type of model employed. */
299  // virtual SacModel
300  // getModelType () const = 0;
301 
302  /* \brief Return the size of a sample from which a model is computed */
303  // inline unsigned int
304  // getSampleSize () const { return SAC_SAMPLE_SIZE.at (getModelType ()); }
305 
306  /** \brief Set the minimum and maximum allowable radius limits for the
307  * model (applicable to models that estimate a radius)
308  * \param min_radius the minimum radius model
309  * \param max_radius the maximum radius model
310  * \todo change this to set limits on the entire model
311  */
312  inline void
313  setRadiusLimits (float min_radius, float max_radius)
314  {
315  radius_min_ = min_radius;
316  radius_max_ = max_radius;
317  }
318 
319  /** \brief Get the minimum and maximum allowable radius limits for the
320  * model as set by the user.
321  *
322  * \param min_radius the resultant minimum radius model
323  * \param max_radius the resultant maximum radius model
324  */
325  inline void
326  getRadiusLimits (float &min_radius, float &max_radius)
327  {
328  min_radius = radius_min_;
329  max_radius = radius_max_;
330  }
331 
332  // friend class ProgressiveSampleConsensus<PointT>;
333 
334  inline boost::shared_ptr<typename Storage<float4>::type>
335  getNormals () { return (normals_); }
336 
337  inline
338  void setNormals (boost::shared_ptr<typename Storage<float4>::type> normals) { normals_ = normals; }
339 
340 
341  protected:
342  /* \brief Check whether a model is valid given the user constraints.
343  * \param model_coefficients the set of model coefficients
344  */
345  // virtual inline bool
346  // isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
347 
348  /** \brief A boost shared pointer to the point cloud data array. */
350  boost::shared_ptr<typename Storage<float4>::type> normals_;
351 
352  /** \brief A pointer to the vector of point indices to use. */
354  /** \brief A pointer to the vector of point indices (stencil) to use. */
356  /** \brief number of indices left in indices_stencil_ */
358 
359  /** \brief The minimum and maximum radius limits for the model.
360  * Applicable to all models that estimate a radius.
361  */
362  float radius_min_, radius_max_;
363 
364  /** \brief Linear-Congruent random number generator engine. */
365  thrust::minstd_rand rngl_;
366  };
367 
368  /* \brief @b SampleConsensusModelFromNormals represents the base model class
369  * for models that require the use of surface normals for estimation.
370  */
371  // template <typename PointT, typename PointNT>
372  // class SampleConsensusModelFromNormals
373  // {
374  // public:
375  // using PointCloudNConstPtr = typename pcl::PointCloud<PointNT>::ConstPtr;
376  // using PointCloudNPtr = typename pcl::PointCloud<PointNT>::Ptr;
377  //
378  // using Ptr = boost::shared_ptr<SampleConsensusModelFromNormals>;
379  // using ConstPtr = boost::shared_ptr<const SampleConsensusModelFromNormals>;
380  //
381  // /* \brief Empty constructor for base SampleConsensusModelFromNormals. */
382  // SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0) {};
383  //
384  // /* \brief Set the normal angular distance weight.
385  // * \param w the relative weight (between 0 and 1) to give to the angular
386  // * distance (0 to pi/2) between point normals and the plane normal.
387  // * (The Euclidean distance will have weight 1-w.)
388  // */
389  // inline void
390  // setNormalDistanceWeight (float w) { normal_distance_weight_ = w; }
391  //
392  // /* \brief Get the normal angular distance weight. */
393  // inline float
394  // getNormalDistanceWeight () { return (normal_distance_weight_); }
395  //
396  // /* \brief Provide a pointer to the input dataset that contains the point
397  // * normals of the XYZ dataset.
398  // *
399  // * \param normals the const boost shared pointer to a PointCloud message
400  // */
401  // inline void
402  // setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
403  //
404  // /* \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
405  // inline PointCloudNConstPtr
406  // getInputNormals () { return (normals_); }
407  //
408  // protected:
409  // /* \brief The relative weight (between 0 and 1) to give to the angular
410  // * distance (0 to pi/2) between point normals and the plane normal.
411  // */
412  // float normal_distance_weight_;
413  //
414  // /* \brief A pointer to the input dataset that contains the point normals
415  // * of the XYZ dataset.
416  // */
417  // PointCloudNConstPtr normals_;
418  // };
419  } // namespace_
420 } // namespace_
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:349
SampleConsensusModel represents the base model class.
Definition: sac_model.h:87
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:91
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: sac_model.h:287
boost::shared_ptr< Coefficients > CoefficientsPtr
Definition: sac_model.h:102
virtual bool isSampleInlier(IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
Definition: sac_model.h:170
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< SampleConsensusModel > Ptr
Definition: sac_model.h:94
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:132
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:313
__inline__ __host__ __device__ bool operator()(int x)
Definition: sac_model.h:67
Check if a certain tuple is a point inlier.
Definition: sac_model.h:57
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:353
typename Storage< int >::type Samples
Definition: sac_model.h:107
typename Storage< int >::type Indices
Definition: sac_model.h:97
boost::shared_ptr< const Coefficients > CoefficientsConstPtr
Definition: sac_model.h:103
SampleConsensusModel(const PointCloudConstPtr &cloud)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:118
Check if a certain tuple is a point inlier.
Definition: predicate.h:59
boost::shared_ptr< typename Storage< float4 >::type > getNormals()
Definition: sac_model.h:335
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:411
boost::shared_ptr< const typename Storage< int >::type > IndicesConstPtr
Definition: sac_model.h:99
__inline__ __host__ __device__ int operator()(const Tuple &t)
thrust::minstd_rand rngl_
Linear-Congruent random number generator engine.
Definition: sac_model.h:365
float radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:362
boost::shared_ptr< const SampleConsensusModel > ConstPtr
Definition: sac_model.h:95
boost::shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:98
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:92
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:412
typename Storage< float >::type Coefficients
Definition: sac_model.h:101
IndicesPtr indices_stencil_
A pointer to the vector of point indices (stencil) to use.
Definition: sac_model.h:355
Default point xyz-rgb structure.
Definition: point_types.h:48
unsigned int nr_indices_in_stencil_
number of indices left in indices_stencil_
Definition: sac_model.h:357
void setNormals(boost::shared_ptr< typename Storage< float4 >::type > normals)
Definition: sac_model.h:338
boost::shared_ptr< typename Storage< float4 >::type > normals_
Definition: sac_model.h:350
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: sac_model.h:268
typename Storage< float4 >::type Hypotheses
Definition: sac_model.h:105
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
Definition: sac_model.h:143
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:326