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