Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
eigen.h
Go to the documentation of this file.
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  *
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 Willow Garage, Inc. 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  * $Id: eigen.h 6126 2012-07-03 20:19:58Z aichim $
38  *
39  */
40 // The computeRoots function included in this is based on materials
41 // covered by the following copyright and license:
42 //
43 // Geometric Tools, LLC
44 // Copyright (c) 1998-2010
45 // Distributed under the Boost Software License, Version 1.0.
46 //
47 // Permission is hereby granted, free of charge, to any person or organization
48 // obtaining a copy of the software and accompanying documentation covered by
49 // this license (the "Software") to use, reproduce, display, distribute,
50 // execute, and transmit the Software, and to prepare derivative works of the
51 // Software, and to permit third-parties to whom the Software is furnished to
52 // do so, all subject to the following:
53 //
54 // The copyright notices in the Software and this entire statement, including
55 // the above license grant, this restriction and the following disclaimer,
56 // must be included in all copies of the Software, in whole or in part, and
57 // all derivative works of the Software, unless such copies or derivative
58 // works are solely in the form of machine-executable object code generated by
59 // a source language processor.
60 //
61 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
62 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
63 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
64 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
65 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
66 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
67 // DEALINGS IN THE SOFTWARE.
68 
69 #ifndef PCL_EIGEN_H_
70 #define PCL_EIGEN_H_
71 
72 #ifndef NOMINMAX
73 #define NOMINMAX
74 #endif
75 
76 #if defined __GNUC__
77 # pragma GCC system_header
78 #elif defined __SUNPRO_CC
79 # pragma disable_warn
80 #elif defined _MSC_VER
81 # pragma warning(push, 1)
82 #endif
83 
84 #include <Eigen/StdVector>
85 #include <Eigen/Core>
86 #include <Eigen/Eigenvalues>
87 #include <Eigen/Geometry>
88 #include <Eigen/SVD>
89 #include <Eigen/LU>
90 #include <Eigen/Dense>
91 #include <Eigen/Eigenvalues>
92 
93 namespace pcl
94 {
95  template <typename Scalar> inline Scalar
96  sqrt (const Scalar &val)
97  {
98  }
99 
100  template<> inline double
101  sqrt<double> (const double &val)
102  {
103  return (::sqrt (val));
104  }
105 
106  template<> inline float
107  sqrt<float> (const float &val)
108  {
109  return (::sqrtf (val));
110  }
111 
112  template<> inline long double
113  sqrt<long double> (const long double &val)
114  {
115  return (::sqrtl (val));
116  }
117 
123  template<typename Scalar, typename Roots> inline void
124  computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
125  {
126  roots (0) = Scalar (0);
127  Scalar d = Scalar (b * b - 4.0 * c);
128  if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN!
129  d = 0.0;
130 
131  Scalar sd = sqrt (d);
132 
133  roots (2) = 0.5f * (b + sd);
134  roots (1) = 0.5f * (b - sd);
135  }
136 
141  template<typename Matrix, typename Roots> inline void
142  computeRoots (const Matrix& m, Roots& roots)
143  {
144  typedef typename Matrix::Scalar Scalar;
145 
146  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
147  // eigenvalues are the roots to this equation, all guaranteed to be
148  // real-valued, because the matrix is symmetric.
149  Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2)
150  + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2)
151  - m (0, 0) * m (1, 2) * m (1, 2)
152  - m (1, 1) * m (0, 2) * m (0, 2)
153  - m (2, 2) * m (0, 1) * m (0, 1);
154  Scalar c1 = m (0, 0) * m (1, 1) -
155  m (0, 1) * m (0, 1) +
156  m (0, 0) * m (2, 2) -
157  m (0, 2) * m (0, 2) +
158  m (1, 1) * m (2, 2) -
159  m (1, 2) * m (1, 2);
160  Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
161 
162 
163  if (fabs (c0) < Eigen::NumTraits<Scalar>::epsilon ())// one root is 0 -> quadratic equation
164  computeRoots2 (c2, c1, roots);
165  else
166  {
167  const Scalar s_inv3 = Scalar (1.0 / 3.0);
168  const Scalar s_sqrt3 = Eigen::internal::sqrt (Scalar (3.0));
169  // Construct the parameters used in classifying the roots of the equation
170  // and in solving the equation for the roots in closed form.
171  Scalar c2_over_3 = c2*s_inv3;
172  Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
173  if (a_over_3 > Scalar (0))
174  a_over_3 = Scalar (0);
175 
176  Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
177 
178  Scalar q = half_b * half_b + a_over_3 * a_over_3*a_over_3;
179  if (q > Scalar (0))
180  q = Scalar (0);
181 
182  // Compute the eigenvalues by solving for the roots of the polynomial.
183  Scalar rho = Eigen::internal::sqrt (-a_over_3);
184  Scalar theta = std::atan2 (Eigen::internal::sqrt (-q), half_b) * s_inv3;
185  Scalar cos_theta = Eigen::internal::cos (theta);
186  Scalar sin_theta = Eigen::internal::sin (theta);
187  roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta;
188  roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
189  roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
190 
191  // Sort in increasing order.
192  if (roots (0) >= roots (1))
193  std::swap (roots (0), roots (1));
194  if (roots (1) >= roots (2))
195  {
196  std::swap (roots (1), roots (2));
197  if (roots (0) >= roots (1))
198  std::swap (roots (0), roots (1));
199  }
200 
201  if (roots (0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
202  computeRoots2 (c2, c1, roots);
203  }
204  }
205 
212  template <typename Matrix, typename Vector> inline void
213  eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
214  {
215  // if diagonal matrix, the eigenvalues are the diagonal elements
216  // and the eigenvectors are not unique, thus set to Identity
217  if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
218  {
219  if (mat.coeff (0) < mat.coeff (2))
220  {
221  eigenvalue = mat.coeff (0);
222  eigenvector [0] = 1.0;
223  eigenvector [1] = 0.0;
224  }
225  else
226  {
227  eigenvalue = mat.coeff (2);
228  eigenvector [0] = 0.0;
229  eigenvector [1] = 1.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 
243  eigenvalue = trace - sqrt (temp);
244 
245  eigenvector [0] = - mat.coeff (1);
246  eigenvector [1] = mat.coeff (0) - eigenvalue;
247  eigenvector.normalize ();
248  }
249 
256  template <typename Matrix, typename Vector> inline void
257  eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
258  {
259  // if diagonal matrix, the eigenvalues are the diagonal elements
260  // and the eigenvectors are not unique, thus set to Identity
261  if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
262  {
263  if (mat.coeff (0) < mat.coeff (3))
264  {
265  eigenvalues.coeffRef (0) = mat.coeff (0);
266  eigenvalues.coeffRef (1) = mat.coeff (3);
267  eigenvectors.coeffRef (0) = 1.0;
268  eigenvectors.coeffRef (1) = 0.0;
269  eigenvectors.coeffRef (2) = 0.0;
270  eigenvectors.coeffRef (3) = 1.0;
271  }
272  else
273  {
274  eigenvalues.coeffRef (0) = mat.coeff (3);
275  eigenvalues.coeffRef (1) = mat.coeff (0);
276  eigenvectors.coeffRef (0) = 0.0;
277  eigenvectors.coeffRef (1) = 1.0;
278  eigenvectors.coeffRef (2) = 1.0;
279  eigenvectors.coeffRef (3) = 0.0;
280  }
281  return;
282  }
283 
284  // 0.5 to optimize further calculations
285  typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
286  typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
287 
288  typename Matrix::Scalar temp = trace * trace - determinant;
289 
290  if (temp < 0)
291  temp = 0;
292  else
293  temp = sqrt (temp);
294 
295  eigenvalues.coeffRef (0) = trace - temp;
296  eigenvalues.coeffRef (1) = trace + temp;
297 
298  // either this is in a row or column depending on RowMajor or ColumnMajor
299  eigenvectors.coeffRef (0) = - mat.coeff (1);
300  eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0);
301  typename Matrix::Scalar norm = static_cast<typename Matrix::Scalar> (1.0) /
302  static_cast<typename Matrix::Scalar> (sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2)));
303  eigenvectors.coeffRef (0) *= norm;
304  eigenvectors.coeffRef (2) *= norm;
305  eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2);
306  eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
307  }
308 
315  template<typename Matrix, typename Vector> inline void
316  computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
317  {
318  typedef typename Matrix::Scalar Scalar;
319  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
320  // only when at least one matrix entry has magnitude larger than 1.
321 
322  Scalar scale = mat.cwiseAbs ().maxCoeff ();
323  if (scale <= std::numeric_limits<Scalar>::min ())
324  scale = Scalar (1.0);
325 
326  Matrix scaledMat = mat / scale;
327 
328  scaledMat.diagonal ().array () -= eigenvalue / scale;
329 
330  Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
331  Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
332  Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
333 
334  Scalar len1 = vec1.squaredNorm ();
335  Scalar len2 = vec2.squaredNorm ();
336  Scalar len3 = vec3.squaredNorm ();
337 
338  if (len1 >= len2 && len1 >= len3)
339  eigenvector = vec1 / Eigen::internal::sqrt (len1);
340  else if (len2 >= len1 && len2 >= len3)
341  eigenvector = vec2 / Eigen::internal::sqrt (len2);
342  else
343  eigenvector = vec3 / Eigen::internal::sqrt (len3);
344  }
345 
353  template<typename Matrix, typename Vector> inline void
354  eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
355  {
356  typedef typename Matrix::Scalar Scalar;
357  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
358  // only when at least one matrix entry has magnitude larger than 1.
359 
360  Scalar scale = mat.cwiseAbs ().maxCoeff ();
361  if (scale <= std::numeric_limits<Scalar>::min ())
362  scale = Scalar (1.0);
363 
364  Matrix scaledMat = mat / scale;
365 
366  Vector eigenvalues;
367  computeRoots (scaledMat, eigenvalues);
368 
369  eigenvalue = eigenvalues (0) * scale;
370 
371  scaledMat.diagonal ().array () -= eigenvalues (0);
372 
373  Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
374  Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
375  Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
376 
377  Scalar len1 = vec1.squaredNorm ();
378  Scalar len2 = vec2.squaredNorm ();
379  Scalar len3 = vec3.squaredNorm ();
380 
381  if (len1 >= len2 && len1 >= len3)
382  eigenvector = vec1 / Eigen::internal::sqrt (len1);
383  else if (len2 >= len1 && len2 >= len3)
384  eigenvector = vec2 / Eigen::internal::sqrt (len2);
385  else
386  eigenvector = vec3 / Eigen::internal::sqrt (len3);
387  }
388 
394  template<typename Matrix, typename Vector> inline void
395  eigen33 (const Matrix& mat, Vector& evals)
396  {
397  typedef typename Matrix::Scalar Scalar;
398  Scalar scale = mat.cwiseAbs ().maxCoeff ();
399  if (scale <= std::numeric_limits<Scalar>::min ())
400  scale = Scalar (1.0);
401 
402  Matrix scaledMat = mat / scale;
403  computeRoots (scaledMat, evals);
404  evals *= scale;
405  }
406 
413  template<typename Matrix, typename Vector> inline void
414  eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
415  {
416  typedef typename Matrix::Scalar Scalar;
417  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
418  // only when at least one matrix entry has magnitude larger than 1.
419 
420  Scalar scale = mat.cwiseAbs ().maxCoeff ();
421  if (scale <= std::numeric_limits<Scalar>::min ())
422  scale = Scalar (1.0);
423 
424  Matrix scaledMat = mat / scale;
425 
426  // Compute the eigenvalues
427  computeRoots (scaledMat, evals);
428 
429  if ((evals (2) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon ())
430  {
431  // all three equal
432  evecs.setIdentity ();
433  }
434  else if ((evals (1) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon () )
435  {
436  // first and second equal
437  Matrix tmp;
438  tmp = scaledMat;
439  tmp.diagonal ().array () -= evals (2);
440 
441  Vector vec1 = tmp.row (0).cross (tmp.row (1));
442  Vector vec2 = tmp.row (0).cross (tmp.row (2));
443  Vector vec3 = tmp.row (1).cross (tmp.row (2));
444 
445  Scalar len1 = vec1.squaredNorm ();
446  Scalar len2 = vec2.squaredNorm ();
447  Scalar len3 = vec3.squaredNorm ();
448 
449  if (len1 >= len2 && len1 >= len3)
450  evecs.col (2) = vec1 / Eigen::internal::sqrt (len1);
451  else if (len2 >= len1 && len2 >= len3)
452  evecs.col (2) = vec2 / Eigen::internal::sqrt (len2);
453  else
454  evecs.col (2) = vec3 / Eigen::internal::sqrt (len3);
455 
456  evecs.col (1) = evecs.col (2).unitOrthogonal ();
457  evecs.col (0) = evecs.col (1).cross (evecs.col (2));
458  }
459  else if ((evals (2) - evals (1)) <= Eigen::NumTraits<Scalar>::epsilon () )
460  {
461  // second and third equal
462  Matrix tmp;
463  tmp = scaledMat;
464  tmp.diagonal ().array () -= evals (0);
465 
466  Vector vec1 = tmp.row (0).cross (tmp.row (1));
467  Vector vec2 = tmp.row (0).cross (tmp.row (2));
468  Vector vec3 = tmp.row (1).cross (tmp.row (2));
469 
470  Scalar len1 = vec1.squaredNorm ();
471  Scalar len2 = vec2.squaredNorm ();
472  Scalar len3 = vec3.squaredNorm ();
473 
474  if (len1 >= len2 && len1 >= len3)
475  evecs.col (0) = vec1 / Eigen::internal::sqrt (len1);
476  else if (len2 >= len1 && len2 >= len3)
477  evecs.col (0) = vec2 / Eigen::internal::sqrt (len2);
478  else
479  evecs.col (0) = vec3 / Eigen::internal::sqrt (len3);
480 
481  evecs.col (1) = evecs.col (0).unitOrthogonal ();
482  evecs.col (2) = evecs.col (0).cross (evecs.col (1));
483  }
484  else
485  {
486  Matrix tmp;
487  tmp = scaledMat;
488  tmp.diagonal ().array () -= evals (2);
489 
490  Vector vec1 = tmp.row (0).cross (tmp.row (1));
491  Vector vec2 = tmp.row (0).cross (tmp.row (2));
492  Vector vec3 = tmp.row (1).cross (tmp.row (2));
493 
494  Scalar len1 = vec1.squaredNorm ();
495  Scalar len2 = vec2.squaredNorm ();
496  Scalar len3 = vec3.squaredNorm ();
497 #ifdef _WIN32
498  Scalar *mmax = new Scalar[3];
499 #else
500  Scalar mmax[3];
501 #endif
502  unsigned int min_el = 2;
503  unsigned int max_el = 2;
504  if (len1 >= len2 && len1 >= len3)
505  {
506  mmax[2] = len1;
507  evecs.col (2) = vec1 / Eigen::internal::sqrt (len1);
508  }
509  else if (len2 >= len1 && len2 >= len3)
510  {
511  mmax[2] = len2;
512  evecs.col (2) = vec2 / Eigen::internal::sqrt (len2);
513  }
514  else
515  {
516  mmax[2] = len3;
517  evecs.col (2) = vec3 / Eigen::internal::sqrt (len3);
518  }
519 
520  tmp = scaledMat;
521  tmp.diagonal ().array () -= evals (1);
522 
523  vec1 = tmp.row (0).cross (tmp.row (1));
524  vec2 = tmp.row (0).cross (tmp.row (2));
525  vec3 = tmp.row (1).cross (tmp.row (2));
526 
527  len1 = vec1.squaredNorm ();
528  len2 = vec2.squaredNorm ();
529  len3 = vec3.squaredNorm ();
530  if (len1 >= len2 && len1 >= len3)
531  {
532  mmax[1] = len1;
533  evecs.col (1) = vec1 / Eigen::internal::sqrt (len1);
534  min_el = len1 <= mmax[min_el] ? 1 : min_el;
535  max_el = len1 > mmax[max_el] ? 1 : max_el;
536  }
537  else if (len2 >= len1 && len2 >= len3)
538  {
539  mmax[1] = len2;
540  evecs.col (1) = vec2 / Eigen::internal::sqrt (len2);
541  min_el = len2 <= mmax[min_el] ? 1 : min_el;
542  max_el = len2 > mmax[max_el] ? 1 : max_el;
543  }
544  else
545  {
546  mmax[1] = len3;
547  evecs.col (1) = vec3 / Eigen::internal::sqrt (len3);
548  min_el = len3 <= mmax[min_el] ? 1 : min_el;
549  max_el = len3 > mmax[max_el] ? 1 : max_el;
550  }
551 
552  tmp = scaledMat;
553  tmp.diagonal ().array () -= evals (0);
554 
555  vec1 = tmp.row (0).cross (tmp.row (1));
556  vec2 = tmp.row (0).cross (tmp.row (2));
557  vec3 = tmp.row (1).cross (tmp.row (2));
558 
559  len1 = vec1.squaredNorm ();
560  len2 = vec2.squaredNorm ();
561  len3 = vec3.squaredNorm ();
562  if (len1 >= len2 && len1 >= len3)
563  {
564  mmax[0] = len1;
565  evecs.col (0) = vec1 / Eigen::internal::sqrt (len1);
566  min_el = len3 <= mmax[min_el] ? 0 : min_el;
567  max_el = len3 > mmax[max_el] ? 0 : max_el;
568  }
569  else if (len2 >= len1 && len2 >= len3)
570  {
571  mmax[0] = len2;
572  evecs.col (0) = vec2 / Eigen::internal::sqrt (len2);
573  min_el = len3 <= mmax[min_el] ? 0 : min_el;
574  max_el = len3 > mmax[max_el] ? 0 : max_el;
575  }
576  else
577  {
578  mmax[0] = len3;
579  evecs.col (0) = vec3 / Eigen::internal::sqrt (len3);
580  min_el = len3 <= mmax[min_el] ? 0 : min_el;
581  max_el = len3 > mmax[max_el] ? 0 : max_el;
582  }
583 
584  unsigned mid_el = 3 - min_el - max_el;
585  evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized ();
586  evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized ();
587 #ifdef _WIN32
588  delete [] mmax;
589 #endif
590  }
591  // Rescale back to the original size.
592  evals *= scale;
593  }
594 
602  template<typename Matrix> inline typename Matrix::Scalar
603  invert2x2 (const Matrix& matrix, Matrix& inverse)
604  {
605  typedef typename Matrix::Scalar Scalar;
606  Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2) ;
607 
608  if (det != 0)
609  {
610  //Scalar inv_det = Scalar (1.0) / det;
611  inverse.coeffRef (0) = matrix.coeff (3);
612  inverse.coeffRef (1) = - matrix.coeff (1);
613  inverse.coeffRef (2) = - matrix.coeff (2);
614  inverse.coeffRef (3) = matrix.coeff (0);
615  inverse /= det;
616  }
617  return det;
618  }
619 
627  template<typename Matrix> inline typename Matrix::Scalar
628  invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
629  {
630  typedef typename Matrix::Scalar Scalar;
631  // elements
632  // a b c
633  // b d e
634  // c e f
635  //| a b c |-1 | fd-ee ce-bf be-cd |
636  //| b d e | = 1/det * | ce-bf af-cc bc-ae |
637  //| c e f | | be-cd bc-ae ad-bb |
638 
639  //det = a(fd-ee) + b(ec-fb) + c(eb-dc)
640 
641  Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
642  Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
643  Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
644 
645  Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
646 
647  if (det != 0)
648  {
649  //Scalar inv_det = Scalar (1.0) / det;
650  inverse.coeffRef (0) = fd_ee;
651  inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
652  inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
653  inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
654  inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
655  inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
656  inverse /= det;
657  }
658  return det;
659  }
660 
667  template<typename Matrix> inline typename Matrix::Scalar
668  invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
669  {
670  typedef typename Matrix::Scalar Scalar;
671 
672  //| a b c |-1 | ie-hf hc-ib fb-ec |
673  //| d e f | = 1/det * | gf-id ia-gc dc-fa |
674  //| g h i | | hd-ge gb-ha ea-db |
675  //det = a(ie-hf) + d(hc-ib) + g(fb-ec)
676 
677  Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
678  Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
679  Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
680  Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec) ;
681 
682  if (det != 0)
683  {
684  inverse.coeffRef (0) = ie_hf;
685  inverse.coeffRef (1) = hc_ib;
686  inverse.coeffRef (2) = fb_ec;
687  inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
688  inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
689  inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
690  inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
691  inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
692  inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
693 
694  inverse /= det;
695  }
696  return det;
697  }
698 
699  template<typename Matrix> inline typename Matrix::Scalar
700  determinant3x3Matrix (const Matrix& matrix)
701  {
702  // result is independent of Row/Col Major storage!
703  return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
704  matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
705  matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
706  }
707 
715  inline void
716  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction,
717  Eigen::Affine3f& transformation);
718 
726  inline Eigen::Affine3f
727  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction);
728 
736  inline void
737  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction,
738  Eigen::Affine3f& transformation);
739 
747  inline Eigen::Affine3f
748  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction);
749 
757  inline void
758  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis,
759  Eigen::Affine3f& transformation);
760 
768  inline Eigen::Affine3f
769  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis);
770 
779  inline void
780  getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis,
781  const Eigen::Vector3f& origin, Eigen::Affine3f& transformation);
782 
790  inline void
791  getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw);
792 
803  inline void
804  getTranslationAndEulerAngles (const Eigen::Affine3f& t, float& x, float& y, float& z,
805  float& roll, float& pitch, float& yaw);
806 
817  inline void
818  getTransformation (float x, float y, float z, float roll, float pitch, float yaw, Eigen::Affine3f& t);
819 
830  inline Eigen::Affine3f
831  getTransformation (float x, float y, float z, float roll, float pitch, float yaw);
832 
838  template <typename Derived> void
839  saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
840 
846  template <typename Derived> void
847  loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);
848 }
849 
850 #include <pcl/common/impl/eigen.hpp>
851 
852 #if defined __SUNPRO_CC
853 # pragma enable_warn
854 #elif defined _MSC_VER
855 # pragma warning(pop)
856 #endif
857 
858 #endif //#ifndef PCL_EIGEN_H_
Generated on Sat Aug 24 2013 17:28:26 for Point Cloud Library (PCL) by   doxygen 1.8.4