Point Cloud Library (PCL)  1.14.0-dev
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 
49 namespace pcl
50 {
51 
52 template <typename Scalar, typename Roots> inline void
53 computeRoots2 (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 
67 template <typename Matrix, typename Roots> inline void
68 computeRoots (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  constexpr 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 
132 template <typename Matrix, typename Vector> inline void
133 eigen22 (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 
171 template <typename Matrix, typename Vector> inline void
172 eigen22 (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 
225 template <typename Matrix, typename Vector> inline void
226 computeCorrespondingEigenVector (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 
256 namespace detail
257 {
258 
259 template <typename Vector, typename Scalar>
260 struct EigenVector {
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  */
272 template <typename Vector, typename Matrix> static EigenVector<Vector, typename Matrix::Scalar>
273 getLargest3x3Eigenvector (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 {crossProduct.row (index) / length, length};
289 }
290 
291 } // namespace detail
292 
293 
294 template <typename Matrix, typename Vector> inline void
295 eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
296 {
297  using Scalar = typename Matrix::Scalar;
298  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
299  // only when at least one matrix entry has magnitude larger than 1.
300 
301  Scalar scale = mat.cwiseAbs ().maxCoeff ();
302  if (scale <= std::numeric_limits < Scalar > ::min ())
303  scale = Scalar (1.0);
304 
305  Matrix scaledMat = mat / scale;
306 
307  Vector eigenvalues;
308  computeRoots (scaledMat, eigenvalues);
309 
310  eigenvalue = eigenvalues (0) * scale;
311  if ( (eigenvalues (1) - eigenvalues (0)) > Eigen::NumTraits < Scalar > ::epsilon ()) {
312  // usual case: first and second are not equal (so first and third are also not equal).
313  // second and third could be equal, but that does not matter here
314  scaledMat.diagonal ().array () -= eigenvalues (0);
315  eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
316  }
317  else if ( (eigenvalues (2) - eigenvalues (0)) > Eigen::NumTraits < Scalar > ::epsilon ()) {
318  // first and second equal: choose any unit vector that is orthogonal to third eigenvector
319  scaledMat.diagonal ().array () -= eigenvalues (2);
320  eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector.unitOrthogonal ();
321  }
322  else {
323  // all three equal: just use an arbitrary unit vector
324  eigenvector << Scalar (1.0), Scalar (0.0), Scalar (0.0);
325  }
326 }
327 
328 
329 template <typename Matrix, typename Vector> inline void
330 eigen33 (const Matrix& mat, Vector& evals)
331 {
332  using Scalar = typename Matrix::Scalar;
333  Scalar scale = mat.cwiseAbs ().maxCoeff ();
334  if (scale <= std::numeric_limits < Scalar > ::min ())
335  scale = Scalar (1.0);
336 
337  Matrix scaledMat = mat / scale;
338  computeRoots (scaledMat, evals);
339  evals *= scale;
340 }
341 
342 
343 template <typename Matrix, typename Vector> inline void
344 eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
345 {
346  using Scalar = typename Matrix::Scalar;
347  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
348  // only when at least one matrix entry has magnitude larger than 1.
349 
350  Scalar scale = mat.cwiseAbs ().maxCoeff ();
351  if (scale <= std::numeric_limits < Scalar > ::min ())
352  scale = Scalar (1.0);
353 
354  Matrix scaledMat = mat / scale;
355 
356  // Compute the eigenvalues
357  computeRoots (scaledMat, evals);
358 
359  if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
360  {
361  // all three equal
362  evecs.setIdentity ();
363  }
364  else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
365  {
366  // first and second equal
367  Matrix tmp;
368  tmp = scaledMat;
369  tmp.diagonal ().array () -= evals (2);
370 
371  evecs.col (2) = detail::getLargest3x3Eigenvector<Vector> (tmp).vector;
372  evecs.col (1) = evecs.col (2).unitOrthogonal ();
373  evecs.col (0) = evecs.col (1).cross (evecs.col (2));
374  }
375  else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ())
376  {
377  // second and third equal
378  Matrix tmp;
379  tmp = scaledMat;
380  tmp.diagonal ().array () -= evals (0);
381 
382  evecs.col (0) = detail::getLargest3x3Eigenvector<Vector> (tmp).vector;
383  evecs.col (1) = evecs.col (0).unitOrthogonal ();
384  evecs.col (2) = evecs.col (0).cross (evecs.col (1));
385  }
386  else
387  {
388  std::array<Scalar, 3> eigenVecLen;
389 
390  for (int i = 0; i < 3; ++i)
391  {
392  Matrix tmp = scaledMat;
393  tmp.diagonal ().array () -= evals (i);
394  const auto vec_len = detail::getLargest3x3Eigenvector<Vector> (tmp);
395  evecs.col (i) = vec_len.vector;
396  eigenVecLen[i] = vec_len.length;
397  }
398 
399  // @TODO: might be redundant or over-complicated as per @SergioRAgostinho
400  // see: https://github.com/PointCloudLibrary/pcl/pull/3441#discussion_r341024181
401  const auto minmax_it = std::minmax_element (eigenVecLen.cbegin (), eigenVecLen.cend ());
402  int min_idx = std::distance (eigenVecLen.cbegin (), minmax_it.first);
403  int max_idx = std::distance (eigenVecLen.cbegin (), minmax_it.second);
404  int mid_idx = 3 - min_idx - max_idx;
405 
406  evecs.col (min_idx) = evecs.col ( (min_idx + 1) % 3).cross (evecs.col ( (min_idx + 2) % 3)).normalized ();
407  evecs.col (mid_idx) = evecs.col ( (mid_idx + 1) % 3).cross (evecs.col ( (mid_idx + 2) % 3)).normalized ();
408  }
409  // Rescale back to the original size.
410  evals *= scale;
411 }
412 
413 
414 template <typename Matrix> inline typename Matrix::Scalar
415 invert2x2 (const Matrix& matrix, Matrix& inverse)
416 {
417  using Scalar = typename Matrix::Scalar;
418  Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2);
419 
420  if (det != 0)
421  {
422  //Scalar inv_det = Scalar (1.0) / det;
423  inverse.coeffRef (0) = matrix.coeff (3);
424  inverse.coeffRef (1) = -matrix.coeff (1);
425  inverse.coeffRef (2) = -matrix.coeff (2);
426  inverse.coeffRef (3) = matrix.coeff (0);
427  inverse /= det;
428  }
429  return det;
430 }
431 
432 
433 template <typename Matrix> inline typename Matrix::Scalar
434 invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
435 {
436  using Scalar = typename Matrix::Scalar;
437  // elements
438  // a b c
439  // b d e
440  // c e f
441  //| a b c |-1 | fd-ee ce-bf be-cd |
442  //| b d e | = 1/det * | ce-bf af-cc bc-ae |
443  //| c e f | | be-cd bc-ae ad-bb |
444 
445  //det = a(fd-ee) + b(ec-fb) + c(eb-dc)
446 
447  Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
448  Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
449  Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
450 
451  Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
452 
453  if (det != 0)
454  {
455  //Scalar inv_det = Scalar (1.0) / det;
456  inverse.coeffRef (0) = fd_ee;
457  inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
458  inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
459  inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
460  inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
461  inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
462  inverse /= det;
463  }
464  return det;
465 }
466 
467 
468 template <typename Matrix> inline typename Matrix::Scalar
469 invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
470 {
471  using Scalar = typename Matrix::Scalar;
472 
473  //| a b c |-1 | ie-hf hc-ib fb-ec |
474  //| d e f | = 1/det * | gf-id ia-gc dc-fa |
475  //| g h i | | hd-ge gb-ha ea-db |
476  //det = a(ie-hf) + d(hc-ib) + g(fb-ec)
477 
478  Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
479  Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
480  Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
481  Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec);
482 
483  if (det != 0)
484  {
485  inverse.coeffRef (0) = ie_hf;
486  inverse.coeffRef (1) = hc_ib;
487  inverse.coeffRef (2) = fb_ec;
488  inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
489  inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
490  inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
491  inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
492  inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
493  inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
494 
495  inverse /= det;
496  }
497  return det;
498 }
499 
500 
501 template <typename Matrix> inline typename Matrix::Scalar
502 determinant3x3Matrix (const Matrix& matrix)
503 {
504  // result is independent of Row/Col Major storage!
505  return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
506  matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
507  matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
508 }
509 
510 
511 void
512 getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
513  const Eigen::Vector3f& y_direction,
514  Eigen::Affine3f& transformation)
515 {
516  Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized();
517  Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized();
518  Eigen::Vector3f tmp2 = z_axis.normalized();
519 
520  transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
521  transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
522  transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
523  transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
524 }
525 
526 
527 Eigen::Affine3f
528 getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
529  const Eigen::Vector3f& y_direction)
530 {
531  Eigen::Affine3f transformation;
532  getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
533  return (transformation);
534 }
535 
536 
537 void
538 getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
539  const Eigen::Vector3f& y_direction,
540  Eigen::Affine3f& transformation)
541 {
542  Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized();
543  Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized();
544  Eigen::Vector3f tmp0 = x_axis.normalized();
545 
546  transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
547  transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
548  transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
549  transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
550 }
551 
552 
553 Eigen::Affine3f
554 getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
555  const Eigen::Vector3f& y_direction)
556 {
557  Eigen::Affine3f transformation;
558  getTransFromUnitVectorsXY (x_axis, y_direction, transformation);
559  return (transformation);
560 }
561 
562 
563 void
564 getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
565  const Eigen::Vector3f& z_axis,
566  Eigen::Affine3f& transformation)
567 {
568  getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
569 }
570 
571 
572 Eigen::Affine3f
573 getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
574  const Eigen::Vector3f& z_axis)
575 {
576  Eigen::Affine3f transformation;
577  getTransformationFromTwoUnitVectors (y_direction, z_axis, transformation);
578  return (transformation);
579 }
580 
581 
582 void
583 getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
584  const Eigen::Vector3f& z_axis,
585  const Eigen::Vector3f& origin,
586  Eigen::Affine3f& transformation)
587 {
588  getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation);
589  Eigen::Vector3f translation = transformation*origin;
590  transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2];
591 }
592 
593 
594 template <typename Scalar> void
595 getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
596 {
597  roll = std::atan2 (t (2, 1), t (2, 2));
598  pitch = asin (-t (2, 0));
599  yaw = std::atan2 (t (1, 0), t (0, 0));
600 }
601 
602 
603 template <typename Scalar> void
604 getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
605  Scalar &x, Scalar &y, Scalar &z,
606  Scalar &roll, Scalar &pitch, Scalar &yaw)
607 {
608  x = t (0, 3);
609  y = t (1, 3);
610  z = t (2, 3);
611  roll = std::atan2 (t (2, 1), t (2, 2));
612  pitch = asin (-t (2, 0));
613  yaw = std::atan2 (t (1, 0), t (0, 0));
614 }
615 
616 
617 template <typename Scalar> void
618 getTransformation (Scalar x, Scalar y, Scalar z,
619  Scalar roll, Scalar pitch, Scalar yaw,
620  Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
621 {
622  Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch),
623  E = std::cos (roll), F = sin (roll), DE = D*E, DF = D*F;
624 
625  t (0, 0) = A*C; t (0, 1) = A*DF - B*E; t (0, 2) = B*F + A*DE; t (0, 3) = x;
626  t (1, 0) = B*C; t (1, 1) = A*E + B*DF; t (1, 2) = B*DE - A*F; t (1, 3) = y;
627  t (2, 0) = -D; t (2, 1) = C*F; t (2, 2) = C*E; t (2, 3) = z;
628  t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1;
629 }
630 
631 
632 template <typename Derived> void
633 saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file)
634 {
635  std::uint32_t rows = static_cast<std::uint32_t> (matrix.rows ()), cols = static_cast<std::uint32_t> (matrix.cols ());
636  file.write (reinterpret_cast<char*> (&rows), sizeof (rows));
637  file.write (reinterpret_cast<char*> (&cols), sizeof (cols));
638  for (std::uint32_t i = 0; i < rows; ++i)
639  for (std::uint32_t j = 0; j < cols; ++j)
640  {
641  typename Derived::Scalar tmp = matrix(i,j);
642  file.write (reinterpret_cast<const char*> (&tmp), sizeof (tmp));
643  }
644 }
645 
646 
647 template <typename Derived> void
648 loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
649 {
650  Eigen::MatrixBase<Derived> &matrix = const_cast<Eigen::MatrixBase<Derived> &> (matrix_);
651 
652  std::uint32_t rows, cols;
653  file.read (reinterpret_cast<char*> (&rows), sizeof (rows));
654  file.read (reinterpret_cast<char*> (&cols), sizeof (cols));
655  if (matrix.rows () != static_cast<int>(rows) || matrix.cols () != static_cast<int>(cols))
656  matrix.derived().resize(rows, cols);
657 
658  for (std::uint32_t i = 0; i < rows; ++i)
659  for (std::uint32_t j = 0; j < cols; ++j)
660  {
661  typename Derived::Scalar tmp;
662  file.read (reinterpret_cast<char*> (&tmp), sizeof (tmp));
663  matrix (i, j) = tmp;
664  }
665 }
666 
667 
668 template <typename Derived, typename OtherDerived>
669 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
670 umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
671 {
672 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
673  return Eigen::umeyama (src, dst, with_scaling);
674 #else
675  using TransformationMatrixType = typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type;
676  using Scalar = typename Eigen::internal::traits<TransformationMatrixType>::Scalar;
677  using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
678  using Index = typename Derived::Index;
679 
680  static_assert (!Eigen::NumTraits<Scalar>::IsComplex, "Numeric type must be real.");
681  static_assert ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value),
682  "You mixed different numeric types. You need to use the cast method of matrixbase to cast numeric types explicitly.");
683 
684  enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
685 
686  using VectorType = Eigen::Matrix<Scalar, Dimension, 1>;
687  using MatrixType = Eigen::Matrix<Scalar, Dimension, Dimension>;
688  using RowMajorMatrixType = typename Eigen::internal::plain_matrix_type_row_major<Derived>::type;
689 
690  const Index m = src.rows (); // dimension
691  const Index n = src.cols (); // number of measurements
692 
693  // required for demeaning ...
694  const RealScalar one_over_n = 1 / static_cast<RealScalar> (n);
695 
696  // computation of mean
697  const VectorType src_mean = src.rowwise ().sum () * one_over_n;
698  const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
699 
700  // demeaning of src and dst points
701  const RowMajorMatrixType src_demean = src.colwise () - src_mean;
702  const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
703 
704  // Eq. (36)-(37)
705  const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
706 
707  // Eq. (38)
708  const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
709 
710  Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
711 
712  // Initialize the resulting transformation with an identity matrix...
713  TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
714 
715  // Eq. (39)
716  VectorType S = VectorType::Ones (m);
717 
718  if ( svd.matrixU ().determinant () * svd.matrixV ().determinant () < 0 )
719  S (m - 1) = -1;
720 
721  // Eq. (40) and (43)
722  Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
723 
724  if (with_scaling)
725  {
726  // Eq. (42)
727  const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S);
728 
729  // Eq. (41)
730  Rt.col (m).head (m) = dst_mean;
731  Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean;
732  Rt.block (0, 0, m, m) *= c;
733  }
734  else
735  {
736  Rt.col (m).head (m) = dst_mean;
737  Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
738  }
739 
740  return (Rt);
741 #endif
742 }
743 
744 
745 template <typename Scalar> bool
746 transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
747  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
748  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
749 {
750  if (line_in.innerSize () != 6 || line_out.innerSize () != 6)
751  {
752  PCL_DEBUG ("transformLine: lines size != 6\n");
753  return (false);
754  }
755 
756  Eigen::Matrix<Scalar, 3, 1> point, vector;
757  point << line_in.template head<3> ();
758  vector << line_out.template tail<3> ();
759 
760  pcl::transformPoint (point, point, transformation);
761  pcl::transformVector (vector, vector, transformation);
762  line_out << point, vector;
763  return (true);
764 }
765 
766 
767 template <typename Scalar> void
768 transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
769  Eigen::Matrix<Scalar, 4, 1> &plane_out,
770  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
771 {
772  Eigen::Hyperplane < Scalar, 3 > plane;
773  plane.coeffs () << plane_in;
774  plane.transform (transformation);
775  plane_out << plane.coeffs ();
776 
777  // Versions prior to 3.3.2 don't normalize the result
778  #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
779  plane_out /= plane_out.template head<3> ().norm ();
780  #endif
781 }
782 
783 
784 template <typename Scalar> void
786  pcl::ModelCoefficients::Ptr plane_out,
787  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
788 {
789  std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
790  Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
791  pcl::transformPlane (v_plane_in, v_plane_in, transformation);
792  plane_out->values.resize (4);
793  std::copy_n(v_plane_in.data (), 4, plane_out->values.begin ());
794 }
795 
796 
797 template <typename Scalar> bool
798 checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
799  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
800  const Scalar norm_limit,
801  const Scalar dot_limit)
802 {
803  if (line_x.innerSize () != 6 || line_y.innerSize () != 6)
804  {
805  PCL_DEBUG ("checkCoordinateSystem: lines size != 6\n");
806  return (false);
807  }
808 
809  if (line_x.template head<3> () != line_y.template head<3> ())
810  {
811  PCL_DEBUG ("checkCoorZdinateSystem: vector origins are different !\n");
812  return (false);
813  }
814 
815  // Make a copy of vector directions
816  // X^Y = Z | Y^Z = X | Z^X = Y
817  Eigen::Matrix<Scalar, 3, 1> v_line_x (line_x.template tail<3> ()),
818  v_line_y (line_y.template tail<3> ()),
819  v_line_z (v_line_x.cross (v_line_y));
820 
821  // Check vectors norms
822  if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit)
823  {
824  PCL_DEBUG ("checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ());
825  return (false);
826  }
827 
828  if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit)
829  {
830  PCL_DEBUG ("checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ());
831  return (false);
832  }
833 
834  if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit)
835  {
836  PCL_DEBUG ("checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ());
837  return (false);
838  }
839 
840  // Check vectors perendicularity
841  if (std::abs (v_line_x.dot (v_line_y)) > dot_limit)
842  {
843  PCL_DEBUG ("checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit);
844  return (false);
845  }
846 
847  if (std::abs (v_line_x.dot (v_line_z)) > dot_limit)
848  {
849  PCL_DEBUG ("checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit);
850  return (false);
851  }
852 
853  if (std::abs (v_line_y.dot (v_line_z)) > dot_limit)
854  {
855  PCL_DEBUG ("checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit);
856  return (false);
857  }
858 
859  return (true);
860 }
861 
862 
863 template <typename Scalar> bool
864 transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
865  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
866  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
867  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
868  Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
869 {
870  if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
871  {
872  PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n");
873  return (false);
874  }
875 
876  // Check if coordinate systems are valid
877  if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y))
878  {
879  PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n");
880  return (false);
881  }
882 
883  // Convert lines into Vector3 :
884  Eigen::Matrix<Scalar, 3, 1> fr0 (from_line_x.template head<3>()),
885  fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
886  fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()),
887 
888  to0 (to_line_x.template head<3>()),
889  to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()),
890  to2 (to_line_y.template head<3>() + to_line_y.template tail<3>());
891 
892  // Code is inspired from http://stackoverflow.com/a/15277421/1816078
893  // Define matrices and points :
894  Eigen::Transform<Scalar, 3, Eigen::Affine> T2, T3 = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
895  Eigen::Matrix<Scalar, 3, 1> x1, y1, z1, x2, y2, z2;
896 
897  // Axes of the coordinate system "fr"
898  x1 = (fr1 - fr0).normalized (); // the versor (unitary vector) of the (fr1-fr0) axis vector
899  y1 = (fr2 - fr0).normalized ();
900 
901  // Axes of the coordinate system "to"
902  x2 = (to1 - to0).normalized ();
903  y2 = (to2 - to0).normalized ();
904 
905  // Transform from CS1 to CS2
906  // Note: if fr0 == (0,0,0) --> CS1 == CS2 --> T2 = Identity
907  T2.linear () << x1, y1, x1.cross (y1);
908 
909  // Transform from CS1 to CS3
910  T3.linear () << x2, y2, x2.cross (y2);
911 
912  // Identity matrix = transform to CS2 to CS3
913  // Note: if CS1 == CS2 --> transformation = T3
914  transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
915  transformation.linear () = T3.linear () * T2.linear ().inverse ();
916  transformation.translation () = to0 - (transformation.linear () * fr0);
917  return (true);
918 }
919 
920 } // namespace pcl
921 
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:604
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:583
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Definition: eigen.hpp:502
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition: eigen.hpp:434
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
Definition: eigen.hpp:648
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:618
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:595
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:564
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:538
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
Definition: eigen.hpp:469
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:295
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Definition: eigen.hpp:633
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
Definition: eigen.hpp:415
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:512
@ 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:798
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:768
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:670
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:864
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:746
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