Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////
23template <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//////////////////////////////////////////////////////////////////////////
51template <typename PointT> bool
52pcl::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 }
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).\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//////////////////////////////////////////////////////////////////////////
247template <typename PointT> void
248pcl::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//////////////////////////////////////////////////////////////////////////
309template <typename PointT> void
311 const Eigen::VectorXf &model_coefficients, const double threshold,
312 Indices &inliers)
313{
314 inliers.clear();
315 error_sqr_dists_.clear();
316 // Check if the model is valid given the user constraints
317 if (!isModelValid (model_coefficients))
318 {
319 return;
320 }
321 inliers.reserve (indices_->size ());
322 error_sqr_dists_.reserve (indices_->size ());
323
324 // c : Ellipse Center
325 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
326 // n : Ellipse (Plane) Normal
327 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
328 // x : Ellipse (Plane) X-Axis
329 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
330 // y : Ellipse (Plane) Y-Axis
331 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
332 // a : Ellipse semi-major axis (X) length
333 const float par_a(model_coefficients[3]);
334 // b : Ellipse semi-minor axis (Y) length
335 const float par_b(model_coefficients[4]);
336
337 // Compute the rotation matrix and its transpose
338 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
339 << x_axis(0), y_axis(0), n_axis(0),
340 x_axis(1), y_axis(1), n_axis(1),
341 x_axis(2), y_axis(2), n_axis(2))
342 .finished();
343 const Eigen::Matrix3f Rot_T = Rot.transpose();
344
345 const auto squared_threshold = threshold * threshold;
346 // Iterate through the 3d points and calculate the distances from them to the ellipse
347 for (std::size_t i = 0; i < indices_->size (); ++i)
348 {
349 // p : Sample Point
350 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
351
352 // Local coordinates of sample point p
353 const Eigen::Vector3f p_ = Rot_T * (p - c);
354
355 // k : Point on Ellipse
356 // Calculate the shortest distance from the point to the ellipse which is given by
357 // the norm of a vector that is normal to the ellipse tangent calculated at the
358 // point it intersects the tangent.
359 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
360 float th_opt;
361 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
362
363 const double sqr_dist = distanceVector.squaredNorm();
364 if (sqr_dist < squared_threshold)
365 {
366 // Returns the indices of the points whose distances are smaller than the threshold
367 inliers.push_back ((*indices_)[i]);
368 error_sqr_dists_.push_back (sqr_dist);
369 }
370 }
371}
372
373//////////////////////////////////////////////////////////////////////////
374template <typename PointT> std::size_t
376 const Eigen::VectorXf &model_coefficients, const double threshold) const
377{
378 // Check if the model is valid given the user constraints
379 if (!isModelValid (model_coefficients))
380 return (0);
381 std::size_t nr_p = 0;
382
383 // c : Ellipse Center
384 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
385 // n : Ellipse (Plane) Normal
386 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
387 // x : Ellipse (Plane) X-Axis
388 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
389 // y : Ellipse (Plane) Y-Axis
390 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
391 // a : Ellipse semi-major axis (X) length
392 const float par_a(model_coefficients[3]);
393 // b : Ellipse semi-minor axis (Y) length
394 const float par_b(model_coefficients[4]);
395
396 // Compute the rotation matrix and its transpose
397 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
398 << x_axis(0), y_axis(0), n_axis(0),
399 x_axis(1), y_axis(1), n_axis(1),
400 x_axis(2), y_axis(2), n_axis(2))
401 .finished();
402 const Eigen::Matrix3f Rot_T = Rot.transpose();
403
404 const auto squared_threshold = threshold * threshold;
405 // Iterate through the 3d points and calculate the distances from them to the ellipse
406 for (std::size_t i = 0; i < indices_->size (); ++i)
407 {
408 // p : Sample Point
409 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
410
411 // Local coordinates of sample point p
412 const Eigen::Vector3f p_ = Rot_T * (p - c);
413
414 // k : Point on Ellipse
415 // Calculate the shortest distance from the point to the ellipse which is given by
416 // the norm of a vector that is normal to the ellipse tangent calculated at the
417 // point it intersects the tangent.
418 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
419 float th_opt;
420 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
421
422 if (distanceVector.squaredNorm() < squared_threshold)
423 nr_p++;
424 }
425 return (nr_p);
426}
427
428//////////////////////////////////////////////////////////////////////////
429template <typename PointT> void
431 const Indices &inliers,
432 const Eigen::VectorXf &model_coefficients,
433 Eigen::VectorXf &optimized_coefficients) const
434{
435 optimized_coefficients = model_coefficients;
436
437 // Needs a set of valid model coefficients
438 if (!isModelValid (model_coefficients))
439 {
440 PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Given model is invalid!\n");
441 return;
442 }
443
444 // Need more than the minimum sample size to make a difference
445 if (inliers.size () <= sample_size_)
446 {
447 PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
448 return;
449 }
450
451 OptimizationFunctor functor(this, inliers);
452 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
453 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm(num_diff);
454 Eigen::VectorXd coeff = model_coefficients.cast<double>();
455 int info = lm.minimize(coeff);
456 optimized_coefficients = coeff.cast<float>();
457
458 // Compute the L2 norm of the residuals
459 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\nFinal solution: %g %g %g %g %g %g %g %g %g %g %g\n",
460 info, lm.fvec.norm (),
461
462 model_coefficients[0],
463 model_coefficients[1],
464 model_coefficients[2],
465 model_coefficients[3],
466 model_coefficients[4],
467 model_coefficients[5],
468 model_coefficients[6],
469 model_coefficients[7],
470 model_coefficients[8],
471 model_coefficients[9],
472 model_coefficients[10],
473
474 optimized_coefficients[0],
475 optimized_coefficients[1],
476 optimized_coefficients[2],
477 optimized_coefficients[3],
478 optimized_coefficients[4],
479 optimized_coefficients[5],
480 optimized_coefficients[6],
481 optimized_coefficients[7],
482 optimized_coefficients[8],
483 optimized_coefficients[9],
484 optimized_coefficients[10]);
485}
486
487//////////////////////////////////////////////////////////////////////////
488template <typename PointT> void
490 const Indices &inliers, const Eigen::VectorXf &model_coefficients,
491 PointCloud &projected_points, bool copy_data_fields) const
492{
493 // Needs a valid set of model coefficients
494 if (!isModelValid (model_coefficients))
495 {
496 PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::projectPoints] Given model is invalid!\n");
497 return;
498 }
499
500 projected_points.header = input_->header;
501 projected_points.is_dense = input_->is_dense;
502
503 // Copy all the data fields from the input cloud to the projected one?
504 if (copy_data_fields)
505 {
506 // Allocate enough space and copy the basics
507 projected_points.resize (input_->size ());
508 projected_points.width = input_->width;
509 projected_points.height = input_->height;
510
511 using FieldList = typename pcl::traits::fieldList<PointT>::type;
512 // Iterate over each point
513 for (std::size_t i = 0; i < projected_points.size(); ++i)
514 {
515 // Iterate over each dimension
516 pcl::for_each_type<FieldList>(NdConcatenateFunctor<PointT, PointT>((*input_)[i], projected_points[i]));
517 }
518
519 // c : Ellipse Center
520 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
521 // n : Ellipse (Plane) Normal
522 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
523 // x : Ellipse (Plane) X-Axis
524 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
525 // y : Ellipse (Plane) Y-Axis
526 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
527 // a : Ellipse semi-major axis (X) length
528 const float par_a(model_coefficients[3]);
529 // b : Ellipse semi-minor axis (Y) length
530 const float par_b(model_coefficients[4]);
531
532 // Compute the rotation matrix and its transpose
533 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
534 << x_axis(0), y_axis(0), n_axis(0),
535 x_axis(1), y_axis(1), n_axis(1),
536 x_axis(2), y_axis(2), n_axis(2))
537 .finished();
538 const Eigen::Matrix3f Rot_T = Rot.transpose();
539
540 // Iterate through the 3d points and calculate the distances from them to the plane
541 for (std::size_t i = 0; i < inliers.size (); ++i)
542 {
543 // p : Sample Point
544 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
545
546 // Local coordinates of sample point p
547 const Eigen::Vector3f p_ = Rot_T * (p - c);
548
549 // k : Point on Ellipse
550 // Calculate the shortest distance from the point to the ellipse which is given by
551 // the norm of a vector that is normal to the ellipse tangent calculated at the
552 // point it intersects the tangent.
553 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
554 float th_opt;
555 dvec2ellipse(params, p_(0), p_(1), th_opt);
556
557 // Retrieve the ellipse point at the tilt angle t, along the local x-axis
558 Eigen::Vector3f k_(0.0, 0.0, 0.0);
559 get_ellipse_point(params, th_opt, k_[0], k_[1]);
560
561 const Eigen::Vector3f k = c + Rot * k_;
562
563 projected_points[i].x = static_cast<float> (k[0]);
564 projected_points[i].y = static_cast<float> (k[1]);
565 projected_points[i].z = static_cast<float> (k[2]);
566 }
567 }
568 else
569 {
570 // Allocate enough space and copy the basics
571 projected_points.resize (inliers.size ());
572 projected_points.width = inliers.size ();
573 projected_points.height = 1;
574
575 using FieldList = typename pcl::traits::fieldList<PointT>::type;
576 // Iterate over each point
577 for (std::size_t i = 0; i < inliers.size (); ++i)
578 // Iterate over each dimension
579 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
580
581 // c : Ellipse Center
582 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
583 // n : Ellipse (Plane) Normal
584 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
585 // x : Ellipse (Plane) X-Axis
586 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
587 // y : Ellipse (Plane) Y-Axis
588 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
589 // a : Ellipse semi-major axis (X) length
590 const float par_a(model_coefficients[3]);
591 // b : Ellipse semi-minor axis (Y) length
592 const float par_b(model_coefficients[4]);
593
594 // Compute the rotation matrix and its transpose
595 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
596 << x_axis(0), y_axis(0), n_axis(0),
597 x_axis(1), y_axis(1), n_axis(1),
598 x_axis(2), y_axis(2), n_axis(2))
599 .finished();
600 const Eigen::Matrix3f Rot_T = Rot.transpose();
601
602 // Iterate through the 3d points and calculate the distances from them to the plane
603 for (std::size_t i = 0; i < inliers.size (); ++i)
604 {
605 // p : Sample Point
606 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
607
608 // Local coordinates of sample point p
609 const Eigen::Vector3f p_ = Rot_T * (p - c);
610
611 // k : Point on Ellipse
612 // Calculate the shortest distance from the point to the ellipse which is given by
613 // the norm of a vector that is normal to the ellipse tangent calculated at the
614 // point it intersects the tangent.
615 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
616 float th_opt;
617 dvec2ellipse(params, p_(0), p_(1), th_opt);
618
619 // Retrieve the ellipse point at the tilt angle t, along the local x-axis
620 //// model_coefficients[5] = static_cast<float>(par_t);
621 Eigen::Vector3f k_(0.0, 0.0, 0.0);
622 get_ellipse_point(params, th_opt, k_[0], k_[1]);
623
624 const Eigen::Vector3f k = c + Rot * k_;
625
626 projected_points[i].x = static_cast<float> (k[0]);
627 projected_points[i].y = static_cast<float> (k[1]);
628 projected_points[i].z = static_cast<float> (k[2]);
629 }
630 }
631}
632
633//////////////////////////////////////////////////////////////////////////
634template <typename PointT> bool
636 const std::set<index_t> &indices,
637 const Eigen::VectorXf &model_coefficients,
638 const double threshold) const
639{
640 // Needs a valid model coefficients
641 if (!isModelValid (model_coefficients))
642 {
643 PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::doSamplesVerifyModel] Given model is invalid!\n");
644 return (false);
645 }
646
647 // c : Ellipse Center
648 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
649 // n : Ellipse (Plane) Normal
650 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
651 // x : Ellipse (Plane) X-Axis
652 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
653 // y : Ellipse (Plane) Y-Axis
654 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
655 // a : Ellipse semi-major axis (X) length
656 const float par_a(model_coefficients[3]);
657 // b : Ellipse semi-minor axis (Y) length
658 const float par_b(model_coefficients[4]);
659
660 // Compute the rotation matrix and its transpose
661 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
662 << x_axis(0), y_axis(0), n_axis(0),
663 x_axis(1), y_axis(1), n_axis(1),
664 x_axis(2), y_axis(2), n_axis(2))
665 .finished();
666 const Eigen::Matrix3f Rot_T = Rot.transpose();
667
668 const auto squared_threshold = threshold * threshold;
669 for (const auto &index : indices)
670 {
671 // p : Sample Point
672 const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
673
674 // Local coordinates of sample point p
675 const Eigen::Vector3f p_ = Rot_T * (p - c);
676
677 // k : Point on Ellipse
678 // Calculate the shortest distance from the point to the ellipse which is given by
679 // the norm of a vector that is normal to the ellipse tangent calculated at the
680 // point it intersects the tangent.
681 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
682 float th_opt;
683 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
684
685 if (distanceVector.squaredNorm() > squared_threshold)
686 return (false);
687 }
688 return (true);
689}
690
691//////////////////////////////////////////////////////////////////////////
692template <typename PointT> bool
693pcl::SampleConsensusModelEllipse3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
694{
695 if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
696 return (false);
697
698 if (radius_min_ != std::numeric_limits<double>::lowest() && (model_coefficients[3] < radius_min_ || model_coefficients[4] < radius_min_))
699 {
700 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",
701 radius_min_, model_coefficients[3], model_coefficients[4]);
702 return (false);
703 }
704 if (radius_max_ != std::numeric_limits<double>::max() && (model_coefficients[3] > radius_max_ || model_coefficients[4] > radius_max_))
705 {
706 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",
707 radius_max_, model_coefficients[3], model_coefficients[4]);
708 return (false);
709 }
710
711 return (true);
712}
713
714
715
716//////////////////////////////////////////////////////////////////////////
717template <typename PointT>
719 const Eigen::VectorXf& par, float th, float& x, float& y)
720{
721 /*
722 * Calculates a point on the ellipse model 'par' using the angle 'th'.
723 */
724
725 // Parametric eq.params
726 const float par_a(par[0]);
727 const float par_b(par[1]);
728 const float par_h(par[2]);
729 const float par_k(par[3]);
730 const float par_t(par[4]);
731
732 x = par_h + std::cos(par_t) * par_a * std::cos(th) -
733 std::sin(par_t) * par_b * std::sin(th);
734 y = par_k + std::sin(par_t) * par_a * std::cos(th) +
735 std::cos(par_t) * par_b * std::sin(th);
736
737 return;
738}
739
740//////////////////////////////////////////////////////////////////////////
741template <typename PointT>
743 const Eigen::VectorXf& par, float u, float v, float& th_opt)
744{
745 /*
746 * Minimum distance vector from point p=(u,v) to the ellipse model 'par'.
747 */
748
749 // Parametric eq.params
750 // (par_a, par_b, and par_t do not need to be declared)
751 const float par_h = par[2];
752 const float par_k = par[3];
753
754 const Eigen::Vector2f center(par_h, par_k);
755 Eigen::Vector2f p(u, v);
756 p -= center;
757
758 // Local x-axis of the ellipse
759 Eigen::Vector2f x_axis(0.0, 0.0);
760 get_ellipse_point(par, 0.0, x_axis(0), x_axis(1));
761 x_axis -= center;
762
763 // Local y-axis of the ellipse
764 Eigen::Vector2f y_axis(0.0, 0.0);
765 get_ellipse_point(par, M_PI / 2.0, y_axis(0), y_axis(1));
766 y_axis -= center;
767
768 // Convert the point p=(u,v) to local ellipse coordinates
769 const float x_proj = p.dot(x_axis) / x_axis.norm();
770 const float y_proj = p.dot(y_axis) / y_axis.norm();
771
772 // Find the ellipse quandrant to where the point p=(u,v) belongs,
773 // and limit the search interval to 'th_min' and 'th_max'.
774 float th_min(0.0), th_max(0.0);
775 const float th = std::atan2(y_proj, x_proj);
776
777 if (-M_PI <= th && th < -M_PI / 2.0) {
778 // Q3
779 th_min = -M_PI;
780 th_max = -M_PI / 2.0;
781 }
782 if (-M_PI / 2.0 <= th && th < 0.0) {
783 // Q4
784 th_min = -M_PI / 2.0;
785 th_max = 0.0;
786 }
787 if (0.0 <= th && th < M_PI / 2.0) {
788 // Q1
789 th_min = 0.0;
790 th_max = M_PI / 2.0;
791 }
792 if (M_PI / 2.0 <= th && th <= M_PI) {
793 // Q2
794 th_min = M_PI / 2.0;
795 th_max = M_PI;
796 }
797
798 // Use an unconstrained line search optimizer to find the optimal th_opt
799 th_opt = golden_section_search(par, u, v, th_min, th_max, 1.e-3);
800
801 // Distance vector from a point (u,v) to a given point in the ellipse model 'par' at an angle 'th_opt'.
802 float x(0.0), y(0.0);
803 get_ellipse_point(par, th_opt, x, y);
804 Eigen::Vector2f distanceVector(u - x, v - y);
805 return distanceVector;
806}
807
808//////////////////////////////////////////////////////////////////////////
809template <typename PointT>
811 const Eigen::VectorXf& par,
812 float u,
813 float v,
814 float th_min,
815 float th_max,
816 float epsilon)
817{
818 /*
819 * Golden section search
820 */
821
822 constexpr float phi(1.61803398874989484820f); // Golden ratio
823
824 // tl (theta lower bound), tu (theta upper bound)
825 float tl(th_min), tu(th_max);
826 float ta = tl + (tu - tl) * (1 - 1 / phi);
827 float tb = tl + (tu - tl) * 1 / phi;
828
829 while ((tu - tl) > epsilon) {
830
831 // theta a
832 float x_a(0.0), y_a(0.0);
833 get_ellipse_point(par, ta, x_a, y_a);
834 float squared_dist_ta = (u - x_a) * (u - x_a) + (v - y_a) * (v - y_a);
835
836 // theta b
837 float x_b(0.0), y_b(0.0);
838 get_ellipse_point(par, tb, x_b, y_b);
839 float squared_dist_tb = (u - x_b) * (u - x_b) + (v - y_b) * (v - y_b);
840
841 if (squared_dist_ta < squared_dist_tb) {
842 tu = tb;
843 tb = ta;
844 ta = tl + (tu - tl) * (1 - 1 / phi);
845 }
846 else if (squared_dist_ta > squared_dist_tb) {
847 tl = ta;
848 ta = tb;
849 tb = tl + (tu - tl) * 1 / phi;
850 }
851 else {
852 tl = ta;
853 tu = tb;
854 ta = tl + (tu - tl) * (1 - 1 / phi);
855 tb = tl + (tu - tl) * 1 / phi;
856 }
857 }
858 return (tl + tu) / 2.0;
859}
860
861
862#define PCL_INSTANTIATE_SampleConsensusModelEllipse3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelEllipse3D<T>;
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...
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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:203
Helper functor structure for concatenate.
Definition concatenate.h:50