Point Cloud Library (PCL) 1.12.1
sac_model.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 <float.h>
41#include <thrust/sequence.h>
42#include <thrust/count.h>
43#include <thrust/remove.h>
44#include <pcl/cuda/point_cloud.h>
45#include <thrust/random/linear_congruential_engine.h>
46
47#include <pcl/pcl_exports.h>
48
49namespace pcl
50{
51 namespace cuda
52 {
53 // Forward declaration
54 //template<class T> class ProgressiveSampleConsensus;
55
56 /** \brief Check if a certain tuple is a point inlier. */
58 {
59 template <typename Tuple> __inline__ __host__ __device__ int
60 operator () (const Tuple &t);
61 };
62
63 /** \brief Check if a certain tuple is a point inlier. */
64 struct isInlier
65 {
66 __inline__ __host__ __device__ bool
67 operator()(int x) { return (x != -1); }
68 };
69
71 {
72 __inline__ __host__ __device__ bool
74 {
75#ifdef __CUDACC__
76 return (isnan (pt.x) | isnan (pt.y) | isnan (pt.z)) == 1;
77#else
78 return (std::isnan (pt.x) | std::isnan (pt.y) | std::isnan (pt.z)) == 1;
79#endif
80 }
81 };
82
83 /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit from
84 * this class.
85 */
86 template <template <typename> class Storage>
88 {
89 public:
93
94 using Ptr = shared_ptr<SampleConsensusModel>;
95 using ConstPtr = shared_ptr<const SampleConsensusModel>;
96
97 using Indices = typename Storage<int>::type;
98 using IndicesPtr = shared_ptr<typename Storage<int>::type>;
99 using IndicesConstPtr = shared_ptr<const typename Storage<int>::type>;
100
101 using Coefficients = typename Storage<float>::type;
102 using CoefficientsPtr = shared_ptr <Coefficients>;
103 using CoefficientsConstPtr = shared_ptr <const Coefficients>;
104
105 using Hypotheses = typename Storage<float4>::type;
106 //TODO: should be std::vector<int> instead of int. but currently, only 1point plane model supports this
107 using Samples = typename Storage<int>::type;
108
109 private:
110 /** \brief Empty constructor for base SampleConsensusModel. */
111 SampleConsensusModel () : radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
112 {};
113
114 public:
115 /** \brief Constructor for base SampleConsensusModel.
116 * \param cloud the input point cloud dataset
117 */
119 radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX)
120 {
121 // Sets the input cloud and creates a vector of "fake" indices
122 setInputCloud (cloud);
123 }
124
125 /* \brief Constructor for base SampleConsensusModel.
126 * \param cloud the input point cloud dataset
127 * \param indices a vector of point indices to be used from \a cloud
128 */
129 /* SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector<int> &indices) :
130 input_ (cloud),
131 indices_ (boost::make_shared <std::vector<int> > (indices)),
132 radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX)
133
134 {
135 if (indices_->size () > input_->points.size ())
136 {
137 ROS_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!", (unsigned long) indices_->size (), (unsigned long) input_->points.size ());
138 indices_->clear ();
139 }
140 };*/
141
142 /** \brief Destructor for base SampleConsensusModel. */
144
145 /** \brief Get a set of random data samples and return them as point
146 * indices. Pure virtual.
147 * \param iterations the internal number of iterations used by SAC methods
148 * \param samples the resultant model samples, <b>stored on the device</b>
149 */
150 virtual void
151 getSamples (int &iterations, Indices &samples) = 0;
152
153 /** \brief Check whether the given index samples can form a valid model,
154 * compute the model coefficients from these samples and store them
155 * in model_coefficients. Pure virtual.
156 * \param samples the point indices found as possible good candidates
157 * for creating a valid model, <b>stored on the device</b>
158 * \param model_coefficients the computed model coefficients
159 */
160 virtual bool
161 computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients) = 0;
162
163 virtual bool
164 generateModelHypotheses (Hypotheses &h, int max_iterations) = 0;
165
166 virtual bool
167 generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations) = 0;
168
169 virtual bool
170 isSampleInlier (IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
171 {return ((*inliers_stencil)[samples[i]] != -1);};
172
173 /* \brief Recompute the model coefficients using the given inlier set
174 * and return them to the user. Pure virtual.
175 *
176 * @note: these are the coefficients of the model after refinement
177 * (e.g., after a least-squares optimization)
178 *
179 * \param inliers the data inliers supporting the model
180 * \param model_coefficients the initial guess for the model coefficients
181 * \param optimized_coefficients the resultant recomputed coefficients
182 * after non-linear optimization
183 */
184 // virtual void
185 // optimizeModelCoefficients (const std::vector<int> &inliers,
186 // const Eigen::VectorXf &model_coefficients,
187 // Eigen::VectorXf &optimized_coefficients) = 0;
188
189 /* \brief Compute all distances from the cloud data to a given model. Pure virtual.
190 * \param model_coefficients the coefficients of a model that we need to
191 * compute distances to
192 * \param distances the resultant estimated distances
193 */
194 // virtual void
195 // getDistancesToModel (const Eigen::VectorXf &model_coefficients,
196 // std::vector<float> &distances) = 0;
197
198 /** \brief Select all the points which respect the given model
199 * coefficients as inliers. Pure virtual.
200 *
201 * \param model_coefficients the coefficients of a model that we need to
202 * compute distances to
203 * \param threshold a maximum admissible distance threshold for
204 * determining the inliers from the outliers
205 * \param inliers the resultant model inliers
206 * \param inliers_stencil
207 */
208 virtual int
209 selectWithinDistance (const Coefficients &model_coefficients,
210 float threshold,
211 IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
212 virtual int
214 float threshold,
215 IndicesPtr &inliers, IndicesPtr &inliers_stencil) = 0;
216 virtual int
218 float threshold,
219 IndicesPtr &inliers_stencil,
220 float3 &centroid) = 0;
221
222 virtual int
223 countWithinDistance (const Coefficients &model_coefficients, float threshold) = 0;
224
225 virtual int
226 countWithinDistance (const Hypotheses &h, int idx, float threshold) = 0;
227
228 int
229 deleteIndices (const IndicesPtr &indices_stencil );
230 int
231 deleteIndices (const Hypotheses &h, int idx, IndicesPtr &inliers, const IndicesPtr &inliers_delete);
232
233 /* \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
234 * \param inliers the data inliers that we want to project on the model
235 * \param model_coefficients the coefficients of a model
236 * \param projected_points the resultant projected points
237 * \param copy_data_fields set to true (default) if we want the \a
238 * projected_points cloud to be an exact copy of the input dataset minus
239 * the point projections on the plane model
240 */
241 // virtual void
242 // projectPoints (const std::vector<int> &inliers,
243 // const Eigen::VectorXf &model_coefficients,
244 // PointCloud &projected_points,
245 // bool copy_data_fields = true) = 0;
246
247 /* \brief Verify whether a subset of indices verifies a given set of
248 * model coefficients. Pure virtual.
249 *
250 * \param indices the data indices that need to be tested against the model
251 * \param model_coefficients the set of model coefficients
252 * \param threshold a maximum admissible distance threshold for
253 * determining the inliers from the outliers
254 */
255 // virtual bool
256 // doSamplesVerifyModel (const std::set<int> &indices,
257 // const Eigen::VectorXf &model_coefficients,
258 // float threshold) = 0;
259
260 /** \brief Provide a pointer to the input dataset
261 * \param cloud the const boost shared pointer to a PointCloud message
262 */
263 virtual void
265
266 /** \brief Get a pointer to the input point cloud dataset. */
267 inline PointCloudConstPtr
268 getInputCloud () const { return (input_); }
269
270 /* \brief Provide a pointer to the vector of indices that represents the input data.
271 * \param indices a pointer to the vector of indices that represents the input data.
272 */
273 // inline void
274 // setIndices (const IndicesPtr &indices) { indices_ = indices; }
275
276 /* \brief Provide the vector of indices that represents the input data.
277 * \param indices the vector of indices that represents the input data.
278 */
279 // inline void
280 // setIndices (std::vector<int> &indices)
281 // {
282 // indices_ = boost::make_shared <std::vector<int> > (indices);
283 // }
284
285 /** \brief Get a pointer to the vector of indices used. */
286 inline IndicesPtr
287 getIndices () const
288 {
289 if (nr_indices_in_stencil_ != indices_->size())
290 {
291 typename Indices::iterator last = thrust::remove_copy (indices_stencil_->begin (), indices_stencil_->end (), indices_->begin (), -1);
292 indices_->erase (last, indices_->end ());
293 }
294
295 return (indices_);
296 }
297
298 /* \brief Return an unique id for each type of model employed. */
299 // virtual SacModel
300 // getModelType () const = 0;
301
302 /* \brief Return the size of a sample from which a model is computed */
303 // inline unsigned int
304 // getSampleSize () const { return SAC_SAMPLE_SIZE.at (getModelType ()); }
305
306 /** \brief Set the minimum and maximum allowable radius limits for the
307 * model (applicable to models that estimate a radius)
308 * \param min_radius the minimum radius model
309 * \param max_radius the maximum radius model
310 * \todo change this to set limits on the entire model
311 */
312 inline void
313 setRadiusLimits (float min_radius, float max_radius)
314 {
315 radius_min_ = min_radius;
316 radius_max_ = max_radius;
317 }
318
319 /** \brief Get the minimum and maximum allowable radius limits for the
320 * model as set by the user.
321 *
322 * \param min_radius the resultant minimum radius model
323 * \param max_radius the resultant maximum radius model
324 */
325 inline void
326 getRadiusLimits (float &min_radius, float &max_radius)
327 {
328 min_radius = radius_min_;
329 max_radius = radius_max_;
330 }
331
332 // friend class ProgressiveSampleConsensus<PointT>;
333
334 inline shared_ptr<typename Storage<float4>::type>
335 getNormals () { return (normals_); }
336
337 inline
338 void setNormals (shared_ptr<typename Storage<float4>::type> normals) { normals_ = normals; }
339
340
341 protected:
342 /* \brief Check whether a model is valid given the user constraints.
343 * \param model_coefficients the set of model coefficients
344 */
345 // virtual inline bool
346 // isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
347
348 /** \brief A boost shared pointer to the point cloud data array. */
350 shared_ptr<typename Storage<float4>::type> normals_;
351
352 /** \brief A pointer to the vector of point indices to use. */
354 /** \brief A pointer to the vector of point indices (stencil) to use. */
356 /** \brief number of indices left in indices_stencil_ */
358
359 /** \brief The minimum and maximum radius limits for the model.
360 * Applicable to all models that estimate a radius.
361 */
363
364 /** \brief Linear-Congruent random number generator engine. */
365 thrust::minstd_rand rngl_;
366 };
367
368 /* \brief @b SampleConsensusModelFromNormals represents the base model class
369 * for models that require the use of surface normals for estimation.
370 */
371 // template <typename PointT, typename PointNT>
372 // class SampleConsensusModelFromNormals
373 // {
374 // public:
375 // using PointCloudNConstPtr = typename pcl::PointCloud<PointNT>::ConstPtr;
376 // using PointCloudNPtr = typename pcl::PointCloud<PointNT>::Ptr;
377 //
378 // using Ptr = shared_ptr<SampleConsensusModelFromNormals>;
379 // using ConstPtr = shared_ptr<const SampleConsensusModelFromNormals>;
380 //
381 // /* \brief Empty constructor for base SampleConsensusModelFromNormals. */
382 // SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0) {};
383 //
384 // /* \brief Set the normal angular distance weight.
385 // * \param w the relative weight (between 0 and 1) to give to the angular
386 // * distance (0 to pi/2) between point normals and the plane normal.
387 // * (The Euclidean distance will have weight 1-w.)
388 // */
389 // inline void
390 // setNormalDistanceWeight (float w) { normal_distance_weight_ = w; }
391 //
392 // /* \brief Get the normal angular distance weight. */
393 // inline float
394 // getNormalDistanceWeight () { return (normal_distance_weight_); }
395 //
396 // /* \brief Provide a pointer to the input dataset that contains the point
397 // * normals of the XYZ dataset.
398 // *
399 // * \param normals the const boost shared pointer to a PointCloud message
400 // */
401 // inline void
402 // setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
403 //
404 // /* \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
405 // inline PointCloudNConstPtr
406 // getInputNormals () { return (normals_); }
407 //
408 // protected:
409 // /* \brief The relative weight (between 0 and 1) to give to the angular
410 // * distance (0 to pi/2) between point normals and the plane normal.
411 // */
412 // float normal_distance_weight_;
413 //
414 // /* \brief A pointer to the input dataset that contains the point normals
415 // * of the XYZ dataset.
416 // */
417 // PointCloudNConstPtr normals_;
418 // };
419 } // namespace_
420} // namespace_
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing.
Definition: point_cloud.h:133
shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:200
shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:199
SampleConsensusModel represents the base model class.
Definition: sac_model.h:88
virtual int countWithinDistance(const Coefficients &model_coefficients, float threshold)=0
virtual int selectWithinDistance(const Coefficients &model_coefficients, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)=0
Select all the points which respect the given model coefficients as inliers.
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: sac_model.h:287
float radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:362
virtual bool computeModelCoefficients(const Indices &samples, Coefficients &model_coefficients)=0
Check whether the given index samples can form a valid model, compute the model coefficients from the...
IndicesPtr indices_stencil_
A pointer to the vector of point indices (stencil) to use.
Definition: sac_model.h:355
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:353
shared_ptr< const Coefficients > CoefficientsConstPtr
Definition: sac_model.h:103
void setNormals(shared_ptr< typename Storage< float4 >::type > normals)
Definition: sac_model.h:338
virtual int selectWithinDistance(Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 &centroid)=0
typename Storage< float4 >::type Hypotheses
Definition: sac_model.h:105
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:91
shared_ptr< typename Storage< float4 >::type > getNormals()
Definition: sac_model.h:335
shared_ptr< typename Storage< float4 >::type > normals_
Definition: sac_model.h:350
void setRadiusLimits(float min_radius, float max_radius)
Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate...
Definition: sac_model.h:313
virtual int countWithinDistance(const Hypotheses &h, int idx, float threshold)=0
int deleteIndices(const Hypotheses &h, int idx, IndicesPtr &inliers, const IndicesPtr &inliers_delete)
virtual bool generateModelHypotheses(Hypotheses &h, Samples &s, int max_iterations)=0
shared_ptr< const SampleConsensusModel > ConstPtr
Definition: sac_model.h:95
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: sac_model.h:268
virtual bool generateModelHypotheses(Hypotheses &h, int max_iterations)=0
SampleConsensusModel(const PointCloudConstPtr &cloud)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:118
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
Definition: sac_model.h:143
unsigned int nr_indices_in_stencil_
number of indices left in indices_stencil_
Definition: sac_model.h:357
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:365
shared_ptr< Coefficients > CoefficientsPtr
Definition: sac_model.h:102
virtual void getSamples(int &iterations, Indices &samples)=0
Get a set of random data samples and return them as point indices.
virtual int selectWithinDistance(const Hypotheses &h, int idx, float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil)=0
shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:98
int deleteIndices(const IndicesPtr &indices_stencil)
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:92
typename Storage< float >::type Coefficients
Definition: sac_model.h:101
virtual bool isSampleInlier(IndicesPtr &inliers_stencil, Samples &samples, unsigned int &i)
Definition: sac_model.h:170
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:349
shared_ptr< SampleConsensusModel > Ptr
Definition: sac_model.h:94
typename Storage< int >::type Samples
Definition: sac_model.h:107
void getRadiusLimits(float &min_radius, float &max_radius)
Get the minimum and maximum allowable radius limits for the model as set by the user.
Definition: sac_model.h:326
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
Check if a certain tuple is a point inlier.
Definition: sac_model.h:58
__inline__ __host__ __device__ int operator()(const Tuple &t)
Default point xyz-rgb structure.
Definition: point_types.h:49
Check if a certain tuple is a point inlier.
Definition: predicate.h:60
__inline__ __host__ __device__ bool operator()(int x)
Definition: sac_model.h:67
__inline__ __host__ __device__ bool operator()(PointXYZRGB pt)
Definition: sac_model.h:73