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