eigen.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *  Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
00007  *  Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
00008  *  Copyright (c) 2012-, Open Perception, Inc.
00009  *
00010  *  All rights reserved.
00011  *
00012  *  Redistribution and use in source and binary forms, with or without
00013  *  modification, are permitted provided that the following conditions
00014  *  are met:
00015  *
00016  *   * Redistributions of source code must retain the above copyright
00017  *     notice, this list of conditions and the following disclaimer.
00018  *   * Redistributions in binary form must reproduce the above
00019  *     copyright notice, this list of conditions and the following
00020  *     disclaimer in the documentation and/or other materials provided
00021  *     with the distribution.
00022  *   * Neither the name of the copyright holder(s) nor the names of its
00023  *     contributors may be used to endorse or promote products derived
00024  *     from this software without specific prior written permission.
00025  *
00026  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00027  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00028  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00029  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00030  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00031  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00032  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00033  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00034  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00035  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00036  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00037  *  POSSIBILITY OF SUCH DAMAGE.
00038  *
00039  * $Id$
00040  *
00041  */
00042 #ifndef PCL_COMMON_EIGEN_H_
00043 #define PCL_COMMON_EIGEN_H_
00044 
00045 #ifndef NOMINMAX
00046 #define NOMINMAX
00047 #endif
00048 
00049 #if defined __GNUC__
00050 #  pragma GCC system_header
00051 #elif defined __SUNPRO_CC
00052 #  pragma disable_warn
00053 #endif
00054 
00055 #include <cmath>
00056 
00057 #include <Eigen/StdVector>
00058 #include <Eigen/Core>
00059 #include <Eigen/Eigenvalues>
00060 #include <Eigen/Geometry>
00061 #include <Eigen/SVD>
00062 #include <Eigen/LU>
00063 #include <Eigen/Dense>
00064 #include <Eigen/Eigenvalues>
00065 
00066 namespace pcl
00067 {
00073   template<typename Scalar, typename Roots> inline void
00074   computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
00075   {
00076     roots (0) = Scalar (0);
00077     Scalar d = Scalar (b * b - 4.0 * c);
00078     if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN!
00079       d = 0.0;
00080 
00081     Scalar sd = ::std::sqrt (d);
00082 
00083     roots (2) = 0.5f * (b + sd);
00084     roots (1) = 0.5f * (b - sd);
00085   }
00086 
00091   template<typename Matrix, typename Roots> inline void
00092   computeRoots (const Matrix& m, Roots& roots)
00093   {
00094     typedef typename Matrix::Scalar Scalar;
00095 
00096     // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
00097     // eigenvalues are the roots to this equation, all guaranteed to be
00098     // real-valued, because the matrix is symmetric.
00099     Scalar c0 =            m (0, 0) * m (1, 1) * m (2, 2)
00100             + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2)
00101                          - m (0, 0) * m (1, 2) * m (1, 2)
00102                          - m (1, 1) * m (0, 2) * m (0, 2)
00103                          - m (2, 2) * m (0, 1) * m (0, 1);
00104     Scalar c1 = m (0, 0) * m (1, 1) -
00105                 m (0, 1) * m (0, 1) +
00106                 m (0, 0) * m (2, 2) -
00107                 m (0, 2) * m (0, 2) +
00108                 m (1, 1) * m (2, 2) -
00109                 m (1, 2) * m (1, 2);
00110     Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
00111 
00112 
00113     if (fabs (c0) < Eigen::NumTraits<Scalar>::epsilon ())// one root is 0 -> quadratic equation
00114       computeRoots2 (c2, c1, roots);
00115     else
00116     {
00117       const Scalar s_inv3 = Scalar (1.0 / 3.0);
00118       const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
00119       // Construct the parameters used in classifying the roots of the equation
00120       // and in solving the equation for the roots in closed form.
00121       Scalar c2_over_3 = c2*s_inv3;
00122       Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
00123       if (a_over_3 > Scalar (0))
00124         a_over_3 = Scalar (0);
00125 
00126       Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
00127 
00128       Scalar q = half_b * half_b + a_over_3 * a_over_3*a_over_3;
00129       if (q > Scalar (0))
00130         q = Scalar (0);
00131 
00132       // Compute the eigenvalues by solving for the roots of the polynomial.
00133       Scalar rho = std::sqrt (-a_over_3);
00134       Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3;
00135       Scalar cos_theta = std::cos (theta);
00136       Scalar sin_theta = std::sin (theta);
00137       roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta;
00138       roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
00139       roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
00140 
00141       // Sort in increasing order.
00142       if (roots (0) >= roots (1))
00143         std::swap (roots (0), roots (1));
00144       if (roots (1) >= roots (2))
00145       {
00146         std::swap (roots (1), roots (2));
00147         if (roots (0) >= roots (1))
00148           std::swap (roots (0), roots (1));
00149       }
00150 
00151       if (roots (0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
00152         computeRoots2 (c2, c1, roots);
00153     }
00154   }
00155 
00162   template <typename Matrix, typename Vector> inline void
00163   eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
00164   {
00165     // if diagonal matrix, the eigenvalues are the diagonal elements
00166     // and the eigenvectors are not unique, thus set to Identity
00167     if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
00168     {
00169       if (mat.coeff (0) < mat.coeff (2))
00170       {
00171         eigenvalue = mat.coeff (0);
00172         eigenvector [0] = 1.0;
00173         eigenvector [1] = 0.0;
00174       }
00175       else
00176       {
00177         eigenvalue = mat.coeff (2);
00178         eigenvector [0] = 0.0;
00179         eigenvector [1] = 1.0;
00180       }
00181       return;
00182     }
00183     
00184     // 0.5 to optimize further calculations
00185     typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
00186     typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
00187 
00188     typename Matrix::Scalar temp = trace * trace - determinant;
00189 
00190     if (temp < 0)
00191       temp = 0;
00192 
00193     eigenvalue = trace - ::std::sqrt (temp);
00194     
00195     eigenvector [0] = - mat.coeff (1);
00196     eigenvector [1] = mat.coeff (0) - eigenvalue;
00197     eigenvector.normalize ();
00198   }
00199 
00206   template <typename Matrix, typename Vector> inline void
00207   eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
00208   {
00209     // if diagonal matrix, the eigenvalues are the diagonal elements
00210     // and the eigenvectors are not unique, thus set to Identity
00211     if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
00212     {
00213       if (mat.coeff (0) < mat.coeff (3))
00214       {
00215         eigenvalues.coeffRef (0) = mat.coeff (0);
00216         eigenvalues.coeffRef (1) = mat.coeff (3);
00217         eigenvectors.coeffRef (0) = 1.0;
00218         eigenvectors.coeffRef (1) = 0.0;
00219         eigenvectors.coeffRef (2) = 0.0;
00220         eigenvectors.coeffRef (3) = 1.0;        
00221       }
00222       else
00223       {
00224         eigenvalues.coeffRef (0) = mat.coeff (3);
00225         eigenvalues.coeffRef (1) = mat.coeff (0);
00226         eigenvectors.coeffRef (0) = 0.0;
00227         eigenvectors.coeffRef (1) = 1.0;
00228         eigenvectors.coeffRef (2) = 1.0;
00229         eigenvectors.coeffRef (3) = 0.0;        
00230       }
00231       return;
00232     }
00233 
00234     // 0.5 to optimize further calculations
00235     typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
00236     typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
00237 
00238     typename Matrix::Scalar temp = trace * trace - determinant;
00239 
00240     if (temp < 0)
00241       temp = 0;
00242     else
00243       temp = ::std::sqrt (temp);
00244 
00245     eigenvalues.coeffRef (0) = trace - temp;
00246     eigenvalues.coeffRef (1) = trace + temp;
00247 
00248     // either this is in a row or column depending on RowMajor or ColumnMajor
00249     eigenvectors.coeffRef (0) = - mat.coeff (1);
00250     eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0);
00251     typename Matrix::Scalar norm = static_cast<typename Matrix::Scalar> (1.0) / 
00252                                    static_cast<typename Matrix::Scalar> (::std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2)));
00253     eigenvectors.coeffRef (0) *= norm;
00254     eigenvectors.coeffRef (2) *= norm;
00255     eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2);
00256     eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
00257   }
00258 
00265   template<typename Matrix, typename Vector> inline void
00266   computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
00267   {
00268     typedef typename Matrix::Scalar Scalar;
00269     // Scale the matrix so its entries are in [-1,1].  The scaling is applied
00270     // only when at least one matrix entry has magnitude larger than 1.
00271 
00272     Scalar scale = mat.cwiseAbs ().maxCoeff ();
00273     if (scale <= std::numeric_limits<Scalar>::min ())
00274       scale = Scalar (1.0);
00275 
00276     Matrix scaledMat = mat / scale;
00277 
00278     scaledMat.diagonal ().array () -= eigenvalue / scale;
00279 
00280     Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
00281     Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
00282     Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
00283 
00284     Scalar len1 = vec1.squaredNorm ();
00285     Scalar len2 = vec2.squaredNorm ();
00286     Scalar len3 = vec3.squaredNorm ();
00287 
00288     if (len1 >= len2 && len1 >= len3)
00289       eigenvector = vec1 / std::sqrt (len1);
00290     else if (len2 >= len1 && len2 >= len3)
00291       eigenvector = vec2 / std::sqrt (len2);
00292     else
00293       eigenvector = vec3 / std::sqrt (len3);
00294   }
00295   
00303   template<typename Matrix, typename Vector> inline void
00304   eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
00305   {
00306     typedef typename Matrix::Scalar Scalar;
00307     // Scale the matrix so its entries are in [-1,1].  The scaling is applied
00308     // only when at least one matrix entry has magnitude larger than 1.
00309 
00310     Scalar scale = mat.cwiseAbs ().maxCoeff ();
00311     if (scale <= std::numeric_limits<Scalar>::min ())
00312       scale = Scalar (1.0);
00313 
00314     Matrix scaledMat = mat / scale;
00315 
00316     Vector eigenvalues;
00317     computeRoots (scaledMat, eigenvalues);
00318 
00319     eigenvalue = eigenvalues (0) * scale;
00320 
00321     scaledMat.diagonal ().array () -= eigenvalues (0);
00322 
00323     Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
00324     Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
00325     Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
00326 
00327     Scalar len1 = vec1.squaredNorm ();
00328     Scalar len2 = vec2.squaredNorm ();
00329     Scalar len3 = vec3.squaredNorm ();
00330 
00331     if (len1 >= len2 && len1 >= len3)
00332       eigenvector = vec1 / std::sqrt (len1);
00333     else if (len2 >= len1 && len2 >= len3)
00334       eigenvector = vec2 / std::sqrt (len2);
00335     else
00336       eigenvector = vec3 / std::sqrt (len3);
00337   }
00338 
00344   template<typename Matrix, typename Vector> inline void
00345   eigen33 (const Matrix& mat, Vector& evals)
00346   {
00347     typedef typename Matrix::Scalar Scalar;
00348     Scalar scale = mat.cwiseAbs ().maxCoeff ();
00349     if (scale <= std::numeric_limits<Scalar>::min ())
00350       scale = Scalar (1.0);
00351 
00352     Matrix scaledMat = mat / scale;
00353     computeRoots (scaledMat, evals);
00354     evals *= scale;
00355   }
00356 
00363   template<typename Matrix, typename Vector> inline void
00364   eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
00365   {
00366     typedef typename Matrix::Scalar Scalar;
00367     // Scale the matrix so its entries are in [-1,1].  The scaling is applied
00368     // only when at least one matrix entry has magnitude larger than 1.
00369 
00370     Scalar scale = mat.cwiseAbs ().maxCoeff ();
00371     if (scale <= std::numeric_limits<Scalar>::min ())
00372       scale = Scalar (1.0);
00373 
00374     Matrix scaledMat = mat / scale;
00375 
00376     // Compute the eigenvalues
00377     computeRoots (scaledMat, evals);
00378 
00379     if ((evals (2) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon ())
00380     {
00381       // all three equal
00382       evecs.setIdentity ();
00383     }
00384     else if ((evals (1) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon () )
00385     {
00386       // first and second equal
00387       Matrix tmp;
00388       tmp = scaledMat;
00389       tmp.diagonal ().array () -= evals (2);
00390 
00391       Vector vec1 = tmp.row (0).cross (tmp.row (1));
00392       Vector vec2 = tmp.row (0).cross (tmp.row (2));
00393       Vector vec3 = tmp.row (1).cross (tmp.row (2));
00394 
00395       Scalar len1 = vec1.squaredNorm ();
00396       Scalar len2 = vec2.squaredNorm ();
00397       Scalar len3 = vec3.squaredNorm ();
00398 
00399       if (len1 >= len2 && len1 >= len3)
00400         evecs.col (2) = vec1 / std::sqrt (len1);
00401       else if (len2 >= len1 && len2 >= len3)
00402         evecs.col (2) = vec2 / std::sqrt (len2);
00403       else
00404         evecs.col (2) = vec3 / std::sqrt (len3);
00405 
00406       evecs.col (1) = evecs.col (2).unitOrthogonal ();
00407       evecs.col (0) = evecs.col (1).cross (evecs.col (2));
00408     }
00409     else if ((evals (2) - evals (1)) <= Eigen::NumTraits<Scalar>::epsilon () )
00410     {
00411       // second and third equal
00412       Matrix tmp;
00413       tmp = scaledMat;
00414       tmp.diagonal ().array () -= evals (0);
00415 
00416       Vector vec1 = tmp.row (0).cross (tmp.row (1));
00417       Vector vec2 = tmp.row (0).cross (tmp.row (2));
00418       Vector vec3 = tmp.row (1).cross (tmp.row (2));
00419 
00420       Scalar len1 = vec1.squaredNorm ();
00421       Scalar len2 = vec2.squaredNorm ();
00422       Scalar len3 = vec3.squaredNorm ();
00423 
00424       if (len1 >= len2 && len1 >= len3)
00425         evecs.col (0) = vec1 / std::sqrt (len1);
00426       else if (len2 >= len1 && len2 >= len3)
00427         evecs.col (0) = vec2 / std::sqrt (len2);
00428       else
00429         evecs.col (0) = vec3 / std::sqrt (len3);
00430 
00431       evecs.col (1) = evecs.col (0).unitOrthogonal ();
00432       evecs.col (2) = evecs.col (0).cross (evecs.col (1));
00433     }
00434     else
00435     {
00436       Matrix tmp;
00437       tmp = scaledMat;
00438       tmp.diagonal ().array () -= evals (2);
00439 
00440       Vector vec1 = tmp.row (0).cross (tmp.row (1));
00441       Vector vec2 = tmp.row (0).cross (tmp.row (2));
00442       Vector vec3 = tmp.row (1).cross (tmp.row (2));
00443 
00444       Scalar len1 = vec1.squaredNorm ();
00445       Scalar len2 = vec2.squaredNorm ();
00446       Scalar len3 = vec3.squaredNorm ();
00447 #ifdef _WIN32
00448       Scalar *mmax = new Scalar[3];
00449 #else
00450       Scalar mmax[3];
00451 #endif
00452       unsigned int min_el = 2;
00453       unsigned int max_el = 2;
00454       if (len1 >= len2 && len1 >= len3)
00455       {
00456         mmax[2] = len1;
00457         evecs.col (2) = vec1 / std::sqrt (len1);
00458       }
00459       else if (len2 >= len1 && len2 >= len3)
00460       {
00461         mmax[2] = len2;
00462         evecs.col (2) = vec2 / std::sqrt (len2);
00463       }
00464       else
00465       {
00466         mmax[2] = len3;
00467         evecs.col (2) = vec3 / std::sqrt (len3);
00468       }
00469 
00470       tmp = scaledMat;
00471       tmp.diagonal ().array () -= evals (1);
00472 
00473       vec1 = tmp.row (0).cross (tmp.row (1));
00474       vec2 = tmp.row (0).cross (tmp.row (2));
00475       vec3 = tmp.row (1).cross (tmp.row (2));
00476 
00477       len1 = vec1.squaredNorm ();
00478       len2 = vec2.squaredNorm ();
00479       len3 = vec3.squaredNorm ();
00480       if (len1 >= len2 && len1 >= len3)
00481       {
00482         mmax[1] = len1;
00483         evecs.col (1) = vec1 / std::sqrt (len1);
00484         min_el = len1 <= mmax[min_el] ? 1 : min_el;
00485         max_el = len1 > mmax[max_el] ? 1 : max_el;
00486       }
00487       else if (len2 >= len1 && len2 >= len3)
00488       {
00489         mmax[1] = len2;
00490         evecs.col (1) = vec2 / std::sqrt (len2);
00491         min_el = len2 <= mmax[min_el] ? 1 : min_el;
00492         max_el = len2 > mmax[max_el] ? 1 : max_el;
00493       }
00494       else
00495       {
00496         mmax[1] = len3;
00497         evecs.col (1) = vec3 / std::sqrt (len3);
00498         min_el = len3 <= mmax[min_el] ? 1 : min_el;
00499         max_el = len3 > mmax[max_el] ? 1 : max_el;
00500       }
00501 
00502       tmp = scaledMat;
00503       tmp.diagonal ().array () -= evals (0);
00504 
00505       vec1 = tmp.row (0).cross (tmp.row (1));
00506       vec2 = tmp.row (0).cross (tmp.row (2));
00507       vec3 = tmp.row (1).cross (tmp.row (2));
00508 
00509       len1 = vec1.squaredNorm ();
00510       len2 = vec2.squaredNorm ();
00511       len3 = vec3.squaredNorm ();
00512       if (len1 >= len2 && len1 >= len3)
00513       {
00514         mmax[0] = len1;
00515         evecs.col (0) = vec1 / std::sqrt (len1);
00516         min_el = len3 <= mmax[min_el] ? 0 : min_el;
00517         max_el = len3 > mmax[max_el] ? 0 : max_el;
00518       }
00519       else if (len2 >= len1 && len2 >= len3)
00520       {
00521         mmax[0] = len2;
00522         evecs.col (0) = vec2 / std::sqrt (len2);
00523         min_el = len3 <= mmax[min_el] ? 0 : min_el;
00524         max_el = len3 > mmax[max_el] ? 0 : max_el;
00525       }
00526       else
00527       {
00528         mmax[0] = len3;
00529         evecs.col (0) = vec3 / std::sqrt (len3);
00530         min_el = len3 <= mmax[min_el] ? 0 : min_el;
00531         max_el = len3 > mmax[max_el] ? 0 : max_el;
00532       }
00533 
00534       unsigned mid_el = 3 - min_el - max_el;
00535       evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized ();
00536       evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized ();
00537 #ifdef _WIN32
00538       delete [] mmax;
00539 #endif
00540     }
00541     // Rescale back to the original size.
00542     evals *= scale;
00543   }
00544 
00552   template<typename Matrix> inline typename Matrix::Scalar
00553   invert2x2 (const Matrix& matrix, Matrix& inverse)
00554   {
00555     typedef typename Matrix::Scalar Scalar;
00556     Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2) ;
00557 
00558     if (det != 0)
00559     {
00560       //Scalar inv_det = Scalar (1.0) / det;
00561       inverse.coeffRef (0) =   matrix.coeff (3);
00562       inverse.coeffRef (1) = - matrix.coeff (1);
00563       inverse.coeffRef (2) = - matrix.coeff (2);
00564       inverse.coeffRef (3) =   matrix.coeff (0);
00565       inverse /= det;
00566     }
00567     return det;
00568   }
00569 
00577   template<typename Matrix> inline typename Matrix::Scalar
00578   invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
00579   {
00580     typedef typename Matrix::Scalar Scalar;
00581     // elements
00582     // a b c
00583     // b d e
00584     // c e f
00585     //| a b c |-1             |   fd-ee    ce-bf   be-cd  |
00586     //| b d e |    =  1/det * |   ce-bf    af-cc   bc-ae  |
00587     //| c e f |               |   be-cd    bc-ae   ad-bb  |
00588 
00589     //det = a(fd-ee) + b(ec-fb) + c(eb-dc)
00590 
00591     Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
00592     Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
00593     Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
00594 
00595     Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
00596 
00597     if (det != 0)
00598     {
00599       //Scalar inv_det = Scalar (1.0) / det;
00600       inverse.coeffRef (0) = fd_ee;
00601       inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
00602       inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
00603       inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
00604       inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
00605       inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
00606       inverse /= det;
00607     }
00608     return det;
00609   }
00610 
00617   template<typename Matrix> inline typename Matrix::Scalar
00618   invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
00619   {
00620     typedef typename Matrix::Scalar Scalar;
00621 
00622     //| a b c |-1             |   ie-hf    hc-ib   fb-ec  |
00623     //| d e f |    =  1/det * |   gf-id    ia-gc   dc-fa  |
00624     //| g h i |               |   hd-ge    gb-ha   ea-db  |
00625     //det = a(ie-hf) + d(hc-ib) + g(fb-ec)
00626 
00627     Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
00628     Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
00629     Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
00630     Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec) ;
00631 
00632     if (det != 0)
00633     {
00634       inverse.coeffRef (0) = ie_hf;
00635       inverse.coeffRef (1) = hc_ib;
00636       inverse.coeffRef (2) = fb_ec;
00637       inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
00638       inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
00639       inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
00640       inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
00641       inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
00642       inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
00643 
00644       inverse /= det;
00645     }
00646     return det;
00647   }
00648 
00649   template<typename Matrix> inline typename Matrix::Scalar
00650   determinant3x3Matrix (const Matrix& matrix)
00651   {
00652     // result is independent of Row/Col Major storage!
00653     return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
00654            matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
00655            matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
00656   }
00657   
00665   inline void
00666   getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 
00667                              const Eigen::Vector3f& y_direction,
00668                              Eigen::Affine3f& transformation);
00669 
00677   inline Eigen::Affine3f
00678   getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 
00679                              const Eigen::Vector3f& y_direction);
00680 
00688   inline void
00689   getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, 
00690                              const Eigen::Vector3f& y_direction,
00691                              Eigen::Affine3f& transformation);
00692 
00700   inline Eigen::Affine3f
00701   getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, 
00702                              const Eigen::Vector3f& y_direction);
00703 
00711   inline void
00712   getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 
00713                                        const Eigen::Vector3f& z_axis,
00714                                        Eigen::Affine3f& transformation);
00715 
00723   inline Eigen::Affine3f
00724   getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 
00725                                        const Eigen::Vector3f& z_axis);
00726 
00735   inline void
00736   getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, 
00737                                                 const Eigen::Vector3f& z_axis,
00738                                                 const Eigen::Vector3f& origin, 
00739                                                 Eigen::Affine3f& transformation);
00740 
00748   inline void
00749   getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw);
00750 
00761   inline void
00762   getTranslationAndEulerAngles (const Eigen::Affine3f& t, 
00763                                 float& x, float& y, float& z,
00764                                 float& roll, float& pitch, float& yaw);
00765 
00776   template <typename Scalar> inline void
00777   getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, 
00778                      Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
00779 
00780   inline void
00781   getTransformation (float x, float y, float z, float roll, float pitch, float yaw, 
00782                      Eigen::Affine3f &t)
00783   {
00784     return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
00785   }
00786 
00787   inline void
00788   getTransformation (double x, double y, double z, double roll, double pitch, double yaw, 
00789                      Eigen::Affine3d &t)
00790   {
00791     return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
00792   }
00793 
00804   inline Eigen::Affine3f
00805   getTransformation (float x, float y, float z, float roll, float pitch, float yaw);
00806 
00812   template <typename Derived> void
00813   saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
00814 
00820   template <typename Derived> void
00821   loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);
00822 
00823 // PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
00824 // followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
00825 // finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
00826 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
00827                            : (int (a) == 1 || int (b) == 1) ? 1 \
00828                            : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
00829                            : (int (a) <= int (b)) ? int (a) : int (b))
00830 
00861   template <typename Derived, typename OtherDerived> 
00862   typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
00863   umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling = false);
00864 }
00865 
00866 #include <pcl/common/impl/eigen.hpp>
00867 
00868 #if defined __SUNPRO_CC
00869 #  pragma enable_warn
00870 #endif
00871 
00872 #endif  //PCL_COMMON_EIGEN_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:25