Point Cloud Library (PCL)  1.14.1-dev
sac_model_ellipse3d.hpp
1 /*
2  * SPDX-License-Identifier: BSD-3-Clause
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception Inc.
6  *
7  * All rights reserved
8  */
9
10 #pragma once
11
12 #include <limits>
13
14 #include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
15 #include <pcl/sample_consensus/sac_model_ellipse3d.h>
16 #include <pcl/common/concatenate.h>
17
18 #include <Eigen/Eigenvalues>
19 #include <complex>
20
21
22 //////////////////////////////////////////////////////////////////////////
23 template <typename PointT> bool
25  const Indices &samples) const
26 {
27  if (samples.size () != sample_size_)
28  {
29  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
30  return (false);
31  }
32
33  // Use three points out of the 6 samples
34  const Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
35  const Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
36  const Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
37
38  // Check if the squared norm of the cross-product is non-zero, otherwise
39  // common_helper_vec, which plays an important role in computeModelCoefficients,
40  // would likely be ill-formed.
41  if ((p1 - p0).cross(p1 - p2).squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
42  {
43  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::isSampleGood] Sample points too similar or collinear!\n");
44  return (false);
45  }
46
47  return (true);
48 }
49
50 //////////////////////////////////////////////////////////////////////////
51 template <typename PointT> bool
52 pcl::SampleConsensusModelEllipse3D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
53 {
54  // Uses 6 samples
55  if (samples.size () != sample_size_)
56  {
57  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
58  return (false);
59  }
60
61  model_coefficients.resize (model_size_); // 11 coefs
62
63  const Eigen::Vector3f p0((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
64  const Eigen::Vector3f p1((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
65  const Eigen::Vector3f p2((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
66  const Eigen::Vector3f p3((*input_)[samples[3]].x, (*input_)[samples[3]].y, (*input_)[samples[3]].z);
67  const Eigen::Vector3f p4((*input_)[samples[4]].x, (*input_)[samples[4]].y, (*input_)[samples[4]].z);
68  const Eigen::Vector3f p5((*input_)[samples[5]].x, (*input_)[samples[5]].y, (*input_)[samples[5]].z);
69
70  const Eigen::Vector3f common_helper_vec = (p1 - p0).cross(p1 - p2);
71  const Eigen::Vector3f ellipse_normal = common_helper_vec.normalized();
72
73  // The same check is implemented in isSampleGood, so be sure to look there too
74  // if you find the need to change something here.
75  if (common_helper_vec.squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
76  {
77  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Sample points too similar or collinear!\n");
78  return (false);
79  }
80
81  // Definition of the local reference frame of the ellipse
82  Eigen::Vector3f x_axis = (p1 - p0).normalized();
83  const Eigen::Vector3f z_axis = ellipse_normal.normalized();
84  const Eigen::Vector3f y_axis = z_axis.cross(x_axis).normalized();
85
86  // Compute the rotation matrix and its transpose
87  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
88  << x_axis(0), y_axis(0), z_axis(0),
89  x_axis(1), y_axis(1), z_axis(1),
90  x_axis(2), y_axis(2), z_axis(2))
91  .finished();
92  const Eigen::Matrix3f Rot_T = Rot.transpose();
93
94  // Convert the points to local coordinates
95  const Eigen::Vector3f p0_ = Rot_T * (p0 - p0);
96  const Eigen::Vector3f p1_ = Rot_T * (p1 - p0);
97  const Eigen::Vector3f p2_ = Rot_T * (p2 - p0);
98  const Eigen::Vector3f p3_ = Rot_T * (p3 - p0);
99  const Eigen::Vector3f p4_ = Rot_T * (p4 - p0);
100  const Eigen::Vector3f p5_ = Rot_T * (p5 - p0);
101
102
103  // Fit an ellipse to the samples to obtain its conic equation parameters
104  // (this implementation follows the manuscript "Direct Least Square Fitting of Ellipses"
105  // A. Fitzgibbon, M. Pilu and R. Fisher, IEEE TPAMI, 21(5) : 476–480, May 1999).
106
107  // xOy projections only
108  const Eigen::VectorXf X = (Eigen::VectorXf(6) << p0_(0), p1_(0), p2_(0), p3_(0), p4_(0), p5_(0)).finished();
109  const Eigen::VectorXf Y = (Eigen::VectorXf(6) << p0_(1), p1_(1), p2_(1), p3_(1), p4_(1), p5_(1)).finished();
110
111  // Design matrix D
112  const Eigen::MatrixXf D = (Eigen::MatrixXf(6,6)
113  << X(0) * X(0), X(0) * Y(0), Y(0) * Y(0), X(0), Y(0), 1.0,
114  X(1) * X(1), X(1) * Y(1), Y(1) * Y(1), X(1), Y(1), 1.0,
115  X(2) * X(2), X(2) * Y(2), Y(2) * Y(2), X(2), Y(2), 1.0,
116  X(3) * X(3), X(3) * Y(3), Y(3) * Y(3), X(3), Y(3), 1.0,
117  X(4) * X(4), X(4) * Y(4), Y(4) * Y(4), X(4), Y(4), 1.0,
118  X(5) * X(5), X(5) * Y(5), Y(5) * Y(5), X(5), Y(5), 1.0)
119  .finished();
120
121  // Scatter matrix S
122  const Eigen::MatrixXf S = D.transpose() * D;
123
124  // Constraint matrix C
125  const Eigen::MatrixXf C = (Eigen::MatrixXf(6,6)
126  << 0.0, 0.0, -2.0, 0.0, 0.0, 0.0,
127  0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
128  -2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
129  0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
130  0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
131  0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
132  .finished();
133
134  // Solve the Generalized Eigensystem: S*a = lambda*C*a
135  Eigen::GeneralizedEigenSolver<Eigen::MatrixXf> solver;
136  solver.compute(S, C);
137  const Eigen::VectorXf eigvals = solver.eigenvalues().real();
138
139  // Find the negative eigenvalue 'neigvec' (the largest, if many exist)
140  int idx(-1);
141  float absmin(0.0);
142  for (size_t i(0); i < static_cast<size_t>(eigvals.size()); ++i) {
143  if (eigvals(i) < absmin && !std::isinf(eigvals(i))) {
144  idx = i;
145  }
146  }
147  // Return "false" in case the negative eigenvalue was not found
148  if (idx == -1) {
149  PCL_DEBUG("[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Failed to find the negative eigenvalue in the GES.\n");
150  return (false);
151  }
152  const Eigen::VectorXf neigvec = solver.eigenvectors().real().col(idx).normalized();
153
154
155  // Convert the conic model parameters to parametric ones
156
157  // Conic parameters
158  const float con_A(neigvec(0));
159  const float con_B(neigvec(1));
160  const float con_C(neigvec(2));
161  const float con_D(neigvec(3));
162  const float con_E(neigvec(4));
163  const float con_F(neigvec(5));
164
165  // Build matrix M0
166  const Eigen::MatrixXf M0 = (Eigen::MatrixXf(3, 3)
167  << con_F, con_D/2.0, con_E/2.0,
168  con_D/2.0, con_A, con_B/2.0,
169  con_E/2.0, con_B/2.0, con_C)
170  .finished();
171
172  // Build matrix M
173  const Eigen::MatrixXf M = (Eigen::MatrixXf(2, 2)
174  << con_A, con_B/2.0,
175  con_B/2.0, con_C)
176  .finished();
177
178  // Calculate the eigenvalues and eigenvectors of matrix M
179  Eigen::EigenSolver<Eigen::MatrixXf> solver_M(M);
180
181  Eigen::VectorXf eigvals_M = solver_M.eigenvalues().real();
182
183  // Order the eigenvalues so that |lambda_0 - con_A| <= |lambda_0 - con_C|
184  float aux_eigval(0.0);
185  if (std::abs(eigvals_M(0) - con_A) > std::abs(eigvals_M(0) - con_C)) {
186  aux_eigval = eigvals_M(0);
187  eigvals_M(0) = eigvals_M(1);
188  eigvals_M(1) = aux_eigval;
189  }
190
191  // Parametric parameters of the ellipse
192  float par_a = std::sqrt(-M0.determinant() / (M.determinant() * eigvals_M(0)));
193  float par_b = std::sqrt(-M0.determinant() / (M.determinant() * eigvals_M(1)));
194  const float par_h = (con_B * con_E - 2.0 * con_C * con_D) / (4.0 * con_A * con_C - std::pow(con_B, 2));
195  const float par_k = (con_B * con_D - 2.0 * con_A * con_E) / (4.0 * con_A * con_C - std::pow(con_B, 2));
196  const float par_t = (M_PI / 2.0 - std::atan((con_A - con_C) / con_B)) / 2.0; // equivalent to: acot((con_A - con_C) / con_B) / 2.0;
197
198  // Convert the center point of the ellipse to global coordinates
199  // (the if statement ensures that 'par_a' always refers to the semi-major axis length)
200  Eigen::Vector3f p_ctr;
201  float aux_par(0.0);
202  if (par_a > par_b) {
203  p_ctr = p0 + Rot * Eigen::Vector3f(par_h, par_k, 0.0);
204  } else {
205  aux_par = par_a;
206  par_a = par_b;
207  par_b = aux_par;
208  p_ctr = p0 + Rot * Eigen::Vector3f(par_k, par_h, 0.0);
209  }
210
211  // Center (x, y, z)
212  model_coefficients[0] = static_cast<float>(p_ctr(0));
213  model_coefficients[1] = static_cast<float>(p_ctr(1));
214  model_coefficients[2] = static_cast<float>(p_ctr(2));
215
216  // Semi-major axis length 'a' (along the local x-axis)
217  model_coefficients[3] = static_cast<float>(par_a);
218  // Semi-minor axis length 'b' (along the local y-axis)
219  model_coefficients[4] = static_cast<float>(par_b);
220
221  // Ellipse normal
222  model_coefficients[5] = static_cast<float>(ellipse_normal[0]);
223  model_coefficients[6] = static_cast<float>(ellipse_normal[1]);
224  model_coefficients[7] = static_cast<float>(ellipse_normal[2]);
225
226  // Retrieve the ellipse point at the tilt angle t (par_t), along the local x-axis
227  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, par_h, par_k, par_t).finished();
228  Eigen::Vector3f p_th_(0.0, 0.0, 0.0);
229  get_ellipse_point(params, par_t, p_th_(0), p_th_(1));
230
231  // Redefinition of the x-axis of the ellipse's local reference frame
232  x_axis = (Rot * p_th_).normalized();
233  model_coefficients[8] = static_cast<float>(x_axis[0]);
234  model_coefficients[9] = static_cast<float>(x_axis[1]);
235  model_coefficients[10] = static_cast<float>(x_axis[2]);
236
237
238  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g).\n",
239  model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
240  model_coefficients[4], model_coefficients[5], model_coefficients[6], model_coefficients[7],
241  model_coefficients[8], model_coefficients[9], model_coefficients[10]);
242  return (true);
243 }
244
245
246 //////////////////////////////////////////////////////////////////////////
247 template <typename PointT> void
248 pcl::SampleConsensusModelEllipse3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
249 {
250  // Check if the model is valid given the user constraints
251  if (!isModelValid (model_coefficients))
252  {
253  distances.clear ();
254  return;
255  }
256  distances.resize (indices_->size ());
257
258  // c : Ellipse Center
259  const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
260  // n : Ellipse (Plane) Normal
261  const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
262  // x : Ellipse (Plane) X-Axis
263  const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
264  // y : Ellipse (Plane) Y-Axis
265  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
266  // a : Ellipse semi-major axis (X) length
267  const float par_a(model_coefficients[3]);
268  // b : Ellipse semi-minor axis (Y) length
269  const float par_b(model_coefficients[4]);
270
271  // Compute the rotation matrix and its transpose
272  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
273  << x_axis(0), y_axis(0), n_axis(0),
274  x_axis(1), y_axis(1), n_axis(1),
275  x_axis(2), y_axis(2), n_axis(2))
276  .finished();
277  const Eigen::Matrix3f Rot_T = Rot.transpose();
278
279  // Ellipse parameters
280  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
281  float th_opt;
282
283  // Iterate through the 3D points and calculate the distances from them to the ellipse
284  for (std::size_t i = 0; i < indices_->size (); ++i)
285  // Calculate the distance from the point to the ellipse:
286  // 1. calculate intersection point of the plane in which the ellipse lies and the
287  // line from the sample point with the direction of the plane normal (projected point)
288  // 2. calculate the intersection point of the line from the ellipse center to the projected point
289  // with the ellipse
290  // 3. calculate distance from corresponding point on the ellipse to the sample point
291  {
292  // p : Sample Point
293  const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
294
295  // Local coordinates of sample point p
296  const Eigen::Vector3f p_ = Rot_T * (p - c);
297
298  // k : Point on Ellipse
299  // Calculate the shortest distance from the point to the ellipse which is given by
300  // the norm of a vector that is normal to the ellipse tangent calculated at the
301  // point it intersects the tangent.
302  const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
303
304  distances[i] = distanceVector.norm();
305  }
306 }
307
308 //////////////////////////////////////////////////////////////////////////
309 template <typename PointT> void
311  const Eigen::VectorXf &model_coefficients, const double threshold,
312  Indices &inliers)
313 {
314  inliers.clear();
315  // Check if the model is valid given the user constraints
316  if (!isModelValid (model_coefficients))
317  {
318  return;
319  }
320  inliers.reserve (indices_->size ());
321
322  // c : Ellipse Center
323  const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
324  // n : Ellipse (Plane) Normal
325  const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
326  // x : Ellipse (Plane) X-Axis
327  const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
328  // y : Ellipse (Plane) Y-Axis
329  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
330  // a : Ellipse semi-major axis (X) length
331  const float par_a(model_coefficients[3]);
332  // b : Ellipse semi-minor axis (Y) length
333  const float par_b(model_coefficients[4]);
334
335  // Compute the rotation matrix and its transpose
336  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
337  << x_axis(0), y_axis(0), n_axis(0),
338  x_axis(1), y_axis(1), n_axis(1),
339  x_axis(2), y_axis(2), n_axis(2))
340  .finished();
341  const Eigen::Matrix3f Rot_T = Rot.transpose();
342
343  const auto squared_threshold = threshold * threshold;
344  // Iterate through the 3d points and calculate the distances from them to the ellipse
345  for (std::size_t i = 0; i < indices_->size (); ++i)
346  {
347  // p : Sample Point
348  const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
349
350  // Local coordinates of sample point p
351  const Eigen::Vector3f p_ = Rot_T * (p - c);
352
353  // k : Point on Ellipse
354  // Calculate the shortest distance from the point to the ellipse which is given by
355  // the norm of a vector that is normal to the ellipse tangent calculated at the
356  // point it intersects the tangent.
357  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
358  float th_opt;
359  const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
360
361  if (distanceVector.squaredNorm() < squared_threshold)
362  {
363  // Returns the indices of the points whose distances are smaller than the threshold
364  inliers.push_back ((*indices_)[i]);
365  }
366  }
367 }
368
369 //////////////////////////////////////////////////////////////////////////
370 template <typename PointT> std::size_t
372  const Eigen::VectorXf &model_coefficients, const double threshold) const
373 {
374  // Check if the model is valid given the user constraints
375  if (!isModelValid (model_coefficients))
376  return (0);
377  std::size_t nr_p = 0;
378
379  // c : Ellipse Center
380  const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
381  // n : Ellipse (Plane) Normal
382  const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
383  // x : Ellipse (Plane) X-Axis
384  const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
385  // y : Ellipse (Plane) Y-Axis
386  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
387  // a : Ellipse semi-major axis (X) length
388  const float par_a(model_coefficients[3]);
389  // b : Ellipse semi-minor axis (Y) length
390  const float par_b(model_coefficients[4]);
391
392  // Compute the rotation matrix and its transpose
393  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
394  << x_axis(0), y_axis(0), n_axis(0),
395  x_axis(1), y_axis(1), n_axis(1),
396  x_axis(2), y_axis(2), n_axis(2))
397  .finished();
398  const Eigen::Matrix3f Rot_T = Rot.transpose();
399
400  const auto squared_threshold = threshold * threshold;
401  // Iterate through the 3d points and calculate the distances from them to the ellipse
402  for (std::size_t i = 0; i < indices_->size (); ++i)
403  {
404  // p : Sample Point
405  const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
406
407  // Local coordinates of sample point p
408  const Eigen::Vector3f p_ = Rot_T * (p - c);
409
410  // k : Point on Ellipse
411  // Calculate the shortest distance from the point to the ellipse which is given by
412  // the norm of a vector that is normal to the ellipse tangent calculated at the
413  // point it intersects the tangent.
414  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
415  float th_opt;
416  const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
417
418  if (distanceVector.squaredNorm() < squared_threshold)
419  nr_p++;
420  }
421  return (nr_p);
422 }
423
424 //////////////////////////////////////////////////////////////////////////
425 template <typename PointT> void
427  const Indices &inliers,
428  const Eigen::VectorXf &model_coefficients,
429  Eigen::VectorXf &optimized_coefficients) const
430 {
431  optimized_coefficients = model_coefficients;
432
433  // Needs a set of valid model coefficients
434  if (!isModelValid (model_coefficients))
435  {
436  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Given model is invalid!\n");
437  return;
438  }
439
440  // Need more than the minimum sample size to make a difference
441  if (inliers.size () <= sample_size_)
442  {
443  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
444  return;
445  }
446
447  OptimizationFunctor functor(this, inliers);
448  Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
449  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm(num_diff);
450  Eigen::VectorXd coeff;
451  int info = lm.minimize(coeff);
452  for (Eigen::Index i = 0; i < coeff.size (); ++i)
453  optimized_coefficients[i] = static_cast<float> (coeff[i]);
454
455  // Compute the L2 norm of the residuals
456  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g %g %g %g %g %g %g\n",
457  info, lm.fvec.norm (),
458
459  model_coefficients[0],
460  model_coefficients[1],
461  model_coefficients[2],
462  model_coefficients[3],
463  model_coefficients[4],
464  model_coefficients[5],
465  model_coefficients[6],
466  model_coefficients[7],
467  model_coefficients[8],
468  model_coefficients[9],
469  model_coefficients[10],
470
471  optimized_coefficients[0],
472  optimized_coefficients[1],
473  optimized_coefficients[2],
474  optimized_coefficients[3],
475  optimized_coefficients[4],
476  optimized_coefficients[5],
477  optimized_coefficients[6],
478  optimized_coefficients[7],
479  optimized_coefficients[8],
480  optimized_coefficients[9],
481  optimized_coefficients[10]);
482 }
483
484 //////////////////////////////////////////////////////////////////////////
485 template <typename PointT> void
487  const Indices &inliers, const Eigen::VectorXf &model_coefficients,
488  PointCloud &projected_points, bool copy_data_fields) const
489 {
490  // Needs a valid set of model coefficients
491  if (!isModelValid (model_coefficients))
492  {
493  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::projectPoints] Given model is invalid!\n");
494  return;
495  }
496
497  projected_points.header = input_->header;
498  projected_points.is_dense = input_->is_dense;
499
500  // Copy all the data fields from the input cloud to the projected one?
501  if (copy_data_fields)
502  {
503  // Allocate enough space and copy the basics
504  projected_points.resize (input_->size ());
505  projected_points.width = input_->width;
506  projected_points.height = input_->height;
507
508  using FieldList = typename pcl::traits::fieldList<PointT>::type;
509  // Iterate over each point
510  for (std::size_t i = 0; i < projected_points.size(); ++i)
511  {
512  // Iterate over each dimension
513  pcl::for_each_type<FieldList>(NdConcatenateFunctor<PointT, PointT>((*input_)[i], projected_points[i]));
514  }
515
516  // c : Ellipse Center
517  const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
518  // n : Ellipse (Plane) Normal
519  const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
520  // x : Ellipse (Plane) X-Axis
521  const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
522  // y : Ellipse (Plane) Y-Axis
523  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
524  // a : Ellipse semi-major axis (X) length
525  const float par_a(model_coefficients[3]);
526  // b : Ellipse semi-minor axis (Y) length
527  const float par_b(model_coefficients[4]);
528
529  // Compute the rotation matrix and its transpose
530  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
531  << x_axis(0), y_axis(0), n_axis(0),
532  x_axis(1), y_axis(1), n_axis(1),
533  x_axis(2), y_axis(2), n_axis(2))
534  .finished();
535  const Eigen::Matrix3f Rot_T = Rot.transpose();
536
537  // Iterate through the 3d points and calculate the distances from them to the plane
538  for (std::size_t i = 0; i < inliers.size (); ++i)
539  {
540  // p : Sample Point
541  const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
542
543  // Local coordinates of sample point p
544  const Eigen::Vector3f p_ = Rot_T * (p - c);
545
546  // k : Point on Ellipse
547  // Calculate the shortest distance from the point to the ellipse which is given by
548  // the norm of a vector that is normal to the ellipse tangent calculated at the
549  // point it intersects the tangent.
550  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
551  float th_opt;
552  dvec2ellipse(params, p_(0), p_(1), th_opt);
553
554  // Retrieve the ellipse point at the tilt angle t, along the local x-axis
555  Eigen::Vector3f k_(0.0, 0.0, 0.0);
556  get_ellipse_point(params, th_opt, k_[0], k_[1]);
557
558  const Eigen::Vector3f k = c + Rot * k_;
559
560  projected_points[i].x = static_cast<float> (k[0]);
561  projected_points[i].y = static_cast<float> (k[1]);
562  projected_points[i].z = static_cast<float> (k[2]);
563  }
564  }
565  else
566  {
567  // Allocate enough space and copy the basics
568  projected_points.resize (inliers.size ());
569  projected_points.width = inliers.size ();
570  projected_points.height = 1;
571
572  using FieldList = typename pcl::traits::fieldList<PointT>::type;
573  // Iterate over each point
574  for (std::size_t i = 0; i < inliers.size (); ++i)
575  // Iterate over each dimension
576  pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
577
578  // c : Ellipse Center
579  const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
580  // n : Ellipse (Plane) Normal
581  const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
582  // x : Ellipse (Plane) X-Axis
583  const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
584  // y : Ellipse (Plane) Y-Axis
585  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
586  // a : Ellipse semi-major axis (X) length
587  const float par_a(model_coefficients[3]);
588  // b : Ellipse semi-minor axis (Y) length
589  const float par_b(model_coefficients[4]);
590
591  // Compute the rotation matrix and its transpose
592  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
593  << x_axis(0), y_axis(0), n_axis(0),
594  x_axis(1), y_axis(1), n_axis(1),
595  x_axis(2), y_axis(2), n_axis(2))
596  .finished();
597  const Eigen::Matrix3f Rot_T = Rot.transpose();
598
599  // Iterate through the 3d points and calculate the distances from them to the plane
600  for (std::size_t i = 0; i < inliers.size (); ++i)
601  {
602  // p : Sample Point
603  const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
604
605  // Local coordinates of sample point p
606  const Eigen::Vector3f p_ = Rot_T * (p - c);
607
608  // k : Point on Ellipse
609  // Calculate the shortest distance from the point to the ellipse which is given by
610  // the norm of a vector that is normal to the ellipse tangent calculated at the
611  // point it intersects the tangent.
612  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
613  float th_opt;
614  dvec2ellipse(params, p_(0), p_(1), th_opt);
615
616  // Retrieve the ellipse point at the tilt angle t, along the local x-axis
617  //// model_coefficients[5] = static_cast<float>(par_t);
618  Eigen::Vector3f k_(0.0, 0.0, 0.0);
619  get_ellipse_point(params, th_opt, k_[0], k_[1]);
620
621  const Eigen::Vector3f k = c + Rot * k_;
622
623  projected_points[i].x = static_cast<float> (k[0]);
624  projected_points[i].y = static_cast<float> (k[1]);
625  projected_points[i].z = static_cast<float> (k[2]);
626  }
627  }
628 }
629
630 //////////////////////////////////////////////////////////////////////////
631 template <typename PointT> bool
633  const std::set<index_t> &indices,
634  const Eigen::VectorXf &model_coefficients,
635  const double threshold) const
636 {
637  // Needs a valid model coefficients
638  if (!isModelValid (model_coefficients))
639  {
640  PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::doSamplesVerifyModel] Given model is invalid!\n");
641  return (false);
642  }
643
644  // c : Ellipse Center
645  const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
646  // n : Ellipse (Plane) Normal
647  const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
648  // x : Ellipse (Plane) X-Axis
649  const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
650  // y : Ellipse (Plane) Y-Axis
651  const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
652  // a : Ellipse semi-major axis (X) length
653  const float par_a(model_coefficients[3]);
654  // b : Ellipse semi-minor axis (Y) length
655  const float par_b(model_coefficients[4]);
656
657  // Compute the rotation matrix and its transpose
658  const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
659  << x_axis(0), y_axis(0), n_axis(0),
660  x_axis(1), y_axis(1), n_axis(1),
661  x_axis(2), y_axis(2), n_axis(2))
662  .finished();
663  const Eigen::Matrix3f Rot_T = Rot.transpose();
664
665  const auto squared_threshold = threshold * threshold;
666  for (const auto &index : indices)
667  {
668  // p : Sample Point
669  const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
670
671  // Local coordinates of sample point p
672  const Eigen::Vector3f p_ = Rot_T * (p - c);
673
674  // k : Point on Ellipse
675  // Calculate the shortest distance from the point to the ellipse which is given by
676  // the norm of a vector that is normal to the ellipse tangent calculated at the
677  // point it intersects the tangent.
678  const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
679  float th_opt;
680  const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
681
682  if (distanceVector.squaredNorm() > squared_threshold)
683  return (false);
684  }
685  return (true);
686 }
687
688 //////////////////////////////////////////////////////////////////////////
689 template <typename PointT> bool
690 pcl::SampleConsensusModelEllipse3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
691 {
692  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
693  return (false);
694
695  if (radius_min_ != std::numeric_limits<double>::lowest() && (model_coefficients[3] < radius_min_ || model_coefficients[4] < radius_min_))
696  {
697  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::isModelValid] Semi-minor axis OR semi-major axis (radii) of ellipse is/are too small: should be larger than %g, but are {%g, %g}.\n",
698  radius_min_, model_coefficients[3], model_coefficients[4]);
699  return (false);
700  }
701  if (radius_max_ != std::numeric_limits<double>::max() && (model_coefficients[3] > radius_max_ || model_coefficients[4] > radius_max_))
702  {
703  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::isModelValid] Semi-minor axis OR semi-major axis (radii) of ellipse is/are too big: should be smaller than %g, but are {%g, %g}.\n",
704  radius_max_, model_coefficients[3], model_coefficients[4]);
705  return (false);
706  }
707
708  return (true);
709 }
710
711
712
713 //////////////////////////////////////////////////////////////////////////
714 template <typename PointT>
716  const Eigen::VectorXf& par, float th, float& x, float& y)
717 {
718  /*
719  * Calculates a point on the ellipse model 'par' using the angle 'th'.
720  */
721
722  // Parametric eq.params
723  const float par_a(par[0]);
724  const float par_b(par[1]);
725  const float par_h(par[2]);
726  const float par_k(par[3]);
727  const float par_t(par[4]);
728
729  x = par_h + std::cos(par_t) * par_a * std::cos(th) -
730  std::sin(par_t) * par_b * std::sin(th);
731  y = par_k + std::sin(par_t) * par_a * std::cos(th) +
732  std::cos(par_t) * par_b * std::sin(th);
733
734  return;
735 }
736
737 //////////////////////////////////////////////////////////////////////////
738 template <typename PointT>
740  const Eigen::VectorXf& par, float u, float v, float& th_opt)
741 {
742  /*
743  * Minimum distance vector from point p=(u,v) to the ellipse model 'par'.
744  */
745
746  // Parametric eq.params
747  // (par_a, par_b, and par_t do not need to be declared)
748  const float par_h = par[2];
749  const float par_k = par[3];
750
751  const Eigen::Vector2f center(par_h, par_k);
752  Eigen::Vector2f p(u, v);
753  p -= center;
754
755  // Local x-axis of the ellipse
756  Eigen::Vector2f x_axis(0.0, 0.0);
757  get_ellipse_point(par, 0.0, x_axis(0), x_axis(1));
758  x_axis -= center;
759
760  // Local y-axis of the ellipse
761  Eigen::Vector2f y_axis(0.0, 0.0);
762  get_ellipse_point(par, M_PI / 2.0, y_axis(0), y_axis(1));
763  y_axis -= center;
764
765  // Convert the point p=(u,v) to local ellipse coordinates
766  const float x_proj = p.dot(x_axis) / x_axis.norm();
767  const float y_proj = p.dot(y_axis) / y_axis.norm();
768
769  // Find the ellipse quandrant to where the point p=(u,v) belongs,
770  // and limit the search interval to 'th_min' and 'th_max'.
771  float th_min(0.0), th_max(0.0);
772  const float th = std::atan2(y_proj, x_proj);
773
774  if (-M_PI <= th && th < -M_PI / 2.0) {
775  // Q3
776  th_min = -M_PI;
777  th_max = -M_PI / 2.0;
778  }
779  if (-M_PI / 2.0 <= th && th < 0.0) {
780  // Q4
781  th_min = -M_PI / 2.0;
782  th_max = 0.0;
783  }
784  if (0.0 <= th && th < M_PI / 2.0) {
785  // Q1
786  th_min = 0.0;
787  th_max = M_PI / 2.0;
788  }
789  if (M_PI / 2.0 <= th && th <= M_PI) {
790  // Q2
791  th_min = M_PI / 2.0;
792  th_max = M_PI;
793  }
794
795  // Use an unconstrained line search optimizer to find the optimal th_opt
796  th_opt = golden_section_search(par, u, v, th_min, th_max, 1.e-3);
797
798  // Distance vector from a point (u,v) to a given point in the ellipse model 'par' at an angle 'th_opt'.
799  float x(0.0), y(0.0);
800  get_ellipse_point(par, th_opt, x, y);
801  Eigen::Vector2f distanceVector(u - x, v - y);
802  return distanceVector;
803 }
804
805 //////////////////////////////////////////////////////////////////////////
806 template <typename PointT>
808  const Eigen::VectorXf& par,
809  float u,
810  float v,
811  float th_min,
812  float th_max,
813  float epsilon)
814 {
815  /*
816  * Golden section search
817  */
818
819  constexpr float phi(1.61803398874989484820f); // Golden ratio
820
821  // tl (theta lower bound), tu (theta upper bound)
822  float tl(th_min), tu(th_max);
823  float ta = tl + (tu - tl) * (1 - 1 / phi);
824  float tb = tl + (tu - tl) * 1 / phi;
825
826  while ((tu - tl) > epsilon) {
827
828  // theta a
829  float x_a(0.0), y_a(0.0);
830  get_ellipse_point(par, ta, x_a, y_a);
831  float squared_dist_ta = (u - x_a) * (u - x_a) + (v - y_a) * (v - y_a);
832
833  // theta b
834  float x_b(0.0), y_b(0.0);
835  get_ellipse_point(par, tb, x_b, y_b);
836  float squared_dist_tb = (u - x_b) * (u - x_b) + (v - y_b) * (v - y_b);
837
838  if (squared_dist_ta < squared_dist_tb) {
839  tu = tb;
840  tb = ta;
841  ta = tl + (tu - tl) * (1 - 1 / phi);
842  }
843  else if (squared_dist_ta > squared_dist_tb) {
844  tl = ta;
845  ta = tb;
846  tb = tl + (tu - tl) * 1 / phi;
847  }
848  else {
849  tl = ta;
850  tu = tb;
851  ta = tl + (tu - tl) * (1 - 1 / phi);
852  tb = tl + (tu - tl) * 1 / phi;
853  }
854  }
855  return (tl + tu) / 2.0;
856 }
857
858
859 #define PCL_INSTANTIATE_SampleConsensusModelEllipse3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelEllipse3D<T>;
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d ellipse 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.
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 3d ellipse model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 3D ellipse model, compute the model coefficien...
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 3d ellipse model coefficients.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 3D ellipse model.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D ellipse model.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:71
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201
Helper functor structure for concatenate.
Definition: concatenate.h:50