Point Cloud Library (PCL)  1.9.1-dev
sac_model_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  * $Id$
35  *
36  */
37 
38 #pragma once
39 
40 #include <pcl/cuda/sample_consensus/sac_model.h>
41 
42 #include <thrust/random.h>
43 
44 namespace pcl
45 {
46  namespace cuda
47  {
48  /** \brief Check if a certain tuple is a point inlier. */
49  struct CountPlanarInlier
50  {
51  float4 coefficients;
52  float threshold;
53 
54  CountPlanarInlier (float4 coeff, float thresh) :
55  coefficients(coeff), threshold(thresh)
56  {}
57 
58  template <typename Tuple> __inline__ __host__ __device__ bool
59  operator () (const Tuple &t);
60  };
61 
62  /** \brief Check if a certain tuple is a point inlier. */
63  struct CheckPlanarInlier
64  {
65  float4 coefficients;
66  float threshold;
67 
68  CheckPlanarInlier (float4 coeff, float thresh) :
69  coefficients(coeff), threshold(thresh)
70  {}
71 
72  template <typename Tuple> __inline__ __host__ __device__ int
73  operator () (const Tuple &t);
74  };
75 
76  ////////////////////////////////////////////////////////////////////////////////////////////
77  /** \brief @b SampleConsensusModelPlane defines a model for 3D plane segmentation.
78  */
79  template <template <typename> class Storage>
81  {
82  public:
86 
88  using PointCloudPtr = typename PointCloud::Ptr;
90 
94 
98 
99  using Ptr = boost::shared_ptr<SampleConsensusModelPlane>;
100 
101  /** \brief Constructor for base SampleConsensusModelPlane.
102  * \param cloud the input point cloud dataset
103  */
105 
106  /* \brief Constructor for base SampleConsensusModelPlane.
107  * \param cloud the input point cloud dataset
108  * \param indices a vector of point indices to be used from \a cloud
109  */
110  // SampleConsensusModelPlane (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : SampleConsensusModel<PointT> (cloud, indices) {};
111 
112  /** \brief Get 3 random non-collinear points as data samples and return them as point indices.
113  * \param iterations the internal number of iterations used by SAC methods
114  * \param samples the resultant model samples
115  * \note assumes unique points!
116  */
117  void
118  getSamples (int &iterations, Indices &samples);
119 
120  /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
121  * these samples and store them in model_coefficients. The plane coefficients are:
122  * a, b, c, d (ax+by+cz+d=0)
123  * \param samples the point indices found as possible good candidates for creating a valid model
124  * \param model_coefficients the resultant model coefficients
125  */
126  bool
127  computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients);
128 
129  bool
130  generateModelHypotheses (Hypotheses &h, int max_iterations);
131 
132  virtual bool
133  generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations)
134  {
135  // TODO: hack.. Samples should be std::vector<int>, not int..
136  return generateModelHypotheses (h, max_iterations);
137  };
138 
139  /* \brief Compute all distances from the cloud data to a given plane model.
140  * \param model_coefficients the coefficients of a plane model that we need to compute distances to
141  * \param distances the resultant estimated distances
142  */
143  // void
144  // getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<float> &distances);
145 
146  /** \brief Select all the points which respect the given model coefficients as inliers.
147  * \param model_coefficients the coefficients of a plane model that we need to
148  * compute distances to
149  * \param threshold a maximum admissible distance threshold for determining the
150  * inliers from the outliers
151  * \param inliers the resultant model inliers
152  * \param inliers_stencil
153  */
154  int
155  selectWithinDistance (const Coefficients &model_coefficients,
156  float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil);
157  int
158  selectWithinDistance (const Hypotheses &h, int idx,
159  float threshold,
160  IndicesPtr &inliers, IndicesPtr &inliers_stencil);
161  int
162  selectWithinDistance (Hypotheses &h, int idx,
163  float threshold,
164  IndicesPtr &inliers_stencil,
165  float3 &centroid);
166 
167  int
168  countWithinDistance (const Coefficients &model_coefficients, float threshold);
169 
170  int
171  countWithinDistance (const Hypotheses &h, int idx, float threshold);
172 
173  /* \brief Recompute the plane coefficients using the given inlier set and return them to the user.
174  * @note: these are the coefficients of the plane model after refinement (eg. after SVD)
175  * \param inliers the data inliers found as supporting the model
176  * \param model_coefficients the initial guess for the model coefficients
177  * \param optimized_coefficients the resultant recomputed coefficients after non-linear optimization
178  */
179  // void
180  // optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients);
181 
182  /* \brief Create a new point cloud with inliers projected onto the plane model.
183  * \param inliers the data inliers that we want to project on the plane model
184  * \param model_coefficients the *normalized* coefficients of a plane model
185  * \param projected_points the resultant projected points
186  * \param copy_data_fields set to true if we need to copy the other data fields
187  */
188  // void
189  // projectPoints (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true);
190 
191  /* \brief Verify whether a subset of indices verifies the given plane model coefficients.
192  * \param indices the data indices that need to be tested against the plane model
193  * \param model_coefficients the plane model coefficients
194  * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
195  */
196  // bool
197  // doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, float threshold);
198 
199  /* \brief Return an unique id for this model (SACMODEL_PLANE). */
200  // inline pcl::SacModel getModelType () const { return (SACMODEL_PLANE); }
201 
202  // protected:
203  /* \brief Check whether a model is valid given the user constraints.
204  * \param model_coefficients the set of model coefficients
205  */
206  // inline bool
207  // isModelValid (const Eigen::VectorXf &model_coefficients)
208  // {
209  // // Needs a valid model coefficients
210  // if (model_coefficients.size () != 4)
211  // {
212  // ROS_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%lu)!", (unsigned long) model_coefficients.size ());
213  // return (false);
214  // }
215  // return (true);
216  // }
217 
218  // private:
219  /* \brief Define the maximum number of iterations for collinearity checks */
220  const static int MAX_ITERATIONS_COLLINEAR = 1000;
221  };
222 
223  /** \brief Check if a certain tuple is a point inlier. */
224  template <template <typename> class Storage>
226  {
229 
233 
235  const int *indices;
237  float bad_value;
238 
239  CreatePlaneHypothesis (const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad) :
240  input(_input), indices(_indices), nr_indices(_nr_indices), bad_value(bad)
241  {}
242 
243  //template <typename Tuple>
244  __inline__ __host__ __device__ float4
245  //operator () (const Tuple &t);
246  operator () (int t);
247  };
248 
249 
251  {
252 
253  __inline__ __host__ __device__
254  parallel_random_generator(unsigned int seed)
255  {
256  m_seed = seed;
257  }
258 
259  __inline__ __host__ __device__
260  unsigned int operator()(const unsigned int n) const
261  {
262  thrust::default_random_engine rng(m_seed);
263  // discard n numbers to avoid correlation
264  rng.discard(n);
265  // return a random number
266  return rng();
267  }
268  unsigned int m_seed;
269  };
270 
271  } // namespace
272 } // namespace
SampleConsensusModelPlane defines a model for 3D plane segmentation.
CheckPlanarInlier(float4 coeff, float thresh)
typename SampleConsensusModel< Storage >::IndicesPtr IndicesPtr
SampleConsensusModel represents the base model class.
Definition: sac_model.h:87
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:91
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
typename SampleConsensusModel< Storage >::PointCloud PointCloud
typename PointCloud::ConstPtr PointCloudConstPtr
CreatePlaneHypothesis(const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad)
virtual bool generateModelHypotheses(Hypotheses &h, Samples &s, int max_iterations)
typename Storage< int >::type Samples
Definition: sac_model.h:107
typename Storage< int >::type Indices
Definition: sac_model.h:97
typename SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
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__ unsigned int operator()(const unsigned int n) const
typename SampleConsensusModel< Storage >::Indices Indices
__inline__ __host__ __device__ parallel_random_generator(unsigned int seed)
boost::shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:98
Check if a certain tuple is a point inlier.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:92
__inline__ __host__ __device__ bool operator()(const Tuple &t)
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:412
typename Storage< float >::type Coefficients
Definition: sac_model.h:101
Default point xyz-rgb structure.
Definition: point_types.h:48
typename Storage< float4 >::type Hypotheses
Definition: sac_model.h:105
CountPlanarInlier(float4 coeff, float thresh)
Check if a certain tuple is a point inlier.