Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
44namespace 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. */
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:
83 using SampleConsensusModel<Storage>::input_;
84 using SampleConsensusModel<Storage>::indices_;
85 using SampleConsensusModel<Storage>::rngl_;
86
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
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;
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);
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
shared_ptr< const PointCloud< PointT > > ConstPtr
SampleConsensusModel represents the base model class.
Definition sac_model.h:88
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< 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
SampleConsensusModelPlane defines a model for 3D plane segmentation.
int countWithinDistance(const Coefficients &model_coefficients, float threshold)
typename SampleConsensusModel< Storage >::Indices Indices
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.
typename SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
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)
shared_ptr< SampleConsensusModelPlane > Ptr
int countWithinDistance(const Hypotheses &h, int idx, float threshold)
typename PointCloud::Ptr PointCloudPtr
shared_ptr< const SampleConsensusModelPlane > ConstPtr
typename SampleConsensusModel< Storage >::Hypotheses Hypotheses
typename SampleConsensusModel< Storage >::Coefficients Coefficients
typename SampleConsensusModel< Storage >::IndicesPtr IndicesPtr
SampleConsensusModelPlane(const PointCloudConstPtr &cloud)
Constructor for base SampleConsensusModelPlane.
typename SampleConsensusModel< Storage >::Samples Samples
int selectWithinDistance(Hypotheses &h, int idx, float threshold, IndicesPtr &inliers_stencil, float3 &centroid)
bool generateModelHypotheses(Hypotheses &h, int max_iterations)
typename SampleConsensusModel< Storage >::PointCloud PointCloud
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