Point Cloud Library (PCL) 1.13.0
eigen.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 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 */
38
39#pragma once
40
41#include <pcl/common/eigen.h>
42#include <pcl/console/print.h>
43
44#include <array>
45#include <algorithm>
46#include <cmath>
47
48
49namespace pcl
50{
51
52template <typename Scalar, typename Roots> inline void
53computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
54{
55 roots (0) = Scalar (0);
56 Scalar d = Scalar (b * b - 4.0 * c);
57 if (d < 0.0) // no real roots ! THIS SHOULD NOT HAPPEN!
58 d = 0.0;
59
60 Scalar sd = std::sqrt (d);
61
62 roots (2) = 0.5f * (b + sd);
63 roots (1) = 0.5f * (b - sd);
64}
65
66
67template <typename Matrix, typename Roots> inline void
68computeRoots (const Matrix& m, Roots& roots)
69{
70 using Scalar = typename Matrix::Scalar;
71
72 // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
73 // eigenvalues are the roots to this equation, all guaranteed to be
74 // real-valued, because the matrix is symmetric.
75 Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2)
76 + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2)
77 - m (0, 0) * m (1, 2) * m (1, 2)
78 - m (1, 1) * m (0, 2) * m (0, 2)
79 - m (2, 2) * m (0, 1) * m (0, 1);
80 Scalar c1 = m (0, 0) * m (1, 1) -
81 m (0, 1) * m (0, 1) +
82 m (0, 0) * m (2, 2) -
83 m (0, 2) * m (0, 2) +
84 m (1, 1) * m (2, 2) -
85 m (1, 2) * m (1, 2);
86 Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
87
88 if (std::abs (c0) < Eigen::NumTraits < Scalar > ::epsilon ()) // one root is 0 -> quadratic equation
89 computeRoots2 (c2, c1, roots);
90 else
91 {
92 const Scalar s_inv3 = Scalar (1.0 / 3.0);
93 const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
94 // Construct the parameters used in classifying the roots of the equation
95 // and in solving the equation for the roots in closed form.
96 Scalar c2_over_3 = c2 * s_inv3;
97 Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
98 if (a_over_3 > Scalar (0))
99 a_over_3 = Scalar (0);
100
101 Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
102
103 Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
104 if (q > Scalar (0))
105 q = Scalar (0);
106
107 // Compute the eigenvalues by solving for the roots of the polynomial.
108 Scalar rho = std::sqrt (-a_over_3);
109 Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3;
110 Scalar cos_theta = std::cos (theta);
111 Scalar sin_theta = std::sin (theta);
112 roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta;
113 roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
114 roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
115
116 // Sort in increasing order.
117 if (roots (0) >= roots (1))
118 std::swap (roots (0), roots (1));
119 if (roots (1) >= roots (2))
120 {
121 std::swap (roots (1), roots (2));
122 if (roots (0) >= roots (1))
123 std::swap (roots (0), roots (1));
124 }
125
126 if (roots (0) <= 0) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0
127 computeRoots2 (c2, c1, roots);
128 }
129}
130
131
132template <typename Matrix, typename Vector> inline void
133eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
134{
135 // if diagonal matrix, the eigenvalues are the diagonal elements
136 // and the eigenvectors are not unique, thus set to Identity
137 if (std::abs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
138 {
139 if (mat.coeff (0) < mat.coeff (2))
140 {
141 eigenvalue = mat.coeff (0);
142 eigenvector[0] = 1.0;
143 eigenvector[1] = 0.0;
144 }
145 else
146 {
147 eigenvalue = mat.coeff (2);
148 eigenvector[0] = 0.0;
149 eigenvector[1] = 1.0;
150 }
151 return;
152 }
153
154 // 0.5 to optimize further calculations
155 typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
156 typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
157
158 typename Matrix::Scalar temp = trace * trace - determinant;
159
160 if (temp < 0)
161 temp = 0;
162
163 eigenvalue = trace - std::sqrt (temp);
164
165 eigenvector[0] = -mat.coeff (1);
166 eigenvector[1] = mat.coeff (0) - eigenvalue;
167 eigenvector.normalize ();
168}
169
170
171template <typename Matrix, typename Vector> inline void
172eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
173{
174 // if diagonal matrix, the eigenvalues are the diagonal elements
175 // and the eigenvectors are not unique, thus set to Identity
176 if (std::abs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
177 {
178 if (mat.coeff (0) < mat.coeff (3))
179 {
180 eigenvalues.coeffRef (0) = mat.coeff (0);
181 eigenvalues.coeffRef (1) = mat.coeff (3);
182 eigenvectors.coeffRef (0) = 1.0;
183 eigenvectors.coeffRef (1) = 0.0;
184 eigenvectors.coeffRef (2) = 0.0;
185 eigenvectors.coeffRef (3) = 1.0;
186 }
187 else
188 {
189 eigenvalues.coeffRef (0) = mat.coeff (3);
190 eigenvalues.coeffRef (1) = mat.coeff (0);
191 eigenvectors.coeffRef (0) = 0.0;
192 eigenvectors.coeffRef (1) = 1.0;
193 eigenvectors.coeffRef (2) = 1.0;
194 eigenvectors.coeffRef (3) = 0.0;
195 }
196 return;
197 }
198
199 // 0.5 to optimize further calculations
200 typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
201 typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
202
203 typename Matrix::Scalar temp = trace * trace - determinant;
204
205 if (temp < 0)
206 temp = 0;
207 else
208 temp = std::sqrt (temp);
209
210 eigenvalues.coeffRef (0) = trace - temp;
211 eigenvalues.coeffRef (1) = trace + temp;
212
213 // either this is in a row or column depending on RowMajor or ColumnMajor
214 eigenvectors.coeffRef (0) = -mat.coeff (1);
215 eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0);
216 typename Matrix::Scalar norm = static_cast<typename Matrix::Scalar> (1.0)
217 / static_cast<typename Matrix::Scalar> (std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2)));
218 eigenvectors.coeffRef (0) *= norm;
219 eigenvectors.coeffRef (2) *= norm;
220 eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2);
221 eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
222}
223
224
225template <typename Matrix, typename Vector> inline void
226computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
227{
228 using Scalar = typename Matrix::Scalar;
229 // Scale the matrix so its entries are in [-1,1]. The scaling is applied
230 // only when at least one matrix entry has magnitude larger than 1.
231
232 Scalar scale = mat.cwiseAbs ().maxCoeff ();
233 if (scale <= std::numeric_limits < Scalar > ::min ())
234 scale = Scalar (1.0);
235
236 Matrix scaledMat = mat / scale;
237
238 scaledMat.diagonal ().array () -= eigenvalue / scale;
239
240 Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
241 Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
242 Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
243
244 Scalar len1 = vec1.squaredNorm ();
245 Scalar len2 = vec2.squaredNorm ();
246 Scalar len3 = vec3.squaredNorm ();
247
248 if (len1 >= len2 && len1 >= len3)
249 eigenvector = vec1 / std::sqrt (len1);
250 else if (len2 >= len1 && len2 >= len3)
251 eigenvector = vec2 / std::sqrt (len2);
252 else
253 eigenvector = vec3 / std::sqrt (len3);
254}
255
256namespace detail
257{
258
259template <typename Vector, typename Scalar>
261 Vector vector;
262 Scalar length;
263}; // struct EigenVector
264
265/**
266 * @brief returns the unit vector along the largest eigen value as well as the
267 * length of the largest eigenvector
268 * @tparam Vector Requested result type, needs to be explicitly provided and has
269 * to be implicitly constructible from ConstRowExpr
270 * @tparam Matrix deduced input type providing similar in API as Eigen::Matrix
271 */
272template <typename Vector, typename Matrix> static EigenVector<Vector, typename Matrix::Scalar>
273getLargest3x3Eigenvector (const Matrix scaledMatrix)
274{
275 using Scalar = typename Matrix::Scalar;
276 using Index = typename Matrix::Index;
277
278 Matrix crossProduct;
279 crossProduct << scaledMatrix.row (0).cross (scaledMatrix.row (1)),
280 scaledMatrix.row (0).cross (scaledMatrix.row (2)),
281 scaledMatrix.row (1).cross (scaledMatrix.row (2));
282
283 // expression template, no evaluation here
284 const auto len = crossProduct.rowwise ().norm ();
285
286 Index index;
287 const Scalar length = len.maxCoeff (&index); // <- first evaluation
288 return EigenVector<Vector, Scalar> {crossProduct.row (index) / length,
289 length};
290}
291
292} // namespace detail
293
294
295template <typename Matrix, typename Vector> inline void
296eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
297{
298 using Scalar = typename Matrix::Scalar;
299 // Scale the matrix so its entries are in [-1,1]. The scaling is applied
300 // only when at least one matrix entry has magnitude larger than 1.
301
302 Scalar scale = mat.cwiseAbs ().maxCoeff ();
303 if (scale <= std::numeric_limits < Scalar > ::min ())
304 scale = Scalar (1.0);
305
306 Matrix scaledMat = mat / scale;
307
308 Vector eigenvalues;
309 computeRoots (scaledMat, eigenvalues);
310
311 eigenvalue = eigenvalues (0) * scale;
312
313 scaledMat.diagonal ().array () -= eigenvalues (0);
314
315 eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
316}
317
318
319template <typename Matrix, typename Vector> inline void
320eigen33 (const Matrix& mat, Vector& evals)
321{
322 using Scalar = typename Matrix::Scalar;
323 Scalar scale = mat.cwiseAbs ().maxCoeff ();
324 if (scale <= std::numeric_limits < Scalar > ::min ())
325 scale = Scalar (1.0);
326
327 Matrix scaledMat = mat / scale;
328 computeRoots (scaledMat, evals);
329 evals *= scale;
330}
331
332
333template <typename Matrix, typename Vector> inline void
334eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
335{
336 using Scalar = typename Matrix::Scalar;
337 // Scale the matrix so its entries are in [-1,1]. The scaling is applied
338 // only when at least one matrix entry has magnitude larger than 1.
339
340 Scalar scale = mat.cwiseAbs ().maxCoeff ();
341 if (scale <= std::numeric_limits < Scalar > ::min ())
342 scale = Scalar (1.0);
343
344 Matrix scaledMat = mat / scale;
345
346 // Compute the eigenvalues
347 computeRoots (scaledMat, evals);
348
349 if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
350 {
351 // all three equal
352 evecs.setIdentity ();
353 }
354 else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
355 {
356 // first and second equal
357 Matrix tmp;
358 tmp = scaledMat;
359 tmp.diagonal ().array () -= evals (2);
360
361 evecs.col (2) = detail::getLargest3x3Eigenvector<Vector> (tmp).vector;
362 evecs.col (1) = evecs.col (2).unitOrthogonal ();
363 evecs.col (0) = evecs.col (1).cross (evecs.col (2));
364 }
365 else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ())
366 {
367 // second and third equal
368 Matrix tmp;
369 tmp = scaledMat;
370 tmp.diagonal ().array () -= evals (0);
371
372 evecs.col (0) = detail::getLargest3x3Eigenvector<Vector> (tmp).vector;
373 evecs.col (1) = evecs.col (0).unitOrthogonal ();
374 evecs.col (2) = evecs.col (0).cross (evecs.col (1));
375 }
376 else
377 {
378 std::array<Scalar, 3> eigenVecLen;
379
380 for (int i = 0; i < 3; ++i)
381 {
382 Matrix tmp = scaledMat;
383 tmp.diagonal ().array () -= evals (i);
384 const auto vec_len = detail::getLargest3x3Eigenvector<Vector> (tmp);
385 evecs.col (i) = vec_len.vector;
386 eigenVecLen[i] = vec_len.length;
387 }
388
389 // @TODO: might be redundant or over-complicated as per @SergioRAgostinho
390 // see: https://github.com/PointCloudLibrary/pcl/pull/3441#discussion_r341024181
391 const auto minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ());
392 int min_idx = std::distance (eigenVecLen.cbegin (), minmax_it.first);
393 int max_idx = std::distance (eigenVecLen.cbegin (), minmax_it.second);
394 int mid_idx = 3 - min_idx - max_idx;
395
396 evecs.col (min_idx) = evecs.col ( (min_idx + 1) % 3).cross (evecs.col ( (min_idx + 2) % 3)).normalized ();
397 evecs.col (mid_idx) = evecs.col ( (mid_idx + 1) % 3).cross (evecs.col ( (mid_idx + 2) % 3)).normalized ();
398 }
399 // Rescale back to the original size.
400 evals *= scale;
401}
402
403
404template <typename Matrix> inline typename Matrix::Scalar
405invert2x2 (const Matrix& matrix, Matrix& inverse)
406{
407 using Scalar = typename Matrix::Scalar;
408 Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2);
409
410 if (det != 0)
411 {
412 //Scalar inv_det = Scalar (1.0) / det;
413 inverse.coeffRef (0) = matrix.coeff (3);
414 inverse.coeffRef (1) = -matrix.coeff (1);
415 inverse.coeffRef (2) = -matrix.coeff (2);
416 inverse.coeffRef (3) = matrix.coeff (0);
417 inverse /= det;
418 }
419 return det;
420}
421
422
423template <typename Matrix> inline typename Matrix::Scalar
424invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
425{
426 using Scalar = typename Matrix::Scalar;
427 // elements
428 // a b c
429 // b d e
430 // c e f
431 //| a b c |-1 | fd-ee ce-bf be-cd |
432 //| b d e | = 1/det * | ce-bf af-cc bc-ae |
433 //| c e f | | be-cd bc-ae ad-bb |
434
435 //det = a(fd-ee) + b(ec-fb) + c(eb-dc)
436
437 Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
438 Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
439 Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
440
441 Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
442
443 if (det != 0)
444 {
445 //Scalar inv_det = Scalar (1.0) / det;
446 inverse.coeffRef (0) = fd_ee;
447 inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
448 inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
449 inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
450 inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
451 inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
452 inverse /= det;
453 }
454 return det;
455}
456
457
458template <typename Matrix> inline typename Matrix::Scalar
459invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
460{
461 using Scalar = typename Matrix::Scalar;
462
463 //| a b c |-1 | ie-hf hc-ib fb-ec |
464 //| d e f | = 1/det * | gf-id ia-gc dc-fa |
465 //| g h i | | hd-ge gb-ha ea-db |
466 //det = a(ie-hf) + d(hc-ib) + g(fb-ec)
467
468 Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
469 Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
470 Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
471 Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec);
472
473 if (det != 0)
474 {
475 inverse.coeffRef (0) = ie_hf;
476 inverse.coeffRef (1) = hc_ib;
477 inverse.coeffRef (2) = fb_ec;
478 inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
479 inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
480 inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
481 inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
482 inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
483 inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
484
485 inverse /= det;
486 }
487 return det;
488}
489
490
491template <typename Matrix> inline typename Matrix::Scalar
492determinant3x3Matrix (const Matrix& matrix)
493{
494 // result is independent of Row/Col Major storage!
495 return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
496 matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
497 matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
498}
499
500
501void
502getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
503 const Eigen::Vector3f& y_direction,
504 Eigen::Affine3f& transformation)
505{
506 Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized();
507 Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized();
508 Eigen::Vector3f tmp2 = z_axis.normalized();
509
510 transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
511 transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
512 transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
513 transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
514}
515
516
517Eigen::Affine3f
518getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
519 const Eigen::Vector3f& y_direction)
520{
521 Eigen::Affine3f transformation;
522 getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
523 return (transformation);
524}
525
526
527void
528getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
529 const Eigen::Vector3f& y_direction,
530 Eigen::Affine3f& transformation)
531{
532 Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized();
533 Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized();
534 Eigen::Vector3f tmp0 = x_axis.normalized();
535
536 transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
537 transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
538 transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
539 transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
540}
541
542
543Eigen::Affine3f
544getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
545 const Eigen::Vector3f& y_direction)
546{
547 Eigen::Affine3f transformation;
548 getTransFromUnitVectorsXY (x_axis, y_direction, transformation);
549 return (transformation);
550}
551
552
553void
554getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
555 const Eigen::Vector3f& z_axis,
556 Eigen::Affine3f& transformation)
557{
558 getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
559}
560
561
562Eigen::Affine3f
563getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
564 const Eigen::Vector3f& z_axis)
565{
566 Eigen::Affine3f transformation;
567 getTransformationFromTwoUnitVectors (y_direction, z_axis, transformation);
568 return (transformation);
569}
570
571
572void
573getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
574 const Eigen::Vector3f& z_axis,
575 const Eigen::Vector3f& origin,
576 Eigen::Affine3f& transformation)
577{
578 getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation);
579 Eigen::Vector3f translation = transformation*origin;
580 transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2];
581}
582
583
584template <typename Scalar> void
585getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
586{
587 roll = std::atan2 (t (2, 1), t (2, 2));
588 pitch = asin (-t (2, 0));
589 yaw = std::atan2 (t (1, 0), t (0, 0));
590}
591
592
593template <typename Scalar> void
594getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
595 Scalar &x, Scalar &y, Scalar &z,
596 Scalar &roll, Scalar &pitch, Scalar &yaw)
597{
598 x = t (0, 3);
599 y = t (1, 3);
600 z = t (2, 3);
601 roll = std::atan2 (t (2, 1), t (2, 2));
602 pitch = asin (-t (2, 0));
603 yaw = std::atan2 (t (1, 0), t (0, 0));
604}
605
606
607template <typename Scalar> void
608getTransformation (Scalar x, Scalar y, Scalar z,
609 Scalar roll, Scalar pitch, Scalar yaw,
610 Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
611{
612 Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch),
613 E = std::cos (roll), F = sin (roll), DE = D*E, DF = D*F;
614
615 t (0, 0) = A*C; t (0, 1) = A*DF - B*E; t (0, 2) = B*F + A*DE; t (0, 3) = x;
616 t (1, 0) = B*C; t (1, 1) = A*E + B*DF; t (1, 2) = B*DE - A*F; t (1, 3) = y;
617 t (2, 0) = -D; t (2, 1) = C*F; t (2, 2) = C*E; t (2, 3) = z;
618 t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1;
619}
620
621
622template <typename Derived> void
623saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file)
624{
625 std::uint32_t rows = static_cast<std::uint32_t> (matrix.rows ()), cols = static_cast<std::uint32_t> (matrix.cols ());
626 file.write (reinterpret_cast<char*> (&rows), sizeof (rows));
627 file.write (reinterpret_cast<char*> (&cols), sizeof (cols));
628 for (std::uint32_t i = 0; i < rows; ++i)
629 for (std::uint32_t j = 0; j < cols; ++j)
630 {
631 typename Derived::Scalar tmp = matrix(i,j);
632 file.write (reinterpret_cast<const char*> (&tmp), sizeof (tmp));
633 }
634}
635
636
637template <typename Derived> void
638loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
639{
640 Eigen::MatrixBase<Derived> &matrix = const_cast<Eigen::MatrixBase<Derived> &> (matrix_);
641
642 std::uint32_t rows, cols;
643 file.read (reinterpret_cast<char*> (&rows), sizeof (rows));
644 file.read (reinterpret_cast<char*> (&cols), sizeof (cols));
645 if (matrix.rows () != static_cast<int>(rows) || matrix.cols () != static_cast<int>(cols))
646 matrix.derived().resize(rows, cols);
647
648 for (std::uint32_t i = 0; i < rows; ++i)
649 for (std::uint32_t j = 0; j < cols; ++j)
650 {
651 typename Derived::Scalar tmp;
652 file.read (reinterpret_cast<char*> (&tmp), sizeof (tmp));
653 matrix (i, j) = tmp;
654 }
655}
656
657
658template <typename Derived, typename OtherDerived>
659typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
660umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
661{
662#if EIGEN_VERSION_AT_LEAST (3, 3, 0)
663 return Eigen::umeyama (src, dst, with_scaling);
664#else
665 using TransformationMatrixType = typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type;
666 using Scalar = typename Eigen::internal::traits<TransformationMatrixType>::Scalar;
667 using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
668 using Index = typename Derived::Index;
669
670 static_assert (!Eigen::NumTraits<Scalar>::IsComplex, "Numeric type must be real.");
671 static_assert ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value),
672 "You mixed different numeric types. You need to use the cast method of matrixbase to cast numeric types explicitly.");
673
674 enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
675
676 using VectorType = Eigen::Matrix<Scalar, Dimension, 1>;
677 using MatrixType = Eigen::Matrix<Scalar, Dimension, Dimension>;
678 using RowMajorMatrixType = typename Eigen::internal::plain_matrix_type_row_major<Derived>::type;
679
680 const Index m = src.rows (); // dimension
681 const Index n = src.cols (); // number of measurements
682
683 // required for demeaning ...
684 const RealScalar one_over_n = 1 / static_cast<RealScalar> (n);
685
686 // computation of mean
687 const VectorType src_mean = src.rowwise ().sum () * one_over_n;
688 const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
689
690 // demeaning of src and dst points
691 const RowMajorMatrixType src_demean = src.colwise () - src_mean;
692 const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
693
694 // Eq. (36)-(37)
695 const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
696
697 // Eq. (38)
698 const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
699
700 Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
701
702 // Initialize the resulting transformation with an identity matrix...
703 TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
704
705 // Eq. (39)
706 VectorType S = VectorType::Ones (m);
707
708 if ( svd.matrixU ().determinant () * svd.matrixV ().determinant () < 0 )
709 S (m - 1) = -1;
710
711 // Eq. (40) and (43)
712 Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
713
714 if (with_scaling)
715 {
716 // Eq. (42)
717 const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S);
718
719 // Eq. (41)
720 Rt.col (m).head (m) = dst_mean;
721 Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean;
722 Rt.block (0, 0, m, m) *= c;
723 }
724 else
725 {
726 Rt.col (m).head (m) = dst_mean;
727 Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
728 }
729
730 return (Rt);
731#endif
732}
733
734
735template <typename Scalar> bool
736transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
737 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
738 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
739{
740 if (line_in.innerSize () != 6 || line_out.innerSize () != 6)
741 {
742 PCL_DEBUG ("transformLine: lines size != 6\n");
743 return (false);
744 }
745
746 Eigen::Matrix<Scalar, 3, 1> point, vector;
747 point << line_in.template head<3> ();
748 vector << line_out.template tail<3> ();
749
750 pcl::transformPoint (point, point, transformation);
751 pcl::transformVector (vector, vector, transformation);
752 line_out << point, vector;
753 return (true);
754}
755
756
757template <typename Scalar> void
758transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
759 Eigen::Matrix<Scalar, 4, 1> &plane_out,
760 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
761{
762 Eigen::Hyperplane < Scalar, 3 > plane;
763 plane.coeffs () << plane_in;
764 plane.transform (transformation);
765 plane_out << plane.coeffs ();
766
767 // Versions prior to 3.3.2 don't normalize the result
768 #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
769 plane_out /= plane_out.template head<3> ().norm ();
770 #endif
771}
772
773
774template <typename Scalar> void
777 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
778{
779 std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
780 Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
781 pcl::transformPlane (v_plane_in, v_plane_in, transformation);
782 plane_out->values.resize (4);
783 std::copy_n(v_plane_in.data (), 4, plane_out->values.begin ());
784}
785
786
787template <typename Scalar> bool
788checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
789 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
790 const Scalar norm_limit,
791 const Scalar dot_limit)
792{
793 if (line_x.innerSize () != 6 || line_y.innerSize () != 6)
794 {
795 PCL_DEBUG ("checkCoordinateSystem: lines size != 6\n");
796 return (false);
797 }
798
799 if (line_x.template head<3> () != line_y.template head<3> ())
800 {
801 PCL_DEBUG ("checkCoorZdinateSystem: vector origins are different !\n");
802 return (false);
803 }
804
805 // Make a copy of vector directions
806 // X^Y = Z | Y^Z = X | Z^X = Y
807 Eigen::Matrix<Scalar, 3, 1> v_line_x (line_x.template tail<3> ()),
808 v_line_y (line_y.template tail<3> ()),
809 v_line_z (v_line_x.cross (v_line_y));
810
811 // Check vectors norms
812 if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit)
813 {
814 PCL_DEBUG ("checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ());
815 return (false);
816 }
817
818 if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit)
819 {
820 PCL_DEBUG ("checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ());
821 return (false);
822 }
823
824 if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit)
825 {
826 PCL_DEBUG ("checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ());
827 return (false);
828 }
829
830 // Check vectors perendicularity
831 if (std::abs (v_line_x.dot (v_line_y)) > dot_limit)
832 {
833 PCL_DEBUG ("checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit);
834 return (false);
835 }
836
837 if (std::abs (v_line_x.dot (v_line_z)) > dot_limit)
838 {
839 PCL_DEBUG ("checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit);
840 return (false);
841 }
842
843 if (std::abs (v_line_y.dot (v_line_z)) > dot_limit)
844 {
845 PCL_DEBUG ("checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit);
846 return (false);
847 }
848
849 return (true);
850}
851
852
853template <typename Scalar> bool
854transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
855 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
856 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
857 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
858 Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
859{
860 if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
861 {
862 PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n");
863 return (false);
864 }
865
866 // Check if coordinate systems are valid
867 if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y))
868 {
869 PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n");
870 return (false);
871 }
872
873 // Convert lines into Vector3 :
874 Eigen::Matrix<Scalar, 3, 1> fr0 (from_line_x.template head<3>()),
875 fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
876 fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()),
877
878 to0 (to_line_x.template head<3>()),
879 to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()),
880 to2 (to_line_y.template head<3>() + to_line_y.template tail<3>());
881
882 // Code is inspired from http://stackoverflow.com/a/15277421/1816078
883 // Define matrices and points :
884 Eigen::Transform<Scalar, 3, Eigen::Affine> T2, T3 = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
885 Eigen::Matrix<Scalar, 3, 1> x1, y1, z1, x2, y2, z2;
886
887 // Axes of the coordinate system "fr"
888 x1 = (fr1 - fr0).normalized (); // the versor (unitary vector) of the (fr1-fr0) axis vector
889 y1 = (fr2 - fr0).normalized ();
890
891 // Axes of the coordinate system "to"
892 x2 = (to1 - to0).normalized ();
893 y2 = (to2 - to0).normalized ();
894
895 // Transform from CS1 to CS2
896 // Note: if fr0 == (0,0,0) --> CS1 == CS2 --> T2 = Identity
897 T2.linear () << x1, y1, x1.cross (y1);
898
899 // Transform from CS1 to CS3
900 T3.linear () << x2, y2, x2.cross (y2);
901
902 // Identity matrix = transform to CS2 to CS3
903 // Note: if CS1 == CS2 --> transformation = T3
904 transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
905 transformation.linear () = T3.linear () * T2.linear ().inverse ();
906 transformation.translation () = to0 - (transformation.linear () * fr0);
907 return (true);
908}
909
910} // namespace pcl
911
void computeCorrespondingEigenVector(const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi defin...
Definition: eigen.hpp:226
void getTranslationAndEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &x, Scalar &y, Scalar &z, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract x,y,z and the Euler angles (intrinsic rotations, ZYX-convention) from the given transformatio...
Definition: eigen.hpp:594
void getTransformationFromTwoUnitVectorsAndOrigin(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, const Eigen::Vector3f &origin, Eigen::Affine3f &transformation)
Get the transformation that will translate origin to (0,0,0) and rotate z_axis into (0,...
Definition: eigen.hpp:573
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Definition: eigen.hpp:492
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition: eigen.hpp:424
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
Definition: eigen.hpp:638
void getTransformation(Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform< Scalar, 3, Eigen::Affine > &t)
Create a transformation from the given translation and Euler angles (intrinsic rotations,...
Definition: eigen.hpp:608
void getEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract the Euler angles (intrinsic rotations, ZYX-convention) from the given transformation.
Definition: eigen.hpp:585
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Definition: eigen.hpp:133
void getTransformationFromTwoUnitVectors(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate z_axis into (0,0,1) and y_direction into a vector with x=...
Definition: eigen.hpp:554
void getTransFromUnitVectorsXY(const Eigen::Vector3f &x_axis, const Eigen::Vector3f &y_direction, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate x_axis into (1,0,0) and y_direction into a vector with z=...
Definition: eigen.hpp:528
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
Definition: eigen.hpp:459
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:296
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Definition: eigen.hpp:623
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
Definition: eigen.hpp:405
void getTransFromUnitVectorsZY(const Eigen::Vector3f &z_axis, const Eigen::Vector3f &y_direction, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate z_axis into (0,0,1) and y_direction into a vector with x=...
Definition: eigen.hpp:502
@ B
Definition: norms.h:54
static EigenVector< Vector, typename Matrix::Scalar > getLargest3x3Eigenvector(const Matrix scaledMatrix)
returns the unit vector along the largest eigen value as well as the length of the largest eigenvecto...
Definition: eigen.hpp:273
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
bool checkCoordinateSystem(const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_x, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_y, const Scalar norm_limit=1e-3, const Scalar dot_limit=1e-3)
Check coordinate system integrity.
Definition: eigen.hpp:788
void transformPlane(const Eigen::Matrix< Scalar, 4, 1 > &plane_in, Eigen::Matrix< Scalar, 4, 1 > &plane_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform plane vectors using an affine matrix.
Definition: eigen.hpp:758
void transformPoint(const Eigen::Matrix< Scalar, 3, 1 > &point_in, Eigen::Matrix< Scalar, 3, 1 > &point_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a point using an affine matrix.
Definition: eigen.h:389
void transformVector(const Eigen::Matrix< Scalar, 3, 1 > &vector_in, Eigen::Matrix< Scalar, 3, 1 > &vector_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a vector using an affine matrix.
Definition: eigen.h:406
Eigen::internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const Eigen::MatrixBase< Derived > &src, const Eigen::MatrixBase< OtherDerived > &dst, bool with_scaling=false)
Returns the transformation between two point sets.
Definition: eigen.hpp:660
bool transformBetween2CoordinateSystems(const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > from_line_x, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > from_line_y, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > to_line_x, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > to_line_y, Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Compute the transformation between two coordinate systems.
Definition: eigen.hpp:854
bool transformLine(const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_in, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a line using an affine matrix.
Definition: eigen.hpp:736
void computeRoots2(const Scalar &b, const Scalar &c, Roots &roots)
Compute the roots of a quadratic polynom x^2 + b*x + c = 0.
Definition: eigen.hpp:53
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
Definition: eigen.hpp:68
shared_ptr< ::pcl::ModelCoefficients > Ptr
shared_ptr< const ::pcl::ModelCoefficients > ConstPtr