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