Point Cloud Library (PCL)  1.7.0
eigen.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
7  * Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
8  * Copyright (c) 2012-, Open Perception, Inc.
9  *
10  * All rights reserved.
11  *
12  * Redistribution and use in source and binary forms, with or without
13  * modification, are permitted provided that the following conditions
14  * are met:
15  *
16  * * Redistributions of source code must retain the above copyright
17  * notice, this list of conditions and the following disclaimer.
18  * * Redistributions in binary form must reproduce the above
19  * copyright notice, this list of conditions and the following
20  * disclaimer in the documentation and/or other materials provided
21  * with the distribution.
22  * * Neither the name of the copyright holder(s) nor the names of its
23  * contributors may be used to endorse or promote products derived
24  * from this software without specific prior written permission.
25  *
26  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
30  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
31  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
32  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
33  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
34  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
35  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
36  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
37  * POSSIBILITY OF SUCH DAMAGE.
38  *
39  * $Id$
40  *
41  */
42 #ifndef PCL_COMMON_EIGEN_H_
43 #define PCL_COMMON_EIGEN_H_
44 
45 #ifndef NOMINMAX
46 #define NOMINMAX
47 #endif
48 
49 #if defined __GNUC__
50 # pragma GCC system_header
51 #elif defined __SUNPRO_CC
52 # pragma disable_warn
53 #endif
54 
55 #include <cmath>
56 
57 #include <Eigen/StdVector>
58 #include <Eigen/Core>
59 #include <Eigen/Eigenvalues>
60 #include <Eigen/Geometry>
61 #include <Eigen/SVD>
62 #include <Eigen/LU>
63 #include <Eigen/Dense>
64 #include <Eigen/Eigenvalues>
65 
66 namespace pcl
67 {
68  /** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0
69  * \param[in] b linear parameter
70  * \param[in] c constant parameter
71  * \param[out] roots solutions of x^2 + b*x + c = 0
72  */
73  template<typename Scalar, typename Roots> inline void
74  computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
75  {
76  roots (0) = Scalar (0);
77  Scalar d = Scalar (b * b - 4.0 * c);
78  if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN!
79  d = 0.0;
80 
81  Scalar sd = ::std::sqrt (d);
82 
83  roots (2) = 0.5f * (b + sd);
84  roots (1) = 0.5f * (b - sd);
85  }
86 
87  /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
88  * \param[in] m input matrix
89  * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
90  */
91  template<typename Matrix, typename Roots> inline void
92  computeRoots (const Matrix& m, Roots& roots)
93  {
94  typedef typename Matrix::Scalar Scalar;
95 
96  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
97  // eigenvalues are the roots to this equation, all guaranteed to be
98  // real-valued, because the matrix is symmetric.
99  Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2)
100  + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2)
101  - m (0, 0) * m (1, 2) * m (1, 2)
102  - m (1, 1) * m (0, 2) * m (0, 2)
103  - m (2, 2) * m (0, 1) * m (0, 1);
104  Scalar c1 = m (0, 0) * m (1, 1) -
105  m (0, 1) * m (0, 1) +
106  m (0, 0) * m (2, 2) -
107  m (0, 2) * m (0, 2) +
108  m (1, 1) * m (2, 2) -
109  m (1, 2) * m (1, 2);
110  Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
111 
112 
113  if (fabs (c0) < Eigen::NumTraits<Scalar>::epsilon ())// one root is 0 -> quadratic equation
114  computeRoots2 (c2, c1, roots);
115  else
116  {
117  const Scalar s_inv3 = Scalar (1.0 / 3.0);
118  const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
119  // Construct the parameters used in classifying the roots of the equation
120  // and in solving the equation for the roots in closed form.
121  Scalar c2_over_3 = c2*s_inv3;
122  Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
123  if (a_over_3 > Scalar (0))
124  a_over_3 = Scalar (0);
125 
126  Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
127 
128  Scalar q = half_b * half_b + a_over_3 * a_over_3*a_over_3;
129  if (q > Scalar (0))
130  q = Scalar (0);
131 
132  // Compute the eigenvalues by solving for the roots of the polynomial.
133  Scalar rho = std::sqrt (-a_over_3);
134  Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3;
135  Scalar cos_theta = std::cos (theta);
136  Scalar sin_theta = std::sin (theta);
137  roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta;
138  roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
139  roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
140 
141  // Sort in increasing order.
142  if (roots (0) >= roots (1))
143  std::swap (roots (0), roots (1));
144  if (roots (1) >= roots (2))
145  {
146  std::swap (roots (1), roots (2));
147  if (roots (0) >= roots (1))
148  std::swap (roots (0), roots (1));
149  }
150 
151  if (roots (0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
152  computeRoots2 (c2, c1, roots);
153  }
154  }
155 
156  /** \brief determine the smallest eigenvalue and its corresponding eigenvector
157  * \param[in] mat input matrix that needs to be symmetric and positive semi definite
158  * \param[out] eigenvalue the smallest eigenvalue of the input matrix
159  * \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix
160  * \ingroup common
161  */
162  template <typename Matrix, typename Vector> inline void
163  eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
164  {
165  // if diagonal matrix, the eigenvalues are the diagonal elements
166  // and the eigenvectors are not unique, thus set to Identity
167  if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
168  {
169  if (mat.coeff (0) < mat.coeff (2))
170  {
171  eigenvalue = mat.coeff (0);
172  eigenvector [0] = 1.0;
173  eigenvector [1] = 0.0;
174  }
175  else
176  {
177  eigenvalue = mat.coeff (2);
178  eigenvector [0] = 0.0;
179  eigenvector [1] = 1.0;
180  }
181  return;
182  }
183 
184  // 0.5 to optimize further calculations
185  typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
186  typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
187 
188  typename Matrix::Scalar temp = trace * trace - determinant;
189 
190  if (temp < 0)
191  temp = 0;
192 
193  eigenvalue = trace - ::std::sqrt (temp);
194 
195  eigenvector [0] = - mat.coeff (1);
196  eigenvector [1] = mat.coeff (0) - eigenvalue;
197  eigenvector.normalize ();
198  }
199 
200  /** \brief determine the smallest eigenvalue and its corresponding eigenvector
201  * \param[in] mat input matrix that needs to be symmetric and positive semi definite
202  * \param[out] eigenvectors the corresponding eigenvector to the smallest eigenvalue of the input matrix
203  * \param[out] eigenvalues the smallest eigenvalue of the input matrix
204  * \ingroup common
205  */
206  template <typename Matrix, typename Vector> inline void
207  eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
208  {
209  // if diagonal matrix, the eigenvalues are the diagonal elements
210  // and the eigenvectors are not unique, thus set to Identity
211  if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
212  {
213  if (mat.coeff (0) < mat.coeff (3))
214  {
215  eigenvalues.coeffRef (0) = mat.coeff (0);
216  eigenvalues.coeffRef (1) = mat.coeff (3);
217  eigenvectors.coeffRef (0) = 1.0;
218  eigenvectors.coeffRef (1) = 0.0;
219  eigenvectors.coeffRef (2) = 0.0;
220  eigenvectors.coeffRef (3) = 1.0;
221  }
222  else
223  {
224  eigenvalues.coeffRef (0) = mat.coeff (3);
225  eigenvalues.coeffRef (1) = mat.coeff (0);
226  eigenvectors.coeffRef (0) = 0.0;
227  eigenvectors.coeffRef (1) = 1.0;
228  eigenvectors.coeffRef (2) = 1.0;
229  eigenvectors.coeffRef (3) = 0.0;
230  }
231  return;
232  }
233 
234  // 0.5 to optimize further calculations
235  typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
236  typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
237 
238  typename Matrix::Scalar temp = trace * trace - determinant;
239 
240  if (temp < 0)
241  temp = 0;
242  else
243  temp = ::std::sqrt (temp);
244 
245  eigenvalues.coeffRef (0) = trace - temp;
246  eigenvalues.coeffRef (1) = trace + temp;
247 
248  // either this is in a row or column depending on RowMajor or ColumnMajor
249  eigenvectors.coeffRef (0) = - mat.coeff (1);
250  eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0);
251  typename Matrix::Scalar norm = static_cast<typename Matrix::Scalar> (1.0) /
252  static_cast<typename Matrix::Scalar> (::std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2)));
253  eigenvectors.coeffRef (0) *= norm;
254  eigenvectors.coeffRef (2) *= norm;
255  eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2);
256  eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
257  }
258 
259  /** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix
260  * \param[in] mat symmetric positive semi definite input matrix
261  * \param[in] eigenvalue the eigenvalue which corresponding eigenvector is to be computed
262  * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
263  * \ingroup common
264  */
265  template<typename Matrix, typename Vector> inline void
266  computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
267  {
268  typedef typename Matrix::Scalar Scalar;
269  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
270  // only when at least one matrix entry has magnitude larger than 1.
271 
272  Scalar scale = mat.cwiseAbs ().maxCoeff ();
273  if (scale <= std::numeric_limits<Scalar>::min ())
274  scale = Scalar (1.0);
275 
276  Matrix scaledMat = mat / scale;
277 
278  scaledMat.diagonal ().array () -= eigenvalue / scale;
279 
280  Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
281  Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
282  Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
283 
284  Scalar len1 = vec1.squaredNorm ();
285  Scalar len2 = vec2.squaredNorm ();
286  Scalar len3 = vec3.squaredNorm ();
287 
288  if (len1 >= len2 && len1 >= len3)
289  eigenvector = vec1 / std::sqrt (len1);
290  else if (len2 >= len1 && len2 >= len3)
291  eigenvector = vec2 / std::sqrt (len2);
292  else
293  eigenvector = vec3 / std::sqrt (len3);
294  }
295 
296  /** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix
297  * \param[in] mat symmetric positive semi definite input matrix
298  * \param[out] eigenvalue smallest eigenvalue of the input matrix
299  * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
300  * \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue.
301  * \ingroup common
302  */
303  template<typename Matrix, typename Vector> inline void
304  eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
305  {
306  typedef typename Matrix::Scalar Scalar;
307  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
308  // only when at least one matrix entry has magnitude larger than 1.
309 
310  Scalar scale = mat.cwiseAbs ().maxCoeff ();
311  if (scale <= std::numeric_limits<Scalar>::min ())
312  scale = Scalar (1.0);
313 
314  Matrix scaledMat = mat / scale;
315 
316  Vector eigenvalues;
317  computeRoots (scaledMat, eigenvalues);
318 
319  eigenvalue = eigenvalues (0) * scale;
320 
321  scaledMat.diagonal ().array () -= eigenvalues (0);
322 
323  Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
324  Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
325  Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
326 
327  Scalar len1 = vec1.squaredNorm ();
328  Scalar len2 = vec2.squaredNorm ();
329  Scalar len3 = vec3.squaredNorm ();
330 
331  if (len1 >= len2 && len1 >= len3)
332  eigenvector = vec1 / std::sqrt (len1);
333  else if (len2 >= len1 && len2 >= len3)
334  eigenvector = vec2 / std::sqrt (len2);
335  else
336  eigenvector = vec3 / std::sqrt (len3);
337  }
338 
339  /** \brief determines the eigenvalues of the symmetric positive semi definite input matrix
340  * \param[in] mat symmetric positive semi definite input matrix
341  * \param[out] evals resulting eigenvalues in ascending order
342  * \ingroup common
343  */
344  template<typename Matrix, typename Vector> inline void
345  eigen33 (const Matrix& mat, Vector& evals)
346  {
347  typedef typename Matrix::Scalar Scalar;
348  Scalar scale = mat.cwiseAbs ().maxCoeff ();
349  if (scale <= std::numeric_limits<Scalar>::min ())
350  scale = Scalar (1.0);
351 
352  Matrix scaledMat = mat / scale;
353  computeRoots (scaledMat, evals);
354  evals *= scale;
355  }
356 
357  /** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix
358  * \param[in] mat symmetric positive semi definite input matrix
359  * \param[out] evecs resulting eigenvalues in ascending order
360  * \param[out] evals corresponding eigenvectors in correct order according to eigenvalues
361  * \ingroup common
362  */
363  template<typename Matrix, typename Vector> inline void
364  eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
365  {
366  typedef typename Matrix::Scalar Scalar;
367  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
368  // only when at least one matrix entry has magnitude larger than 1.
369 
370  Scalar scale = mat.cwiseAbs ().maxCoeff ();
371  if (scale <= std::numeric_limits<Scalar>::min ())
372  scale = Scalar (1.0);
373 
374  Matrix scaledMat = mat / scale;
375 
376  // Compute the eigenvalues
377  computeRoots (scaledMat, evals);
378 
379  if ((evals (2) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon ())
380  {
381  // all three equal
382  evecs.setIdentity ();
383  }
384  else if ((evals (1) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon () )
385  {
386  // first and second equal
387  Matrix tmp;
388  tmp = scaledMat;
389  tmp.diagonal ().array () -= evals (2);
390 
391  Vector vec1 = tmp.row (0).cross (tmp.row (1));
392  Vector vec2 = tmp.row (0).cross (tmp.row (2));
393  Vector vec3 = tmp.row (1).cross (tmp.row (2));
394 
395  Scalar len1 = vec1.squaredNorm ();
396  Scalar len2 = vec2.squaredNorm ();
397  Scalar len3 = vec3.squaredNorm ();
398 
399  if (len1 >= len2 && len1 >= len3)
400  evecs.col (2) = vec1 / std::sqrt (len1);
401  else if (len2 >= len1 && len2 >= len3)
402  evecs.col (2) = vec2 / std::sqrt (len2);
403  else
404  evecs.col (2) = vec3 / std::sqrt (len3);
405 
406  evecs.col (1) = evecs.col (2).unitOrthogonal ();
407  evecs.col (0) = evecs.col (1).cross (evecs.col (2));
408  }
409  else if ((evals (2) - evals (1)) <= Eigen::NumTraits<Scalar>::epsilon () )
410  {
411  // second and third equal
412  Matrix tmp;
413  tmp = scaledMat;
414  tmp.diagonal ().array () -= evals (0);
415 
416  Vector vec1 = tmp.row (0).cross (tmp.row (1));
417  Vector vec2 = tmp.row (0).cross (tmp.row (2));
418  Vector vec3 = tmp.row (1).cross (tmp.row (2));
419 
420  Scalar len1 = vec1.squaredNorm ();
421  Scalar len2 = vec2.squaredNorm ();
422  Scalar len3 = vec3.squaredNorm ();
423 
424  if (len1 >= len2 && len1 >= len3)
425  evecs.col (0) = vec1 / std::sqrt (len1);
426  else if (len2 >= len1 && len2 >= len3)
427  evecs.col (0) = vec2 / std::sqrt (len2);
428  else
429  evecs.col (0) = vec3 / std::sqrt (len3);
430 
431  evecs.col (1) = evecs.col (0).unitOrthogonal ();
432  evecs.col (2) = evecs.col (0).cross (evecs.col (1));
433  }
434  else
435  {
436  Matrix tmp;
437  tmp = scaledMat;
438  tmp.diagonal ().array () -= evals (2);
439 
440  Vector vec1 = tmp.row (0).cross (tmp.row (1));
441  Vector vec2 = tmp.row (0).cross (tmp.row (2));
442  Vector vec3 = tmp.row (1).cross (tmp.row (2));
443 
444  Scalar len1 = vec1.squaredNorm ();
445  Scalar len2 = vec2.squaredNorm ();
446  Scalar len3 = vec3.squaredNorm ();
447 #ifdef _WIN32
448  Scalar *mmax = new Scalar[3];
449 #else
450  Scalar mmax[3];
451 #endif
452  unsigned int min_el = 2;
453  unsigned int max_el = 2;
454  if (len1 >= len2 && len1 >= len3)
455  {
456  mmax[2] = len1;
457  evecs.col (2) = vec1 / std::sqrt (len1);
458  }
459  else if (len2 >= len1 && len2 >= len3)
460  {
461  mmax[2] = len2;
462  evecs.col (2) = vec2 / std::sqrt (len2);
463  }
464  else
465  {
466  mmax[2] = len3;
467  evecs.col (2) = vec3 / std::sqrt (len3);
468  }
469 
470  tmp = scaledMat;
471  tmp.diagonal ().array () -= evals (1);
472 
473  vec1 = tmp.row (0).cross (tmp.row (1));
474  vec2 = tmp.row (0).cross (tmp.row (2));
475  vec3 = tmp.row (1).cross (tmp.row (2));
476 
477  len1 = vec1.squaredNorm ();
478  len2 = vec2.squaredNorm ();
479  len3 = vec3.squaredNorm ();
480  if (len1 >= len2 && len1 >= len3)
481  {
482  mmax[1] = len1;
483  evecs.col (1) = vec1 / std::sqrt (len1);
484  min_el = len1 <= mmax[min_el] ? 1 : min_el;
485  max_el = len1 > mmax[max_el] ? 1 : max_el;
486  }
487  else if (len2 >= len1 && len2 >= len3)
488  {
489  mmax[1] = len2;
490  evecs.col (1) = vec2 / std::sqrt (len2);
491  min_el = len2 <= mmax[min_el] ? 1 : min_el;
492  max_el = len2 > mmax[max_el] ? 1 : max_el;
493  }
494  else
495  {
496  mmax[1] = len3;
497  evecs.col (1) = vec3 / std::sqrt (len3);
498  min_el = len3 <= mmax[min_el] ? 1 : min_el;
499  max_el = len3 > mmax[max_el] ? 1 : max_el;
500  }
501 
502  tmp = scaledMat;
503  tmp.diagonal ().array () -= evals (0);
504 
505  vec1 = tmp.row (0).cross (tmp.row (1));
506  vec2 = tmp.row (0).cross (tmp.row (2));
507  vec3 = tmp.row (1).cross (tmp.row (2));
508 
509  len1 = vec1.squaredNorm ();
510  len2 = vec2.squaredNorm ();
511  len3 = vec3.squaredNorm ();
512  if (len1 >= len2 && len1 >= len3)
513  {
514  mmax[0] = len1;
515  evecs.col (0) = vec1 / std::sqrt (len1);
516  min_el = len3 <= mmax[min_el] ? 0 : min_el;
517  max_el = len3 > mmax[max_el] ? 0 : max_el;
518  }
519  else if (len2 >= len1 && len2 >= len3)
520  {
521  mmax[0] = len2;
522  evecs.col (0) = vec2 / std::sqrt (len2);
523  min_el = len3 <= mmax[min_el] ? 0 : min_el;
524  max_el = len3 > mmax[max_el] ? 0 : max_el;
525  }
526  else
527  {
528  mmax[0] = len3;
529  evecs.col (0) = vec3 / std::sqrt (len3);
530  min_el = len3 <= mmax[min_el] ? 0 : min_el;
531  max_el = len3 > mmax[max_el] ? 0 : max_el;
532  }
533 
534  unsigned mid_el = 3 - min_el - max_el;
535  evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized ();
536  evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized ();
537 #ifdef _WIN32
538  delete [] mmax;
539 #endif
540  }
541  // Rescale back to the original size.
542  evals *= scale;
543  }
544 
545  /** \brief Calculate the inverse of a 2x2 matrix
546  * \param[in] matrix matrix to be inverted
547  * \param[out] inverse the resultant inverted matrix
548  * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
549  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
550  * \ingroup common
551  */
552  template<typename Matrix> inline typename Matrix::Scalar
553  invert2x2 (const Matrix& matrix, Matrix& inverse)
554  {
555  typedef typename Matrix::Scalar Scalar;
556  Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2) ;
557 
558  if (det != 0)
559  {
560  //Scalar inv_det = Scalar (1.0) / det;
561  inverse.coeffRef (0) = matrix.coeff (3);
562  inverse.coeffRef (1) = - matrix.coeff (1);
563  inverse.coeffRef (2) = - matrix.coeff (2);
564  inverse.coeffRef (3) = matrix.coeff (0);
565  inverse /= det;
566  }
567  return det;
568  }
569 
570  /** \brief Calculate the inverse of a 3x3 symmetric matrix.
571  * \param[in] matrix matrix to be inverted
572  * \param[out] inverse the resultant inverted matrix
573  * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
574  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
575  * \ingroup common
576  */
577  template<typename Matrix> inline typename Matrix::Scalar
578  invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
579  {
580  typedef typename Matrix::Scalar Scalar;
581  // elements
582  // a b c
583  // b d e
584  // c e f
585  //| a b c |-1 | fd-ee ce-bf be-cd |
586  //| b d e | = 1/det * | ce-bf af-cc bc-ae |
587  //| c e f | | be-cd bc-ae ad-bb |
588 
589  //det = a(fd-ee) + b(ec-fb) + c(eb-dc)
590 
591  Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
592  Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
593  Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
594 
595  Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
596 
597  if (det != 0)
598  {
599  //Scalar inv_det = Scalar (1.0) / det;
600  inverse.coeffRef (0) = fd_ee;
601  inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
602  inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
603  inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
604  inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
605  inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
606  inverse /= det;
607  }
608  return det;
609  }
610 
611  /** \brief Calculate the inverse of a general 3x3 matrix.
612  * \param[in] matrix matrix to be inverted
613  * \param[out] inverse the resultant inverted matrix
614  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
615  * \ingroup common
616  */
617  template<typename Matrix> inline typename Matrix::Scalar
618  invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
619  {
620  typedef typename Matrix::Scalar Scalar;
621 
622  //| a b c |-1 | ie-hf hc-ib fb-ec |
623  //| d e f | = 1/det * | gf-id ia-gc dc-fa |
624  //| g h i | | hd-ge gb-ha ea-db |
625  //det = a(ie-hf) + d(hc-ib) + g(fb-ec)
626 
627  Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
628  Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
629  Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
630  Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec) ;
631 
632  if (det != 0)
633  {
634  inverse.coeffRef (0) = ie_hf;
635  inverse.coeffRef (1) = hc_ib;
636  inverse.coeffRef (2) = fb_ec;
637  inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
638  inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
639  inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
640  inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
641  inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
642  inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
643 
644  inverse /= det;
645  }
646  return det;
647  }
648 
649  template<typename Matrix> inline typename Matrix::Scalar
650  determinant3x3Matrix (const Matrix& matrix)
651  {
652  // result is independent of Row/Col Major storage!
653  return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
654  matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
655  matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
656  }
657 
658  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
659  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
660  * \param[in] z_axis the z-axis
661  * \param[in] y_direction the y direction
662  * \param[out] transformation the resultant 3D rotation
663  * \ingroup common
664  */
665  inline void
666  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
667  const Eigen::Vector3f& y_direction,
668  Eigen::Affine3f& transformation);
669 
670  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
671  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
672  * \param[in] z_axis the z-axis
673  * \param[in] y_direction the y direction
674  * \return the resultant 3D rotation
675  * \ingroup common
676  */
677  inline Eigen::Affine3f
678  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
679  const Eigen::Vector3f& y_direction);
680 
681  /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
682  * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
683  * \param[in] x_axis the x-axis
684  * \param[in] y_direction the y direction
685  * \param[out] transformation the resultant 3D rotation
686  * \ingroup common
687  */
688  inline void
689  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
690  const Eigen::Vector3f& y_direction,
691  Eigen::Affine3f& transformation);
692 
693  /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
694  * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
695  * \param[in] x_axis the x-axis
696  * \param[in] y_direction the y direction
697  * \return the resulting 3D rotation
698  * \ingroup common
699  */
700  inline Eigen::Affine3f
701  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
702  const Eigen::Vector3f& y_direction);
703 
704  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
705  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
706  * \param[in] y_direction the y direction
707  * \param[in] z_axis the z-axis
708  * \param[out] transformation the resultant 3D rotation
709  * \ingroup common
710  */
711  inline void
712  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
713  const Eigen::Vector3f& z_axis,
714  Eigen::Affine3f& transformation);
715 
716  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
717  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
718  * \param[in] y_direction the y direction
719  * \param[in] z_axis the z-axis
720  * \return transformation the resultant 3D rotation
721  * \ingroup common
722  */
723  inline Eigen::Affine3f
724  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
725  const Eigen::Vector3f& z_axis);
726 
727  /** \brief Get the transformation that will translate \a orign to (0,0,0) and rotate \a z_axis into (0,0,1)
728  * and \a y_direction into a vector with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
729  * \param[in] y_direction the y direction
730  * \param[in] z_axis the z-axis
731  * \param[in] origin the origin
732  * \param[in] transformation the resultant transformation matrix
733  * \ingroup common
734  */
735  inline void
736  getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
737  const Eigen::Vector3f& z_axis,
738  const Eigen::Vector3f& origin,
739  Eigen::Affine3f& transformation);
740 
741  /** \brief Extract the Euler angles (XYZ-convention) from the given transformation
742  * \param[in] t the input transformation matrix
743  * \param[in] roll the resulting roll angle
744  * \param[in] pitch the resulting pitch angle
745  * \param[in] yaw the resulting yaw angle
746  * \ingroup common
747  */
748  inline void
749  getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw);
750 
751  /** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation
752  * \param[in] t the input transformation matrix
753  * \param[out] x the resulting x translation
754  * \param[out] y the resulting y translation
755  * \param[out] z the resulting z translation
756  * \param[out] roll the resulting roll angle
757  * \param[out] pitch the resulting pitch angle
758  * \param[out] yaw the resulting yaw angle
759  * \ingroup common
760  */
761  inline void
762  getTranslationAndEulerAngles (const Eigen::Affine3f& t,
763  float& x, float& y, float& z,
764  float& roll, float& pitch, float& yaw);
765 
766  /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
767  * \param[in] x the input x translation
768  * \param[in] y the input y translation
769  * \param[in] z the input z translation
770  * \param[in] roll the input roll angle
771  * \param[in] pitch the input pitch angle
772  * \param[in] yaw the input yaw angle
773  * \param[out] t the resulting transformation matrix
774  * \ingroup common
775  */
776  template <typename Scalar> inline void
777  getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw,
778  Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
779 
780  inline void
781  getTransformation (float x, float y, float z, float roll, float pitch, float yaw,
782  Eigen::Affine3f &t)
783  {
784  return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
785  }
786 
787  inline void
788  getTransformation (double x, double y, double z, double roll, double pitch, double yaw,
789  Eigen::Affine3d &t)
790  {
791  return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
792  }
793 
794  /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
795  * \param[in] x the input x translation
796  * \param[in] y the input y translation
797  * \param[in] z the input z translation
798  * \param[in] roll the input roll angle
799  * \param[in] pitch the input pitch angle
800  * \param[in] yaw the input yaw angle
801  * \return the resulting transformation matrix
802  * \ingroup common
803  */
804  inline Eigen::Affine3f
805  getTransformation (float x, float y, float z, float roll, float pitch, float yaw);
806 
807  /** \brief Write a matrix to an output stream
808  * \param[in] matrix the matrix to output
809  * \param[out] file the output stream
810  * \ingroup common
811  */
812  template <typename Derived> void
813  saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
814 
815  /** \brief Read a matrix from an input stream
816  * \param[out] matrix the resulting matrix, read from the input stream
817  * \param[in,out] file the input stream
818  * \ingroup common
819  */
820  template <typename Derived> void
821  loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);
822 
823 // PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
824 // followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
825 // finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
826 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
827  : (int (a) == 1 || int (b) == 1) ? 1 \
828  : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
829  : (int (a) <= int (b)) ? int (a) : int (b))
830 
831  /** \brief Returns the transformation between two point sets.
832  * The algorithm is based on:
833  * "Least-squares estimation of transformation parameters between two point patterns",
834  * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
835  *
836  * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
837  * \f{align*}
838  * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
839  * \f}
840  * is minimized.
841  *
842  * The algorithm is based on the analysis of the covariance matrix
843  * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
844  * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where
845  * \f$d\f$ is corresponding to the dimension (which is typically small).
846  * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
847  * though the actual computational effort lies in the covariance
848  * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when
849  * the input point sets have dimension \f$d \times m\f$.
850  *
851  * \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$
852  * \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$.
853  * \param[in] with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. (default: false)
854  * \return The homogeneous transformation
855  * \f{align*}
856  * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
857  * \f}
858  * minimizing the resudiual above. This transformation is always returned as an
859  * Eigen::Matrix.
860  */
861  template <typename Derived, typename OtherDerived>
862  typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
863  umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling = false);
864 }
865 
866 #include <pcl/common/impl/eigen.hpp>
867 
868 #if defined __SUNPRO_CC
869 # pragma enable_warn
870 #endif
871 
872 #endif //PCL_COMMON_EIGEN_H_