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