Point Cloud Library (PCL)  1.11.1-dev
sac_model_cone.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, 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  */
38 
39 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
40 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
41 
42 #include <pcl/sample_consensus/eigen.h>
43 #include <pcl/sample_consensus/sac_model_cone.h>
44 #include <pcl/common/common.h> // for getAngle3D
45 #include <pcl/common/concatenate.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT, typename PointNT> bool
50 {
51  if (samples.size () != sample_size_)
52  {
53  PCL_ERROR ("[pcl::SampleConsensusModelCone::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
54  return (false);
55  }
56  return (true);
57 }
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename PointT, typename PointNT> bool
62  const Indices &samples, Eigen::VectorXf &model_coefficients) const
63 {
64  // Need 3 samples
65  if (samples.size () != sample_size_)
66  {
67  PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
68  return (false);
69  }
70 
71  if (!normals_)
72  {
73  PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n");
74  return (false);
75  }
76 
77  Eigen::Vector4f p1 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z, 0.0f);
78  Eigen::Vector4f p2 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z, 0.0f);
79  Eigen::Vector4f p3 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z, 0.0f);
80 
81  Eigen::Vector4f n1 ((*normals_)[samples[0]].normal[0], (*normals_)[samples[0]].normal[1], (*normals_)[samples[0]].normal[2], 0.0f);
82  Eigen::Vector4f n2 ((*normals_)[samples[1]].normal[0], (*normals_)[samples[1]].normal[1], (*normals_)[samples[1]].normal[2], 0.0f);
83  Eigen::Vector4f n3 ((*normals_)[samples[2]].normal[0], (*normals_)[samples[2]].normal[1], (*normals_)[samples[2]].normal[2], 0.0f);
84 
85  //calculate apex (intersection of the three planes defined by points and belonging normals
86  Eigen::Vector4f ortho12 = n1.cross3(n2);
87  Eigen::Vector4f ortho23 = n2.cross3(n3);
88  Eigen::Vector4f ortho31 = n3.cross3(n1);
89 
90  float denominator = n1.dot(ortho23);
91 
92  float d1 = p1.dot (n1);
93  float d2 = p2.dot (n2);
94  float d3 = p3.dot (n3);
95 
96  Eigen::Vector4f apex = (d1 * ortho23 + d2 * ortho31 + d3 * ortho12) / denominator;
97 
98  //compute axis (normal of plane defined by: { apex+(p1-apex)/(||p1-apex||), apex+(p2-apex)/(||p2-apex||), apex+(p3-apex)/(||p3-apex||)}
99  Eigen::Vector4f ap1 = p1 - apex;
100  Eigen::Vector4f ap2 = p2 - apex;
101  Eigen::Vector4f ap3 = p3 - apex;
102 
103  Eigen::Vector4f np1 = apex + (ap1/ap1.norm ());
104  Eigen::Vector4f np2 = apex + (ap2/ap2.norm ());
105  Eigen::Vector4f np3 = apex + (ap3/ap3.norm ());
106 
107  Eigen::Vector4f np1np2 = np2 - np1;
108  Eigen::Vector4f np1np3 = np3 - np1;
109 
110  Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3);
111  axis_dir.normalize ();
112 
113  // normalize the vector (apex->p) for opening angle calculation
114  ap1.normalize ();
115  ap2.normalize ();
116  ap3.normalize ();
117 
118  //compute opening angle
119  float opening_angle = ( std::acos (ap1.dot (axis_dir)) + std::acos (ap2.dot (axis_dir)) + std::acos (ap3.dot (axis_dir)) ) / 3.0f;
120 
121  model_coefficients.resize (model_size_);
122  // model_coefficients.template head<3> () = line_pt.template head<3> ();
123  model_coefficients[0] = apex[0];
124  model_coefficients[1] = apex[1];
125  model_coefficients[2] = apex[2];
126  // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
127  model_coefficients[3] = axis_dir[0];
128  model_coefficients[4] = axis_dir[1];
129  model_coefficients[5] = axis_dir[2];
130  // cone radius
131  model_coefficients[6] = opening_angle;
132 
133  if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
134  return (false);
135  if (model_coefficients[6] != std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
136  return (false);
137 
138  return (true);
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointT, typename PointNT> void
144  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
145 {
146  // Check if the model is valid given the user constraints
147  if (!isModelValid (model_coefficients))
148  {
149  distances.clear ();
150  return;
151  }
152 
153  distances.resize (indices_->size ());
154 
155  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
156  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
157  float opening_angle = model_coefficients[6];
158 
159  float apexdotdir = apex.dot (axis_dir);
160  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
161  // Iterate through the 3d points and calculate the distances from them to the cone
162  for (std::size_t i = 0; i < indices_->size (); ++i)
163  {
164  Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
165 
166  // Calculate the point's projection on the cone axis
167  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
168  Eigen::Vector4f pt_proj = apex + k * axis_dir;
169 
170  // Calculate the actual radius of the cone at the level of the projected point
171  Eigen::Vector4f height = apex - pt_proj;
172  float actual_cone_radius = tanf (opening_angle) * height.norm ();
173 
174  // Approximate the distance from the point to the cone as the difference between
175  // dist(point,cone_axis) and actual cone radius
176  const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
177 
178  // Calculate the direction of the point from center
179  Eigen::Vector4f dir = pt - pt_proj;
180  dir.normalize ();
181 
182  // Calculate the cones perfect normals
183  height.normalize ();
184  Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * dir;
185 
186  // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
187  Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
188  double d_normal = std::abs (getAngle3D (n, cone_normal));
189  d_normal = (std::min) (d_normal, M_PI - d_normal);
190 
191  distances[i] = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
192  }
193 }
194 
195 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
196 template <typename PointT, typename PointNT> void
198  const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
199 {
200  // Check if the model is valid given the user constraints
201  if (!isModelValid (model_coefficients))
202  {
203  inliers.clear ();
204  return;
205  }
206 
207  inliers.clear ();
208  error_sqr_dists_.clear ();
209  inliers.reserve (indices_->size ());
210  error_sqr_dists_.reserve (indices_->size ());
211 
212  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
213  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
214  float opening_angle = model_coefficients[6];
215 
216  float apexdotdir = apex.dot (axis_dir);
217  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
218  // Iterate through the 3d points and calculate the distances from them to the cone
219  for (std::size_t i = 0; i < indices_->size (); ++i)
220  {
221  Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
222 
223  // Calculate the point's projection on the cone axis
224  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
225  Eigen::Vector4f pt_proj = apex + k * axis_dir;
226 
227  // Calculate the actual radius of the cone at the level of the projected point
228  Eigen::Vector4f height = apex - pt_proj;
229  double actual_cone_radius = tan(opening_angle) * height.norm ();
230 
231  // Approximate the distance from the point to the cone as the difference between
232  // dist(point,cone_axis) and actual cone radius
233  const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
234  if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
235  continue;
236 
237  // Calculate the direction of the point from center
238  Eigen::Vector4f pp_pt_dir = pt - pt_proj;
239  pp_pt_dir.normalize ();
240 
241  // Calculate the cones perfect normals
242  height.normalize ();
243  Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir;
244 
245  // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
246  Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
247  double d_normal = std::abs (getAngle3D (n, cone_normal));
248  d_normal = (std::min) (d_normal, M_PI - d_normal);
249 
250  double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
251 
252  if (distance < threshold)
253  {
254  // Returns the indices of the points whose distances are smaller than the threshold
255  inliers.push_back ((*indices_)[i]);
256  error_sqr_dists_.push_back (distance);
257  }
258  }
259 }
260 
261 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
262 template <typename PointT, typename PointNT> std::size_t
264  const Eigen::VectorXf &model_coefficients, const double threshold) const
265 {
266 
267  // Check if the model is valid given the user constraints
268  if (!isModelValid (model_coefficients))
269  return (0);
270 
271  std::size_t nr_p = 0;
272 
273  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
274  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
275  float opening_angle = model_coefficients[6];
276 
277  float apexdotdir = apex.dot (axis_dir);
278  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
279  // Iterate through the 3d points and calculate the distances from them to the cone
280  for (std::size_t i = 0; i < indices_->size (); ++i)
281  {
282  Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
283 
284  // Calculate the point's projection on the cone axis
285  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
286  Eigen::Vector4f pt_proj = apex + k * axis_dir;
287 
288  // Calculate the actual radius of the cone at the level of the projected point
289  Eigen::Vector4f height = apex - pt_proj;
290  double actual_cone_radius = tan(opening_angle) * height.norm ();
291 
292  // Approximate the distance from the point to the cone as the difference between
293  // dist(point,cone_axis) and actual cone radius
294  const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
295  if (weighted_euclid_dist > threshold) // Early termination: cannot be an inlier
296  continue;
297 
298  // Calculate the direction of the point from center
299  Eigen::Vector4f pp_pt_dir = pt - pt_proj;
300  pp_pt_dir.normalize ();
301 
302  // Calculate the cones perfect normals
303  height.normalize ();
304  Eigen::Vector4f cone_normal = sinf (opening_angle) * height + std::cos (opening_angle) * pp_pt_dir;
305 
306  // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
307  Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
308  double d_normal = std::abs (getAngle3D (n, cone_normal));
309  d_normal = (std::min) (d_normal, M_PI - d_normal);
310 
311  if (std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist) < threshold)
312  nr_p++;
313  }
314  return (nr_p);
315 }
316 
317 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
318 template <typename PointT, typename PointNT> void
320  const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
321 {
322  optimized_coefficients = model_coefficients;
323 
324  // Needs a set of valid model coefficients
325  if (!isModelValid (model_coefficients))
326  {
327  PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Given model is invalid!\n");
328  return;
329  }
330 
331  // Need more than the minimum sample size to make a difference
332  if (inliers.size () <= sample_size_)
333  {
334  PCL_ERROR ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
335  return;
336  }
337 
338  OptimizationFunctor functor (this, inliers);
339  Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
340  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
341  int info = lm.minimize (optimized_coefficients);
342 
343  // Compute the L2 norm of the residuals
344  PCL_DEBUG ("[pcl::SampleConsensusModelCone::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",
345  info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
346  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]);
347 
348  Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
349  line_dir.normalize ();
350  optimized_coefficients[3] = line_dir[0];
351  optimized_coefficients[4] = line_dir[1];
352  optimized_coefficients[5] = line_dir[2];
353 }
354 
355 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
356 template <typename PointT, typename PointNT> void
358  const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
359 {
360  // Needs a valid set of model coefficients
361  if (!isModelValid (model_coefficients))
362  {
363  PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Given model is invalid!\n");
364  return;
365  }
366 
367  projected_points.header = input_->header;
368  projected_points.is_dense = input_->is_dense;
369 
370  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
371  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
372  float opening_angle = model_coefficients[6];
373 
374  float apexdotdir = apex.dot (axis_dir);
375  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
376 
377  // Copy all the data fields from the input cloud to the projected one?
378  if (copy_data_fields)
379  {
380  // Allocate enough space and copy the basics
381  projected_points.resize (input_->size ());
382  projected_points.width = input_->width;
383  projected_points.height = input_->height;
384 
385  using FieldList = typename pcl::traits::fieldList<PointT>::type;
386  // Iterate over each point
387  for (std::size_t i = 0; i < projected_points.size (); ++i)
388  // Iterate over each dimension
389  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
390 
391  // Iterate through the 3d points and calculate the distances from them to the cone
392  for (const auto &inlier : inliers)
393  {
394  Eigen::Vector4f pt ((*input_)[inlier].x,
395  (*input_)[inlier].y,
396  (*input_)[inlier].z,
397  1);
398 
399  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
400 
401  pcl::Vector4fMap pp = projected_points[inlier].getVector4fMap ();
402  pp.matrix () = apex + k * axis_dir;
403 
404  Eigen::Vector4f dir = pt - pp;
405  dir.normalize ();
406 
407  // Calculate the actual radius of the cone at the level of the projected point
408  Eigen::Vector4f height = apex - pp;
409  float actual_cone_radius = tanf (opening_angle) * height.norm ();
410 
411  // Calculate the projection of the point onto the cone
412  pp += dir * actual_cone_radius;
413  }
414  }
415  else
416  {
417  // Allocate enough space and copy the basics
418  projected_points.resize (inliers.size ());
419  projected_points.width = inliers.size ();
420  projected_points.height = 1;
421 
422  using FieldList = typename pcl::traits::fieldList<PointT>::type;
423  // Iterate over each point
424  for (std::size_t i = 0; i < inliers.size (); ++i)
425  // Iterate over each dimension
426  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
427 
428  // Iterate through the 3d points and calculate the distances from them to the cone
429  for (std::size_t i = 0; i < inliers.size (); ++i)
430  {
431  pcl::Vector4fMap pp = projected_points[i].getVector4fMap ();
432  pcl::Vector4fMapConst pt = (*input_)[inliers[i]].getVector4fMap ();
433 
434  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
435  // Calculate the projection of the point on the line
436  pp.matrix () = apex + k * axis_dir;
437 
438  Eigen::Vector4f dir = pt - pp;
439  dir.normalize ();
440 
441  // Calculate the actual radius of the cone at the level of the projected point
442  Eigen::Vector4f height = apex - pp;
443  float actual_cone_radius = tanf (opening_angle) * height.norm ();
444 
445  // Calculate the projection of the point onto the cone
446  pp += dir * actual_cone_radius;
447  }
448  }
449 }
450 
451 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
452 template <typename PointT, typename PointNT> bool
454  const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
455 {
456  // Needs a valid model coefficients
457  if (!isModelValid (model_coefficients))
458  {
459  PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Given model is invalid!\n");
460  return (false);
461  }
462 
463  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
464  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
465  float openning_angle = model_coefficients[6];
466 
467  float apexdotdir = apex.dot (axis_dir);
468  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
469 
470  // Iterate through the 3d points and calculate the distances from them to the cone
471  for (const auto &index : indices)
472  {
473  Eigen::Vector4f pt ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z, 0.0f);
474 
475  // Calculate the point's projection on the cone axis
476  float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
477  Eigen::Vector4f pt_proj = apex + k * axis_dir;
478  Eigen::Vector4f dir = pt - pt_proj;
479  dir.normalize ();
480 
481  // Calculate the actual radius of the cone at the level of the projected point
482  Eigen::Vector4f height = apex - pt_proj;
483  double actual_cone_radius = tan (openning_angle) * height.norm ();
484 
485  // Approximate the distance from the point to the cone as the difference between
486  // dist(point,cone_axis) and actual cone radius
487  if (std::abs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
488  return (false);
489  }
490 
491  return (true);
492 }
493 
494 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
495 template <typename PointT, typename PointNT> double
497  const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
498 {
499  Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
500  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
501  return sqrt(pcl::sqrPointToLineDistance (pt, apex, axis_dir));
502 }
503 
504 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
505 template <typename PointT, typename PointNT> bool
506 pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
507 {
508  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
509  return (false);
510 
511  // Check against template, if given
512  if (eps_angle_ > 0.0)
513  {
514  // Obtain the cone direction
515  const Eigen::Vector3f coeff(model_coefficients[3], model_coefficients[4], model_coefficients[5]);
516 
517  double angle_diff = std::abs (getAngle3D (axis_, coeff));
518  angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
519  // Check whether the current cone model satisfies our angle threshold criterion with respect to the given axis
520  if (angle_diff > eps_angle_)
521  return (false);
522  }
523 
524  if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
525  return (false);
526  if (model_coefficients[6] != std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
527  return (false);
528 
529  return (true);
530 }
531 
532 #define PCL_INSTANTIATE_SampleConsensusModelCone(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCone<PointT, PointNT>;
533 
534 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
535 
pcl::SampleConsensusModelCone::countWithinDistance
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.
Definition: sac_model_cone.hpp:263
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::SampleConsensusModelCone::isModelValid
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
Definition: sac_model_cone.hpp:506
common.h
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::SampleConsensusModelCone::doSamplesVerifyModel
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 cone model coefficients.
Definition: sac_model_cone.hpp:453
pcl::NdConcatenateFunctor
Helper functor structure for concatenate.
Definition: concatenate.h:49
pcl::PointCloud< pcl::PointXYZRGB >
pcl::SampleConsensusModelCone::projectPoints
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 cone model.
Definition: sac_model_cone.hpp:357
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::SampleConsensusModelCone::pointToAxisDistance
double pointToAxisDistance(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)
Definition: sac_model_cone.hpp:496
pcl::SampleConsensusModelCone::getDistancesToModel
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cone model.
Definition: sac_model_cone.hpp:143
pcl::getAngle3D
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:47
M_PI
#define M_PI
Definition: pcl_macros.h:201
pcl::SampleConsensusModelCone::isSampleGood
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
Definition: sac_model_cone.hpp:49
pcl::PointCloud::is_dense
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:397
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:386
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::SampleConsensusModel
SampleConsensusModel represents the base model class.
Definition: sac_model.h:69
pcl::Vector4fMap
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
Definition: point_types.hpp:182
pcl::SampleConsensusModelCone::selectWithinDistance
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.
Definition: sac_model_cone.hpp:197
pcl::SampleConsensusModelCone::computeModelCoefficients
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
Definition: sac_model_cone.hpp:61
pcl::sqrPointToLineDistance
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:75
pcl::SampleConsensusModelCone::optimizeModelCoefficients
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cone coefficients using the given inlier set and return them to the user.
Definition: sac_model_cone.hpp:319
pcl::Vector4fMapConst
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
Definition: point_types.hpp:183