Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
45namespace pcl
46{
47 namespace cuda
48 {
49 /** \brief Check if a certain tuple is a point inlier. */
51 {
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 {
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 {
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 {
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 {
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 {
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 {
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:
160 using SampleConsensusModel<Storage>::input_;
161 using SampleConsensusModel<Storage>::normals_;
162 using SampleConsensusModel<Storage>::indices_;
164 using SampleConsensusModel<Storage>::rngl_;
165
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
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;
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);
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;
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);
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
shared_ptr< const PointCloud< PointT > > ConstPtr
SampleConsensusModel1PointPlane defines a model for 3D plane segmentation.
typename SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
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...
typename SampleConsensusModel< Storage >::PointCloud PointCloud
int countWithinDistance(const Coefficients &model_coefficients, float threshold)
typename SampleConsensusModel< Storage >::IndicesPtr IndicesPtr
typename SampleConsensusModel< Storage >::Samples Samples
int countWithinDistance(const Hypotheses &h, int idx, float threshold)
bool generateModelHypotheses(Hypotheses &h, Samples &s, int max_iterations)
typename SampleConsensusModel< Storage >::Coefficients Coefficients
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.
shared_ptr< SampleConsensusModel1PointPlane > Ptr
typename SampleConsensusModel< Storage >::Hypotheses Hypotheses
typename SampleConsensusModel< Storage >::Indices Indices
int selectWithinDistance(Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 &centroid)
shared_ptr< const SampleConsensusModel1PointPlane > ConstPtr
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
IndicesPtr indices_stencil_
A pointer to the vector of point indices (stencil) to use.
Definition sac_model.h:359
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition sac_model.h:357
typename Storage< float4 >::type Hypotheses
Definition sac_model.h:105
shared_ptr< typename Storage< float4 >::type > normals_
Definition sac_model.h:354
shared_ptr< const typename Storage< int >::type > IndicesConstPtr
Definition sac_model.h:99
thrust::minstd_rand rngl_
Linear-Congruent random number generator engine.
Definition sac_model.h:369
PointCloudAOS< Storage > PointCloud
Definition sac_model.h:90
shared_ptr< typename Storage< int >::type > IndicesPtr
Definition sac_model.h:98
typename Storage< float >::type Coefficients
Definition sac_model.h:101
typename Storage< int >::type Indices
Definition sac_model.h:97
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:353
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