Point Cloud Library (PCL)  1.14.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  using PointCloudPtr = typename PointCloud::Ptr;
169 
173 
177 
178 
179  using Ptr = shared_ptr<SampleConsensusModel1PointPlane>;
180  using ConstPtr = shared_ptr<const SampleConsensusModel1PointPlane>;
181 
182  /** \brief Constructor for base SampleConsensusModel1PointPlane.
183  * \param cloud the input point cloud dataset
184  */
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
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
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing.
Definition: point_cloud.h:133
SampleConsensusModel1PointPlane defines a model for 3D plane segmentation.
void getSamples(int &iterations, Indices &samples)
Get 3 random non-collinear points as data samples and return them as point indices.
bool computeModelCoefficients(const Indices &samples, Coefficients &model_coefficients)
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
int countWithinDistance(const Coefficients &model_coefficients, float threshold)
int countWithinDistance(const Hypotheses &h, int idx, float threshold)
bool generateModelHypotheses(Hypotheses &h, Samples &s, int max_iterations)
int selectWithinDistance(const Coefficients &model_coefficients, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)
Select all the points which respect the given model coefficients as inliers.
SampleConsensusModel1PointPlane(const PointCloudConstPtr &cloud)
Constructor for base SampleConsensusModel1PointPlane.
int selectWithinDistance(Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 &centroid)
bool generateModelHypotheses(Hypotheses &h, int max_iterations)
int selectWithinDistance(const Hypotheses &h, int idx, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)
SampleConsensusModel represents the base model class.
Definition: sac_model.h:88
typename Storage< float4 >::type Hypotheses
Definition: sac_model.h:105
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:91
shared_ptr< const SampleConsensusModel > ConstPtr
Definition: sac_model.h:95
shared_ptr< const typename Storage< int >::type > IndicesConstPtr
Definition: sac_model.h:99
shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:98
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:92
typename Storage< float >::type Coefficients
Definition: sac_model.h:101
typename Storage< int >::type Indices
Definition: sac_model.h:97
shared_ptr< SampleConsensusModel > Ptr
Definition: sac_model.h:94
typename Storage< int >::type Samples
Definition: sac_model.h:107
Check if a certain tuple is a point inlier.
CheckPlanarInlier(float4 coeff, float thresh)
__inline__ __host__ __device__ int operator()(const Tuple &t)
Check if a certain tuple is a point inlier.
__inline__ __host__ __device__ int operator()(const PointXYZRGB &pt, const int &idx)
CheckPlanarInlierIndices(float4 coeff, float thresh)
Check if a certain tuple is a point inlier.
CheckPlanarInlierKinectIndices(float4 coeff, float thresh, float angle_thresh)
__inline__ __host__ __device__ int operator()(const PointXYZRGB &pt, const int &idx)
Check if a certain tuple is a point inlier.
__inline__ __host__ __device__ int operator()(const Tuple &t, const int &idx)
CheckPlanarInlierKinectNormalIndices(float4 coeff, float thresh, float angle_thresh)
Check if a certain tuple is a point inlier.
__inline__ __host__ __device__ int operator()(const Tuple &pt, const int &idx)
CheckPlanarInlierNormalIndices(float4 coeff, float thresh, float angle_thresh)
Check if a certain tuple is a point inlier.
__inline__ __host__ __device__ bool operator()(const Tuple &t)
CountPlanarInlier(float4 coeff, float thresh)
Check if a certain tuple is a point inlier.
typename PointCloud::ConstPtr PointCloudConstPtr
typename SampleConsensusModel< Storage >::PointCloud PointCloud
Create1PointPlaneHypothesis(const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad)
__inline__ __host__ __device__ float4 operator()(int t)
typename SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
typename SampleConsensusModel< Storage >::Indices Indices
Check if a certain tuple is a point inlier.
Create1PointPlaneSampleHypothesis(const PointXYZRGB *_input, const float4 *normals, const int *_indices, int width, int height, int _nr_indices, float bad)
__inline__ __host__ __device__ thrust::tuple< int, float4 > operator()(int t)
typename SampleConsensusModel< Storage >::Indices Indices
typename SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
typename SampleConsensusModel< Storage >::PointCloud PointCloud
Check if a certain tuple is a point inlier.
const Storage< PointXYZRGB >::type & input_
__inline__ __host__ __device__ int operator()(const int &idx)
NewCheckPlanarInlier(float4 coeff, float thresh, const typename Storage< PointXYZRGB >::type &input)
Default point xyz-rgb structure.
Definition: point_types.h:49
__inline__ __host__ __device__ parallel_random_generator(unsigned int seed)
__inline__ __host__ __device__ unsigned int operator()(const unsigned int n) const