Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sac_model_cylinder.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
43 
44 #include <pcl/sample_consensus/eigen.h>
45 #include <pcl/sample_consensus/sac_model_cylinder.h>
46 #include <pcl/common/concatenate.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT, typename PointNT> bool
51 {
52  if (samples.size () != sample_size_)
53  {
54  PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
55  return (false);
56  }
57  return (true);
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT, typename PointNT> bool
63  const Indices &samples, Eigen::VectorXf &model_coefficients) const
64 {
65  // Need 2 samples
66  if (samples.size () != sample_size_)
67  {
68  PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
69  return (false);
70  }
71 
72  if (!normals_)
73  {
74  PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n");
75  return (false);
76  }
77 
78  if (std::abs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
79  std::abs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
80  std::abs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
81  {
82  return (false);
83  }
84 
85  Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0.0f);
86  Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0.0f);
87 
88  Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0.0f);
89  Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0.0f);
90  Eigen::Vector4f w = n1 + p1 - p2;
91 
92  float a = n1.dot (n1);
93  float b = n1.dot (n2);
94  float c = n2.dot (n2);
95  float d = n1.dot (w);
96  float e = n2.dot (w);
97  float denominator = a*c - b*b;
98  float sc, tc;
99  // Compute the line parameters of the two closest points
100  if (denominator < 1e-8) // The lines are almost parallel
101  {
102  sc = 0.0f;
103  tc = (b > c ? d / b : e / c); // Use the largest denominator
104  }
105  else
106  {
107  sc = (b*e - c*d) / denominator;
108  tc = (a*e - b*d) / denominator;
109  }
110 
111  // point_on_axis, axis_direction
112  Eigen::Vector4f line_pt = p1 + n1 + sc * n1;
113  Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
114  line_dir.normalize ();
115 
116  model_coefficients.resize (model_size_);
117  // model_coefficients.template head<3> () = line_pt.template head<3> ();
118  model_coefficients[0] = line_pt[0];
119  model_coefficients[1] = line_pt[1];
120  model_coefficients[2] = line_pt[2];
121  // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
122  model_coefficients[3] = line_dir[0];
123  model_coefficients[4] = line_dir[1];
124  model_coefficients[5] = line_dir[2];
125  // cylinder radius
126  model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)));
127 
128  if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
129  return (false);
130 
131  return (true);
132 }
133 
134 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
135 template <typename PointT, typename PointNT> void
137  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
138 {
139  // Check if the model is valid given the user constraints
140  if (!isModelValid (model_coefficients))
141  {
142  distances.clear ();
143  return;
144  }
145 
146  distances.resize (indices_->size ());
147 
148  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
149  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
150  float ptdotdir = line_pt.dot (line_dir);
151  float dirdotdir = 1.0f / line_dir.dot (line_dir);
152  // Iterate through the 3d points and calculate the distances from them to the sphere
153  for (std::size_t i = 0; i < indices_->size (); ++i)
154  {
155  // Approximate the distance from the point to the cylinder as the difference between
156  // dist(point,cylinder_axis) and cylinder radius
157  // @note need to revise this.
158  Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
159  Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
160 
161  double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
162 
163  // Calculate the point's projection on the cylinder axis
164  float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
165  Eigen::Vector4f pt_proj = line_pt + k * line_dir;
166  Eigen::Vector4f dir = pt - pt_proj;
167  dir.normalize ();
168 
169  // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
170  double d_normal = std::abs (getAngle3D (n, dir));
171  d_normal = (std::min) (d_normal, M_PI - d_normal);
172 
173  distances[i] = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
174  }
175 }
176 
177 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
178 template <typename PointT, typename PointNT> void
180  const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
181 {
182  // Check if the model is valid given the user constraints
183  if (!isModelValid (model_coefficients))
184  {
185  inliers.clear ();
186  return;
187  }
188 
189  inliers.clear ();
190  error_sqr_dists_.clear ();
191  inliers.reserve (indices_->size ());
192  error_sqr_dists_.reserve (indices_->size ());
193 
194  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
195  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
196  float ptdotdir = line_pt.dot (line_dir);
197  float dirdotdir = 1.0f / line_dir.dot (line_dir);
198  // Iterate through the 3d points and calculate the distances from them to the sphere
199  for (std::size_t i = 0; i < indices_->size (); ++i)
200  {
201  // Approximate the distance from the point to the cylinder as the difference between
202  // dist(point,cylinder_axis) and cylinder radius
203  Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
204  Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
205  double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
206 
207  // Calculate the point's projection on the cylinder axis
208  float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
209  Eigen::Vector4f pt_proj = line_pt + k * line_dir;
210  Eigen::Vector4f dir = pt - pt_proj;
211  dir.normalize ();
212 
213  // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
214  double d_normal = std::abs (getAngle3D (n, dir));
215  d_normal = (std::min) (d_normal, M_PI - d_normal);
216 
217  double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
218  if (distance < threshold)
219  {
220  // Returns the indices of the points whose distances are smaller than the threshold
221  inliers.push_back ((*indices_)[i]);
222  error_sqr_dists_.push_back (distance);
223  }
224  }
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename PointT, typename PointNT> std::size_t
230  const Eigen::VectorXf &model_coefficients, const double threshold) const
231 {
232  // Check if the model is valid given the user constraints
233  if (!isModelValid (model_coefficients))
234  return (0);
235 
236  std::size_t nr_p = 0;
237 
238  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
239  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
240  float ptdotdir = line_pt.dot (line_dir);
241  float dirdotdir = 1.0f / line_dir.dot (line_dir);
242  // Iterate through the 3d points and calculate the distances from them to the sphere
243  for (std::size_t i = 0; i < indices_->size (); ++i)
244  {
245  // Approximate the distance from the point to the cylinder as the difference between
246  // dist(point,cylinder_axis) and cylinder radius
247  Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f);
248  Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f);
249  double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
250 
251  // Calculate the point's projection on the cylinder axis
252  float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
253  Eigen::Vector4f pt_proj = line_pt + k * line_dir;
254  Eigen::Vector4f dir = pt - pt_proj;
255  dir.normalize ();
256 
257  // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
258  double d_normal = std::abs (getAngle3D (n, dir));
259  d_normal = (std::min) (d_normal, M_PI - d_normal);
260 
261  if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold)
262  nr_p++;
263  }
264  return (nr_p);
265 }
266 
267 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
268 template <typename PointT, typename PointNT> void
270  const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
271 {
272  optimized_coefficients = model_coefficients;
273 
274  // Needs a set of valid model coefficients
275  if (!isModelValid (model_coefficients))
276  {
277  PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Given model is invalid!\n");
278  return;
279  }
280 
281  // Need more than the minimum sample size to make a difference
282  if (inliers.size () <= sample_size_)
283  {
284  PCL_ERROR ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
285  return;
286  }
287 
288  OptimizationFunctor functor (this, inliers);
289  Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
290  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
291  int info = lm.minimize (optimized_coefficients);
292 
293  // Compute the L2 norm of the residuals
294  PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
295  info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
296  model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
297 
298  Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
299  line_dir.normalize ();
300  optimized_coefficients[3] = line_dir[0];
301  optimized_coefficients[4] = line_dir[1];
302  optimized_coefficients[5] = line_dir[2];
303 }
304 
305 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
306 template <typename PointT, typename PointNT> void
308  const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
309 {
310  // Needs a valid set of model coefficients
311  if (!isModelValid (model_coefficients))
312  {
313  PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n");
314  return;
315  }
316 
317  projected_points.header = input_->header;
318  projected_points.is_dense = input_->is_dense;
319 
320  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
321  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
322  float ptdotdir = line_pt.dot (line_dir);
323  float dirdotdir = 1.0f / line_dir.dot (line_dir);
324 
325  // Copy all the data fields from the input cloud to the projected one?
326  if (copy_data_fields)
327  {
328  // Allocate enough space and copy the basics
329  projected_points.points.resize (input_->points.size ());
330  projected_points.width = input_->width;
331  projected_points.height = input_->height;
332 
333  using FieldList = typename pcl::traits::fieldList<PointT>::type;
334  // Iterate over each point
335  for (std::size_t i = 0; i < projected_points.points.size (); ++i)
336  // Iterate over each dimension
337  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
338 
339  // Iterate through the 3d points and calculate the distances from them to the cylinder
340  for (const auto &inlier : inliers)
341  {
342  Eigen::Vector4f p (input_->points[inlier].x,
343  input_->points[inlier].y,
344  input_->points[inlier].z,
345  1);
346 
347  float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
348 
349  pcl::Vector4fMap pp = projected_points.points[inlier].getVector4fMap ();
350  pp.matrix () = line_pt + k * line_dir;
351 
352  Eigen::Vector4f dir = p - pp;
353  dir.normalize ();
354 
355  // Calculate the projection of the point onto the cylinder
356  pp += dir * model_coefficients[6];
357  }
358  }
359  else
360  {
361  // Allocate enough space and copy the basics
362  projected_points.points.resize (inliers.size ());
363  projected_points.width = static_cast<std::uint32_t> (inliers.size ());
364  projected_points.height = 1;
365 
366  using FieldList = typename pcl::traits::fieldList<PointT>::type;
367  // Iterate over each point
368  for (std::size_t i = 0; i < inliers.size (); ++i)
369  // Iterate over each dimension
370  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
371 
372  // Iterate through the 3d points and calculate the distances from them to the cylinder
373  for (std::size_t i = 0; i < inliers.size (); ++i)
374  {
375  pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
376  pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap ();
377 
378  float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
379  // Calculate the projection of the point on the line
380  pp.matrix () = line_pt + k * line_dir;
381 
382  Eigen::Vector4f dir = p - pp;
383  dir.normalize ();
384 
385  // Calculate the projection of the point onto the cylinder
386  pp += dir * model_coefficients[6];
387  }
388  }
389 }
390 
391 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
392 template <typename PointT, typename PointNT> bool
394  const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
395 {
396  // Needs a valid model coefficients
397  if (!isModelValid (model_coefficients))
398  {
399  PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Given model is invalid!\n");
400  return (false);
401  }
402 
403  for (const auto &index : indices)
404  {
405  // Approximate the distance from the point to the cylinder as the difference between
406  // dist(point,cylinder_axis) and cylinder radius
407  // @note need to revise this.
408  Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0.0f);
409  if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold)
410  return (false);
411  }
412 
413  return (true);
414 }
415 
416 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
417 template <typename PointT, typename PointNT> double
419  const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
420 {
421  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
422  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
423  return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir));
424 }
425 
426 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
427 template <typename PointT, typename PointNT> void
429  const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const
430 {
431  Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
432  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
433 
434  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir);
435  pt_proj = line_pt + k * line_dir;
436 
437  Eigen::Vector4f dir = pt - pt_proj;
438  dir.normalize ();
439 
440  // Calculate the projection of the point onto the cylinder
441  pt_proj += dir * model_coefficients[6];
442 }
443 
444 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
445 template <typename PointT, typename PointNT> bool
446 pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
447 {
448  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
449  return (false);
450 
451  // Check against template, if given
452  if (eps_angle_ > 0.0)
453  {
454  // Obtain the cylinder direction
455  const Eigen::Vector3f coeff(model_coefficients[3], model_coefficients[4], model_coefficients[5]);
456 
457  double angle_diff = std::abs (getAngle3D (axis_, coeff));
458  angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
459  // Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis
460  if (angle_diff > eps_angle_)
461  return (false);
462  }
463 
464  if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[6] < radius_min_)
465  return (false);
466  if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[6] > radius_max_)
467  return (false);
468 
469  return (true);
470 }
471 
472 #define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder<PointT, PointNT>;
473 
474 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
475 
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the cylinder model.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree. ...
Definition: common.hpp:46
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
double pointToLineDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
Get the distance from a point to a line (represented by a point and a direction)
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cylinder model.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cylinder model, compute the model coefficients...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given cylinder model coefficients.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:70
double sqrPointToLineDistance(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
Get the square distance from a point to a line (represented by a point and a direction) ...
Definition: distances.h:71
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cylinder coefficients using the given inlier set and return them to the user...
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void projectPointToCylinder(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const
Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, cylinder_radius_R)
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
Helper functor structure for concatenate.
Definition: concatenate.h:51
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407