Point Cloud Library (PCL)  1.15.1-dev
sac_model_torus.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_TORUS_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
43 
44 // clang-format off
45 #include <pcl/sample_consensus/sac_model_torus.h>
46 #include <pcl/common/concatenate.h>
47 // clang-format on
48 
49 #include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT, typename PointNT>
52 bool
54  const Indices& samples) const
55 {
56  if (samples.size() != sample_size_) {
57  PCL_ERROR("[pcl::SampleConsensusTorus::isSampleGood] Wrong number of samples (is "
58  "%lu, should be %lu)!\n",
59  samples.size(),
60  sample_size_);
61  return (false);
62  }
63 
64  Eigen::Vector3f n0 = Eigen::Vector3f((*normals_)[samples[0]].getNormalVector3fMap());
65  Eigen::Vector3f n1 = Eigen::Vector3f((*normals_)[samples[1]].getNormalVector3fMap());
66  Eigen::Vector3f n2 = Eigen::Vector3f((*normals_)[samples[2]].getNormalVector3fMap());
67  Eigen::Vector3f n3 = Eigen::Vector3f((*normals_)[samples[3]].getNormalVector3fMap());
68 
69  // Required for numeric stability on computeModelCoefficients
70  if (std::abs((n0).cross(n1).squaredNorm()) <
71  Eigen::NumTraits<float>::dummy_precision() ||
72  std::abs((n0).cross(n2).squaredNorm()) <
73  Eigen::NumTraits<float>::dummy_precision() ||
74  std::abs((n0).cross(n3).squaredNorm()) <
75  Eigen::NumTraits<float>::dummy_precision() ||
76  std::abs((n1).cross(n2).squaredNorm()) <
77  Eigen::NumTraits<float>::dummy_precision() ||
78  std::abs((n1).cross(n3).squaredNorm()) <
79  Eigen::NumTraits<float>::dummy_precision()) {
80  PCL_ERROR("[pcl::SampleConsensusModelEllipse3D::isSampleGood] Sample points "
81  "normals too similar or collinear!\n");
82  return (false);
83  }
84  return (true);
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
88 float
89 crossDot(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2, const Eigen::Vector3f& v3)
90 {
91  return v1.cross(v2).dot(v3);
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointT, typename PointNT>
96 bool
98  const Indices& samples, Eigen::VectorXf& model_coefficients) const
99 {
100 
101  // Make sure that the samples are valid
102  if (!isSampleGood(samples)) {
103  PCL_ERROR("[pcl::SampleConsensusModelTorus::computeModelCoefficients] Invalid set "
104  "of samples given!\n");
105  return (false);
106  }
107 
108  if (!normals_) {
109  PCL_ERROR("[pcl::SampleConsensusModelTorus::computeModelCoefficients] No input "
110  "dataset containing normals was given!\n");
111  return (false);
112  }
113  // Find axis using:
114 
115  // @article{article,
116  // author = {Lukacs, G. and Marshall, David and Martin, R.},
117  // year = {2001},
118  // month = {09},
119  // pages = {},
120  // title = {Geometric Least-Squares Fitting of Spheres, Cylinders, Cones and Tori}
121  //}
122 
123  const Eigen::Vector3f n0 = Eigen::Vector3f((*normals_)[samples[0]].getNormalVector3fMap());
124  const Eigen::Vector3f n1 = Eigen::Vector3f((*normals_)[samples[1]].getNormalVector3fMap());
125  const Eigen::Vector3f n2 = Eigen::Vector3f((*normals_)[samples[2]].getNormalVector3fMap());
126  const Eigen::Vector3f n3 = Eigen::Vector3f((*normals_)[samples[3]].getNormalVector3fMap());
127 
128  const Eigen::Vector3f p0 = Eigen::Vector3f((*input_)[samples[0]].getVector3fMap());
129  const Eigen::Vector3f p1 = Eigen::Vector3f((*input_)[samples[1]].getVector3fMap());
130  const Eigen::Vector3f p2 = Eigen::Vector3f((*input_)[samples[2]].getVector3fMap());
131  const Eigen::Vector3f p3 = Eigen::Vector3f((*input_)[samples[3]].getVector3fMap());
132 
133  const float a01 = crossDot(n0, n1, n2);
134  const float b01 = crossDot(n0, n1, n3);
135  const float a0 = crossDot(p2 - p1, n0, n2);
136  const float a1 = crossDot(p0 - p2, n1, n2);
137  const float b0 = crossDot(p3 - p1, n0, n3);
138  const float b1 = crossDot(p0 - p3, n1, n3);
139  const float a = crossDot(p0 - p2, p1 - p0, n2);
140  const float b = crossDot(p0 - p3, p1 - p0, n3);
141 
142  // a10*t0*t1 + a0*t0 + a1*t1 + a = 0
143  // b10*t0*t1 + b0*t0 + b1*t1 + b = 0
144  //
145  // (a0 - b0*a10/b10)* t0 + (a1-b1*a10/b10) *t1 + a - b*a10/b10
146  // t0 = k * t1 + p
147 
148  const float k = -(a1 - b1 * a01 / b01) / (a0 - b0 * a01 / b01);
149  const float p = -(a - b * a01 / b01) / (a0 - b0 * a01 / b01);
150 
151  // Second deg eqn.
152  //
153  // b10*k*t1*t1 + b10*p*t1 | + b0*k *t1 + b0*p | + b1*t1 | + b = 0
154  //
155  // (b10*k) * t1*t1 + (b10*p + b0*k + b1) * t1 + (b0*p + b)
156 
157  const float _a = (b01 * k);
158  const float _b = (b01 * p + b0 * k + b1);
159  const float _c = (b0 * p + b);
160 
161  const float eps = Eigen::NumTraits<float>::dummy_precision();
162 
163  // Check for imaginary solutions, or small denominators.
164  if ((_b * _b - 4 * _a * _c) < 0 || std::abs(a0 - b0 * a01) < eps ||
165  std::abs(b01) < eps || std::abs(_a) < eps) {
166  PCL_DEBUG("[pcl::SampleConsensusModelTorus::computeModelCoefficients] Can't "
167  "compute model coefficients with this method!\n");
168  return (false);
169  }
170 
171  const float s0 = (-_b + std::sqrt(_b * _b - 4 * _a * _c)) / (2 * _a);
172  const float s1 = (-_b - std::sqrt(_b * _b - 4 * _a * _c)) / (2 * _a);
173 
174  float r_maj_stddev_cycle1 = std::numeric_limits<float>::max();
175 
176  for (float s : {s0, s1}) {
177 
178  const float t1 = s;
179  const float t0 = k * t1 + p;
180 
181  // Direction vector
182  Eigen::Vector3f d = ((p1 + n1 * t1) - (p0 + n0 * t0));
183  d.normalize();
184  // Flip direction, so that the first element of the direction vector is
185  // positive, for consistency.
186  if (d[0] < 0) {
187  d *= -1;
188  }
189 
190  // Flip normals if required. Note |d| = 1
191  // d
192  // if (n0.dot(d) / n0.norm() < M_PI / 2 ) n0 = -n0;
193  // if (n1.dot(d) / n1.norm() < M_PI / 2 ) n1 = -n1;
194  // if (n2.dot(d) / n2.norm() < M_PI / 2 ) n2 = -n2;
195  // if (n3.dot(d) / n3.norm() < M_PI / 2 ) n3 = -n3;
196 
197  // We fit the points to the plane of the torus.
198  // Ax + By + Cz + D = 0
199  // We know that all for each point plus its normal
200  // times the minor radius will give us a point
201  // in that plane
202  // Pplane_i = P_i + n_i * r
203  // we substitute A,x,B,y,C,z
204  // dx *( P_i_x + n_i_x * r ) + dy *( P_i_y + n_i_y * r ) +dz *( P_i_z + n_i_z * r )
205  // + D = 0 and finally (dx*P_i_x + dy*P_i_y + dz*P_i_z) + (dx*n_i_x + dy*n_i_y +
206  // dz*n_i_z ) * r + D = 0 We can set up a linear least squares system of two
207  // variables r and D
208  //
209  Eigen::MatrixXf A(4, 2);
210  A << d.dot(n0), 1, d.dot(n1), 1, d.dot(n2), 1, d.dot(n3), 1;
211 
212  Eigen::Matrix<float, -1, -1> B(4, 1);
213  B << -d.dot(p0), -d.dot(p1), -d.dot(p2), -d.dot(p3);
214 
215  Eigen::Matrix<float, -1, -1> sol;
216  sol = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
217 
218  const float r_min = -sol(0);
219  const float D = sol(1);
220 
221  // Axis line and plane intersect to find the centroid of the torus
222  // We take a random point on the line. We find P_rand + lambda * d belongs in the
223  // plane
224 
225  const Eigen::Vector3f Pany = (p1 + n1 * t1);
226 
227  const float lambda = (-d.dot(Pany) - D) / d.dot(d);
228 
229  const Eigen::Vector3f centroid = Pany + d * lambda;
230 
231  // Finally, the major radius. The least square solution will be
232  // the average in this case.
233  const float r_maj = std::sqrt(((p0 - r_min * n0 - centroid).squaredNorm() +
234  (p1 - r_min * n1 - centroid).squaredNorm() +
235  (p2 - r_min * n2 - centroid).squaredNorm() +
236  (p3 - r_min * n3 - centroid).squaredNorm()) /
237  4.f);
238 
239  const float r_maj_stddev =
240  std::sqrt((std::pow(r_maj - (p0 - r_min * n0 - centroid).norm(), 2) +
241  std::pow(r_maj - (p1 - r_min * n1 - centroid).norm(), 2) +
242  std::pow(r_maj - (p2 - r_min * n2 - centroid).norm(), 2) +
243  std::pow(r_maj - (p3 - r_min * n3 - centroid).norm(), 2)) /
244  4.f);
245  // We select the minimum stddev cycle
246  if (r_maj_stddev < r_maj_stddev_cycle1) {
247  r_maj_stddev_cycle1 = r_maj_stddev;
248  }
249  else {
250  break;
251  }
252 
253  model_coefficients.resize(model_size_);
254  model_coefficients[0] = r_maj;
255  model_coefficients[1] = r_min;
256 
257  model_coefficients[2] = centroid[0];
258  model_coefficients[3] = centroid[1];
259  model_coefficients[4] = centroid[2];
260 
261  model_coefficients[5] = d[0];
262  model_coefficients[6] = d[1];
263  model_coefficients[7] = d[2];
264  }
265  return true;
266 }
267 
268 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
269 template <typename PointT, typename PointNT>
270 void
272  const Eigen::VectorXf& model_coefficients, std::vector<double>& distances) const
273 {
274 
275  if (!isModelValid(model_coefficients)) {
276  distances.clear();
277  return;
278  }
279 
280  distances.resize(indices_->size());
281 
282  // Iterate through the 3d points and calculate the distances to the closest torus
283  // point
284  for (std::size_t i = 0; i < indices_->size(); ++i) {
285  const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
286  const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
287 
288  Eigen::Vector3f torus_closest;
289  projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
290 
291  distances[i] = (torus_closest - pt).norm();
292  }
293 }
294 
295 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
296 template <typename PointT, typename PointNT>
297 void
299  const Eigen::VectorXf& model_coefficients, const double threshold, Indices& inliers)
300 {
301  // Check if the model is valid given the user constraints
302  if (!isModelValid(model_coefficients)) {
303  inliers.clear();
304  return;
305  }
306  const float squared_threshold = threshold * threshold;
307  inliers.clear();
308  error_sqr_dists_.clear();
309  inliers.reserve(indices_->size());
310  error_sqr_dists_.reserve(indices_->size());
311 
312  for (std::size_t i = 0; i < indices_->size(); ++i) {
313  const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
314  const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
315 
316  Eigen::Vector3f torus_closest;
317  projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
318 
319  const float distance = (torus_closest - pt).squaredNorm();
320 
321  if (distance < squared_threshold) {
322  // Returns the indices of the points whose distances are smaller than the
323  // threshold
324  inliers.push_back((*indices_)[i]);
325  error_sqr_dists_.push_back(std::sqrt(distance));
326  }
327  }
328 }
329 
330 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
331 template <typename PointT, typename PointNT>
332 std::size_t
334  const Eigen::VectorXf& model_coefficients, const double threshold) const
335 {
336  if (!isModelValid(model_coefficients))
337  return (0);
338 
339  const float squared_threshold = threshold * threshold;
340  std::size_t nr_p = 0;
341 
342  for (std::size_t i = 0; i < indices_->size(); ++i) {
343  const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
344  const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
345 
346  Eigen::Vector3f torus_closest;
347  projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
348 
349  const float distance = (torus_closest - pt).squaredNorm();
350 
351  if (distance < squared_threshold) {
352  nr_p++;
353  }
354  }
355  return (nr_p);
356 }
357 
358 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
359 template <typename PointT, typename PointNT>
360 void
362  const Indices& inliers,
363  const Eigen::VectorXf& model_coefficients,
364  Eigen::VectorXf& optimized_coefficients) const
365 {
366 
367  optimized_coefficients = model_coefficients;
368 
369  // Needs a set of valid model coefficients
370  if (!isModelValid(model_coefficients)) {
371  PCL_ERROR("[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Given model "
372  "is invalid!\n");
373  return;
374  }
375 
376  // Need more than the minimum sample size to make a difference
377  if (inliers.size() <= sample_size_) {
378  PCL_ERROR("[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Not enough "
379  "inliers to refine/optimize the model's coefficients (%lu)! Returning "
380  "the same coefficients.\n",
381  inliers.size());
382  return;
383  }
384 
385  OptimizationFunctor functor(this, inliers);
386  Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
387  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm(
388  num_diff);
389 
390  Eigen::VectorXd coeff = model_coefficients.cast<double>();
391  int info = lm.minimize(coeff);
392 
393  if (!info) {
394  PCL_ERROR(
395  "[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Optimizer returned"
396  "with error (%i)! Returning ",
397  info);
398  return;
399  }
400 
401  // Normalize direction vector
402  coeff.tail<3>().normalize();
403  optimized_coefficients = coeff.cast<float>();
404 }
405 
406 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
407 template <typename PointT, typename PointNT>
408 void
410  const Eigen::Vector3f& p_in,
411  const Eigen::Vector3f& p_n,
412  const Eigen::VectorXf& model_coefficients,
413  Eigen::Vector3f& pt_out) const
414 {
415 
416  // Fetch optimization parameters
417  const float& R = model_coefficients[0];
418  const float& r = model_coefficients[1];
419 
420  const float& x0 = model_coefficients[2];
421  const float& y0 = model_coefficients[3];
422  const float& z0 = model_coefficients[4];
423 
424  const float& nx = model_coefficients[5];
425  const float& ny = model_coefficients[6];
426  const float& nz = model_coefficients[7];
427 
428  // Normal of the plane where the torus circle lies
429  Eigen::Vector3f n{nx, ny, nz};
430  n.normalize();
431 
432  Eigen::Vector3f pt0{x0, y0, z0};
433 
434  // Ax + By + Cz + D = 0
435  const float D = -n.dot(pt0);
436 
437  // Project to the torus circle plane folling the point normal
438  // we want to find lambda such that p + pn_n*lambda lies on the
439  // torus plane.
440  // A*(pt_x + lambda*pn_x) + B *(pt_y + lambda*pn_y) + ... + D = 0
441  // given that: n = [A,B,C]
442  // n.dot(P) + lambda*n.dot(pn) + D = 0
443  //
444 
445  Eigen::Vector3f pt_proj;
446  // If the point lies in the torus plane, we just use it as projected
447 
448  // C++20 -> [[likely]]
449  if (std::abs(n.dot(p_n)) > Eigen::NumTraits<float>::dummy_precision()) {
450  float lambda = (-D - n.dot(p_in)) / n.dot(p_n);
451  pt_proj = p_in + lambda * p_n;
452  }
453  else {
454  pt_proj = p_in;
455  }
456 
457  // Closest point from the inner circle to the current point
458  const Eigen::Vector3f circle_closest = (pt_proj - pt0).normalized() * R + pt0;
459 
460  // From the that closest point we move towards the goal point until we
461  // meet the surface of the torus
462  pt_out = (p_in - circle_closest).normalized() * r + circle_closest;
463 }
464 
465 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
466 template <typename PointT, typename PointNT>
467 void
469  const Indices& inliers,
470  const Eigen::VectorXf& model_coefficients,
471  PointCloud& projected_points,
472  bool copy_data_fields) const
473 {
474  // Needs a valid set of model coefficients
475  if (!isModelValid(model_coefficients)) {
476  PCL_ERROR(
477  "[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n");
478  return;
479  }
480 
481  // Copy all the data fields from the input cloud to the projected one?
482  if (copy_data_fields) {
483  // Allocate enough space and copy the basics
484  projected_points.resize(input_->size());
485  projected_points.width = input_->width;
486  projected_points.height = input_->height;
487 
488  using FieldList = typename pcl::traits::fieldList<PointT>::type;
489  // Iterate over each point
490  for (std::size_t i = 0; i < input_->size(); ++i)
491  // Iterate over each dimension
492  pcl::for_each_type<FieldList>(
493  NdConcatenateFunctor<PointT, PointT>((*input_)[i], projected_points[i]));
494 
495  // Iterate through the 3d points and calculate the distances from them to the plane
496  for (const auto& inlier : inliers) {
497  Eigen::Vector3f q;
498  const Eigen::Vector3f pt_n = (*normals_)[inlier].getNormalVector3fMap();
499  projectPointToTorus(
500  (*input_)[inlier].getVector3fMap(), pt_n, model_coefficients, q);
501  projected_points[inlier].getVector3fMap() = q;
502  }
503  }
504  else {
505  // Allocate enough space and copy the basics
506  projected_points.resize(inliers.size());
507  projected_points.width = inliers.size();
508  projected_points.height = 1;
509 
510  using FieldList = typename pcl::traits::fieldList<PointT>::type;
511  // Iterate over each point
512  for (std::size_t i = 0; i < inliers.size(); ++i) {
513  // Iterate over each dimension
514  pcl::for_each_type<FieldList>(NdConcatenateFunctor<PointT, PointT>(
515  (*input_)[inliers[i]], projected_points[i]));
516  }
517 
518  for (const auto& inlier : inliers) {
519  Eigen::Vector3f q;
520  const Eigen::Vector3f pt_n = (*normals_)[inlier].getNormalVector3fMap();
521  projectPointToTorus(
522  (*input_)[inlier].getVector3fMap(), pt_n, model_coefficients, q);
523  projected_points[inlier].getVector3fMap() = q;
524  }
525  }
526 }
527 
528 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
529 template <typename PointT, typename PointNT>
530 bool
532  const std::set<index_t>& indices,
533  const Eigen::VectorXf& model_coefficients,
534  const double threshold) const
535 {
536 
537  for (const auto& index : indices) {
538  const Eigen::Vector3f pt_n = (*normals_)[index].getNormalVector3fMap();
539  Eigen::Vector3f torus_closest;
540  projectPointToTorus((*input_)[index].getVector3fMap(), pt_n, model_coefficients, torus_closest);
541 
542  if (((*input_)[index].getVector3fMap() - torus_closest).squaredNorm() > threshold * threshold)
543  return (false);
544  }
545  return true;
546 }
547 
548 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
549 template <typename PointT, typename PointNT>
550 bool
552  const Eigen::VectorXf& model_coefficients) const
553 {
554  if (!SampleConsensusModel<PointT>::isModelValid(model_coefficients))
555  return (false);
556 
557  if (radius_min_ != std::numeric_limits<double>::lowest() &&
558  (model_coefficients[0] < radius_min_ || model_coefficients[1] < radius_min_)) {
559  PCL_DEBUG(
560  "[pcl::SampleConsensusModelTorus::isModelValid] Major radius OR minor radius "
561  "of torus is/are too small: should be larger than %g, but are {%g, %g}.\n",
562  radius_min_,
563  model_coefficients[0],
564  model_coefficients[1]);
565  return (false);
566  }
567  if (radius_max_ != std::numeric_limits<double>::max() &&
568  (model_coefficients[0] > radius_max_ || model_coefficients[1] > radius_max_)) {
569  PCL_DEBUG(
570  "[pcl::SampleConsensusModelTorus::isModelValid] Major radius OR minor radius "
571  "of torus is/are too big: should be smaller than %g, but are {%g, %g}.\n",
572  radius_max_,
573  model_coefficients[0],
574  model_coefficients[1]);
575  return (false);
576  }
577  return (true);
578 }
579 
580 #define PCL_INSTANTIATE_SampleConsensusModelTorus(PointT, PointNT) \
581  template class PCL_EXPORTS pcl::SampleConsensusModelTorus<PointT, PointNT>;
582 
583 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:174
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:463
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:399
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:401
SampleConsensusModel represents the base model class.
Definition: sac_model.h:71
void projectPointToTorus(const Eigen::Vector3f &pt, const Eigen::Vector3f &pt_n, const Eigen::VectorXf &model_coefficients, Eigen::Vector3f &pt_proj) const
Project a point onto a torus given by its model coefficients (radii, torus_center_point,...
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.
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 torus model coefficients.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid torus model, compute the model coefficients fr...
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the torus coefficients using the given inlier set and return them to the user.
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 isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given torus model.
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 torus model.
@ B
Definition: norms.h:54
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Helper functor structure for concatenate.
Definition: concatenate.h:50