Point Cloud Library (PCL)  1.9.1-dev
sac_model_1point_plane.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  * Author: Nico Blodow (blodow@cs.tum.edu)
35  *
36  * $Id$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/cuda/sample_consensus/sac_model.h>
43 #include <thrust/random.h>
44 
45 namespace pcl
46 {
47  namespace cuda
48  {
49  /** \brief Check if a certain tuple is a point inlier. */
51  {
52  float4 coefficients;
53  float threshold;
54 
55  CountPlanarInlier (float4 coeff, float thresh) :
56  coefficients(coeff), threshold(thresh)
57  {}
58 
59  template <typename Tuple> __inline__ __host__ __device__ bool
60  operator () (const Tuple &t);
61  };
62 
63  /** \brief Check if a certain tuple is a point inlier. */
64  template <template <typename> class Storage>
66  {
67  float4 coefficients;
68  float threshold;
69  const typename Storage<PointXYZRGB>::type &input_;
70 
71  NewCheckPlanarInlier (float4 coeff, float thresh, const typename Storage<PointXYZRGB>::type &input) :
72  coefficients(coeff), threshold(thresh), input_(input)
73  {}
74 
75  __inline__ __host__ __device__ int
76  operator () (const int &idx);
77  };
78 
79  /** \brief Check if a certain tuple is a point inlier. */
81  {
82  float4 coefficients;
83  float threshold;
84 
85  CheckPlanarInlier (float4 coeff, float thresh) :
86  coefficients(coeff), threshold(thresh)
87  {}
88 
89  template <typename Tuple> __inline__ __host__ __device__ int
90  operator () (const Tuple &t);
91  };
92 
93  /** \brief Check if a certain tuple is a point inlier. */
95  {
96  float4 coefficients;
97  float threshold;
98 
99  CheckPlanarInlierIndices (float4 coeff, float thresh) :
100  coefficients(coeff), threshold(thresh)
101  {}
102 
103  __inline__ __host__ __device__ int
104  operator () (const PointXYZRGB &pt, const int &idx);
105  };
106 
107  /** \brief Check if a certain tuple is a point inlier. */
109  {
110  float4 coefficients;
111  float threshold;
113 
114  CheckPlanarInlierKinectNormalIndices (float4 coeff, float thresh, float angle_thresh) :
115  coefficients(coeff), threshold(thresh), angle_threshold (angle_thresh)
116  {}
117 
118  template <typename Tuple> __inline__ __host__ __device__ int
119  operator () (const Tuple &t, const int &idx);
120  };
121 
122  /** \brief Check if a certain tuple is a point inlier. */
124  {
125  float4 coefficients;
126  float threshold;
128 
129  CheckPlanarInlierKinectIndices (float4 coeff, float thresh, float angle_thresh) :
130  coefficients(coeff), threshold(thresh), angle_threshold (angle_thresh)
131  {}
132 
133  __inline__ __host__ __device__ int
134  operator () (const PointXYZRGB &pt, const int &idx);
135  };
136 
137  /** \brief Check if a certain tuple is a point inlier. */
139  {
140  float4 coefficients;
141  float threshold;
143 
144  CheckPlanarInlierNormalIndices (float4 coeff, float thresh, float angle_thresh) :
145  coefficients(coeff), threshold(thresh), angle_threshold (angle_thresh)
146  {}
147 
148  template <typename Tuple>
149  __inline__ __host__ __device__ int
150  operator () (const Tuple &pt, const int &idx);
151  };
152 
153  ////////////////////////////////////////////////////////////////////////////////////////////
154  /** \brief @b SampleConsensusModel1PointPlane defines a model for 3D plane segmentation.
155  */
156  template <template <typename> class Storage>
158  {
159  public:
165 
167  typedef typename PointCloud::Ptr PointCloudPtr;
169 
173 
177 
178 
179  typedef boost::shared_ptr<SampleConsensusModel1PointPlane> Ptr;
180 
181  /** \brief Constructor for base SampleConsensusModel1PointPlane.
182  * \param cloud the input point cloud dataset
183  */
184  SampleConsensusModel1PointPlane (const PointCloudConstPtr &cloud);
185 
186  /** \brief Get 3 random non-collinear points as data samples and return them as point indices.
187  * \param iterations the internal number of iterations used by SAC methods
188  * \param samples the resultant model samples
189  * \note assumes unique points!
190  */
191  void
192  getSamples (int &iterations, Indices &samples);
193 
194  /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
195  * these samples and store them in model_coefficients. The plane coefficients are:
196  * a, b, c, d (ax+by+cz+d=0)
197  * \param samples the point indices found as possible good candidates for creating a valid model
198  * \param model_coefficients the resultant model coefficients
199  */
200  bool
201  computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients);
202 
203  bool
204  generateModelHypotheses (Hypotheses &h, int max_iterations);
205 
206  bool
207  generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations);
208 
209  /** \brief Select all the points which respect the given model coefficients as inliers.
210  * \param model_coefficients the coefficients of a plane model that we need to
211  * compute distances to
212  * \param threshold a maximum admissible distance threshold for determining the
213  * inliers from the outliers
214  * \param inliers the resultant model inliers
215  * \param inliers_stencil
216  */
217  int
218  selectWithinDistance (const Coefficients &model_coefficients,
219  float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil);
220  int
221  selectWithinDistance (const Hypotheses &h, int idx,
222  float threshold,
223  IndicesPtr &inliers, IndicesPtr &inliers_stencil);
224  int
225  selectWithinDistance (Hypotheses &h, int idx,
226  float threshold,
227  IndicesPtr &inliers_stencil,
228  float3 &centroid);
229  int
230  countWithinDistance (const Coefficients &model_coefficients, float threshold);
231 
232  int
233  countWithinDistance (const Hypotheses &h, int idx, float threshold);
234 
235  // private:
236  // /** \brief Define the maximum number of iterations for collinearity checks */
237  const static int MAX_ITERATIONS_COLLINEAR = 1000;
238  };
239 
240  /** \brief Check if a certain tuple is a point inlier. */
241  template <template <typename> class Storage>
243  {
248 
250  const int *indices;
252  float bad_value;
253 
254  Create1PointPlaneHypothesis (const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad) :
255  input(_input), indices(_indices), nr_indices(_nr_indices), bad_value(bad)
256  {}
257 
258  //template <typename Tuple>
259  __inline__ __host__ __device__ float4
260  //operator () (const Tuple &t);
261  operator () (int t);
262  };
263 
264  /** \brief Check if a certain tuple is a point inlier. */
265  template <template <typename> class Storage>
267  {
272 
274  const float4 *normals_;
275  const int *indices;
276  int width_;
277  int height_;
279  float bad_value;
280  thrust::default_random_engine rng;
281 
282  Create1PointPlaneSampleHypothesis (const PointXYZRGB *_input, const float4* normals, const int *_indices, int width, int height, int _nr_indices, float bad) :
283  input(_input), normals_(normals), indices(_indices), width_(width), height_(height), nr_indices(_nr_indices), bad_value(bad)
284  {
285  }
286 
287  //template <typename Tuple>
288  __inline__ __host__ __device__ thrust::tuple<int,float4>
289  //operator () (const Tuple &t);
290  operator () (int t);
291  };
292 
294  {
295 
296  __inline__ __host__ __device__
297  parallel_random_generator(unsigned int seed)
298  {
299  m_seed = seed;
300  }
301 
302  __inline__ __host__ __device__
303  unsigned int operator()(const unsigned int n) const
304  {
305  thrust::default_random_engine rng(m_seed);
306  // discard n numbers to avoid correlation
307  rng.discard(n);
308  // return a random number
309  return rng();
310  }
311  unsigned int m_seed;
312  };
313 
314  } // namespace
315 } // namespace
SampleConsensusModel< Storage >::Indices Indices
Check if a certain tuple is a point inlier.
SampleConsensusModel< Storage >::Coefficients Coefficients
SampleConsensusModel< Storage >::Hypotheses Hypotheses
CheckPlanarInlier(float4 coeff, float thresh)
SampleConsensusModel< Storage >::Indices Indices
SampleConsensusModel represents the base model class.
Definition: sac_model.h:87
boost::shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:98
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:132
Storage< int >::type Samples
Definition: sac_model.h:107
boost::shared_ptr< const typename Storage< int >::type > IndicesConstPtr
Definition: sac_model.h:99
Storage< float >::type Coefficients
Definition: sac_model.h:101
SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
Create1PointPlaneSampleHypothesis(const PointXYZRGB *_input, const float4 *normals, const int *_indices, int width, int height, int _nr_indices, float bad)
SampleConsensusModel1PointPlane defines a model for 3D plane segmentation.
SampleConsensusModel< Storage >::PointCloud PointCloud
Storage< int >::type Indices
Definition: sac_model.h:97
SampleConsensusModel< Storage >::IndicesPtr IndicesPtr
Check if a certain tuple is a point inlier.
CheckPlanarInlierNormalIndices(float4 coeff, float thresh, float angle_thresh)
SampleConsensusModel< Storage >::PointCloud PointCloud
CheckPlanarInlierIndices(float4 coeff, float thresh)
Storage< float4 >::type Hypotheses
Definition: sac_model.h:105
Check if a certain tuple is a point inlier.
Check if a certain tuple is a point inlier.
SampleConsensusModel< Storage >::Samples Samples
SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
SampleConsensusModel< Storage >::PointCloud PointCloud
boost::shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:200
Check if a certain tuple is a point inlier.
SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
__inline__ __host__ __device__ parallel_random_generator(unsigned int seed)
boost::shared_ptr< SampleConsensusModel1PointPlane > Ptr
SampleConsensusModel< Storage >::Indices Indices
CheckPlanarInlierKinectNormalIndices(float4 coeff, float thresh, float angle_thresh)
CheckPlanarInlierKinectIndices(float4 coeff, float thresh, float angle_thresh)
__inline__ __host__ __device__ bool operator()(const Tuple &t)
NewCheckPlanarInlier(float4 coeff, float thresh, const typename Storage< PointXYZRGB >::type &input)
boost::shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:201
Create1PointPlaneHypothesis(const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad)
Default point xyz-rgb structure.
Definition: point_types.h:48
Check if a certain tuple is a point inlier.
__inline__ __host__ __device__ unsigned int operator()(const unsigned int n) const
CountPlanarInlier(float4 coeff, float thresh)
Check if a certain tuple is a point inlier.
Check if a certain tuple is a point inlier.
Check if a certain tuple is a point inlier.
const Storage< PointXYZRGB >::type & input_