Point Cloud Library (PCL)  1.3.1
eigen.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: eigen.h 2517 2011-09-19 17:47:45Z jspricke $
00035  *
00036  */
00037 // This file is part of Eigen, a lightweight C++ template library
00038 // for linear algebra.
00039 //
00040 // Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
00041 //
00042 // Eigen is free software; you can redistribute it and/or
00043 // modify it under the terms of the GNU Lesser General Public
00044 // License as published by the Free Software Foundation; either
00045 // version 3 of the License, or (at your option) any later version.
00046 //
00047 // Alternatively, you can redistribute it and/or
00048 // modify it under the terms of the GNU General Public License as
00049 // published by the Free Software Foundation; either version 2 of
00050 // the License, or (at your option) any later version.
00051 //
00052 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00053 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00054 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00055 // GNU General Public License for more details.
00056 //
00057 // You should have received a copy of the GNU Lesser General Public
00058 // License and a copy of the GNU General Public License along with
00059 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00060 
00061 // The computeRoots function included in this is based on materials
00062 // covered by the following copyright and license:
00063 // 
00064 // Geometric Tools, LLC
00065 // Copyright (c) 1998-2010
00066 // Distributed under the Boost Software License, Version 1.0.
00067 // 
00068 // Permission is hereby granted, free of charge, to any person or organization
00069 // obtaining a copy of the software and accompanying documentation covered by
00070 // this license (the "Software") to use, reproduce, display, distribute,
00071 // execute, and transmit the Software, and to prepare derivative works of the
00072 // Software, and to permit third-parties to whom the Software is furnished to
00073 // do so, all subject to the following:
00074 // 
00075 // The copyright notices in the Software and this entire statement, including
00076 // the above license grant, this restriction and the following disclaimer,
00077 // must be included in all copies of the Software, in whole or in part, and
00078 // all derivative works of the Software, unless such copies or derivative
00079 // works are solely in the form of machine-executable object code generated by
00080 // a source language processor.
00081 // 
00082 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00083 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00084 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
00085 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
00086 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
00087 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
00088 // DEALINGS IN THE SOFTWARE.
00089 
00090 #ifndef PCL_EIGEN_H_
00091 #define PCL_EIGEN_H_
00092 
00093 #define NOMINMAX
00094 
00095 #include <Eigen/Core>
00096 #include <Eigen/Eigenvalues>
00097 #include <Eigen/Geometry>
00098 
00099 namespace pcl
00100 {
00101   template<typename Scalar, typename Roots> inline void computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
00102   {
00103     roots(0) = Scalar(0);
00104     Scalar d = b * b - 4.0 * c;
00105     if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN!
00106       d = 0.0;
00107 
00108     Scalar sd = sqrt (d);
00109     
00110     roots (2) = 0.5f * (b + sd);
00111     roots (1) = 0.5f * (b - sd);
00112   }
00113   
00114   template<typename Matrix, typename Roots> inline void 
00115   computeRoots (const Matrix& m, Roots& roots)
00116   {
00117     typedef typename Matrix::Scalar Scalar;
00118 
00119     // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
00120     // eigenvalues are the roots to this equation, all guaranteed to be
00121     // real-valued, because the matrix is symmetric.
00122     Scalar c0 =               m(0,0)*m(1,1)*m(2,2) 
00123                 + Scalar(2) * m(0,1)*m(0,2)*m(1,2) 
00124                             - m(0,0)*m(1,2)*m(1,2) 
00125                             - m(1,1)*m(0,2)*m(0,2) 
00126                             - m(2,2)*m(0,1)*m(0,1);
00127     Scalar c1 = m(0,0)*m(1,1) - 
00128                 m(0,1)*m(0,1) + 
00129                 m(0,0)*m(2,2) - 
00130                 m(0,2)*m(0,2) + 
00131                 m(1,1)*m(2,2) - 
00132                 m(1,2)*m(1,2);
00133     Scalar c2 = m(0,0) + m(1,1) + m(2,2);
00134 
00135 
00136     if (fabs(c0) < Eigen::NumTraits<Scalar>::epsilon())// one root is 0 -> quadratic equation
00137       computeRoots2 (c2, c1, roots);
00138     else
00139     {
00140       const Scalar s_inv3 = 1.0/3.0;
00141       const Scalar s_sqrt3 = Eigen::internal::sqrt (Scalar (3.0));
00142       // Construct the parameters used in classifying the roots of the equation
00143       // and in solving the equation for the roots in closed form.
00144       Scalar c2_over_3 = c2*s_inv3;
00145       Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
00146       if (a_over_3 > Scalar (0))
00147         a_over_3 = Scalar (0);
00148 
00149       Scalar half_b = Scalar(0.5) * (c0 + c2_over_3 * (Scalar(2) * c2_over_3 * c2_over_3 - c1));
00150 
00151       Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
00152       if (q > Scalar(0))
00153         q = Scalar(0);
00154 
00155       // Compute the eigenvalues by solving for the roots of the polynomial.
00156       Scalar rho = Eigen::internal::sqrt (-a_over_3);
00157       Scalar theta = std::atan2 (Eigen::internal::sqrt (-q), half_b)*s_inv3;
00158       Scalar cos_theta = Eigen::internal::cos (theta);
00159       Scalar sin_theta = Eigen::internal::sin (theta);
00160       roots(0) = c2_over_3 + Scalar(2) * rho * cos_theta;
00161       roots(1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
00162       roots(2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
00163 
00164       // Sort in increasing order.
00165       if (roots (0) >= roots (1))
00166         std::swap (roots (0), roots (1));
00167       if (roots (1) >= roots (2))
00168       {
00169         std::swap (roots (1), roots (2));
00170         if (roots (0) >= roots (1))
00171           std::swap (roots (0), roots (1));
00172       }
00173       
00174       if (roots(0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
00175         computeRoots2 (c2, c1, roots);
00176     }
00177   }
00178 
00179   template<typename Matrix, typename Vector> void 
00180   eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
00181   {
00182     typedef typename Matrix::Scalar Scalar;
00183     // Scale the matrix so its entries are in [-1,1].  The scaling is applied
00184     // only when at least one matrix entry has magnitude larger than 1.
00185 
00186     Scalar scale = mat.cwiseAbs ().maxCoeff ();
00187     if (scale <= std::numeric_limits<Scalar>::min())
00188       scale = Scalar(1.0);
00189     
00190     Matrix scaledMat = mat / scale;
00191 
00192     // Compute the eigenvalues
00193     computeRoots (scaledMat,evals);
00194     
00195     if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
00196     {
00197       // all three equal
00198       evecs.setIdentity();
00199     }
00200     else if ((evals(1)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon() )
00201     {
00202       // first and second equal
00203       Matrix tmp;
00204       tmp = scaledMat;
00205       tmp.diagonal ().array () -= evals (2);
00206 
00207       Vector vec1 = tmp.row (0).cross (tmp.row (1));
00208       Vector vec2 = tmp.row (0).cross (tmp.row (2));
00209       Vector vec3 = tmp.row (1).cross (tmp.row (2));
00210 
00211       Scalar len1 = vec1.squaredNorm();
00212       Scalar len2 = vec2.squaredNorm();
00213       Scalar len3 = vec3.squaredNorm();
00214 
00215       if (len1 >= len2 && len1 >= len3)
00216         evecs.col (2) = vec1 / Eigen::internal::sqrt (len1);
00217       else if (len2 >= len1 && len2 >= len3)
00218         evecs.col (2) = vec2 / Eigen::internal::sqrt (len2);
00219       else
00220         evecs.col (2) = vec3 / Eigen::internal::sqrt (len3);
00221     
00222       evecs.col (1) = evecs.col (2).unitOrthogonal ();
00223       evecs.col (0) = evecs.col (1).cross (evecs.col (2));
00224     }
00225     else if ((evals(2)-evals(1))<=Eigen::NumTraits<Scalar>::epsilon() )
00226     {
00227       // second and third equal
00228       Matrix tmp;
00229       tmp = scaledMat;
00230       tmp.diagonal ().array () -= evals (0);
00231 
00232       Vector vec1 = tmp.row (0).cross (tmp.row (1));
00233       Vector vec2 = tmp.row (0).cross (tmp.row (2));
00234       Vector vec3 = tmp.row (1).cross (tmp.row (2));
00235 
00236       Scalar len1 = vec1.squaredNorm();
00237       Scalar len2 = vec2.squaredNorm();
00238       Scalar len3 = vec3.squaredNorm();
00239 
00240       if (len1 >= len2 && len1 >= len3)
00241         evecs.col (0) = vec1 / Eigen::internal::sqrt (len1);
00242       else if (len2 >= len1 && len2 >= len3)
00243         evecs.col (0) = vec2 / Eigen::internal::sqrt (len2);
00244       else
00245         evecs.col (0) = vec3 / Eigen::internal::sqrt (len3);
00246 
00247       evecs.col (1) = evecs.col (0).unitOrthogonal ();
00248       evecs.col (2) = evecs.col (0).cross (evecs.col (1));
00249     }
00250     else
00251     {
00252       Matrix tmp;
00253       tmp = scaledMat;
00254       tmp.diagonal ().array () -= evals (2);
00255 
00256       Vector vec1 = tmp.row (0).cross (tmp.row (1));
00257       Vector vec2 = tmp.row (0).cross (tmp.row (2));
00258       Vector vec3 = tmp.row (1).cross (tmp.row (2));
00259 
00260       Scalar len1 = vec1.squaredNorm ();
00261       Scalar len2 = vec2.squaredNorm ();
00262       Scalar len3 = vec3.squaredNorm ();
00263 #ifdef _WIN32
00264       Scalar *mmax = new Scalar[3];
00265 #else
00266       Scalar mmax[3];
00267 #endif
00268       unsigned int min_el = 2;
00269       unsigned int max_el = 2;
00270       if (len1 >= len2 && len1 >= len3)
00271       {
00272         mmax[2] = len1;
00273         evecs.col (2) = vec1 / Eigen::internal::sqrt (len1);
00274       }
00275       else if (len2 >= len1 && len2 >= len3)
00276       {
00277         mmax[2] = len2;
00278         evecs.col (2) = vec2 / Eigen::internal::sqrt (len2);
00279       }
00280       else
00281       {
00282         mmax[2] = len3;
00283         evecs.col (2) = vec3 / Eigen::internal::sqrt (len3);
00284       }
00285 
00286       tmp = scaledMat;
00287       tmp.diagonal ().array () -= evals (1);
00288 
00289       vec1 = tmp.row (0).cross (tmp.row (1));
00290       vec2 = tmp.row (0).cross (tmp.row (2));
00291       vec3 = tmp.row (1).cross (tmp.row (2));
00292 
00293       len1 = vec1.squaredNorm ();
00294       len2 = vec2.squaredNorm ();
00295       len3 = vec3.squaredNorm ();
00296       if (len1 >= len2 && len1 >= len3)
00297       {
00298         mmax[1] = len1;
00299         evecs.col (1) = vec1 / Eigen::internal::sqrt (len1);
00300         min_el = len1 <= mmax[min_el]? 1: min_el;
00301         max_el = len1 > mmax[max_el]? 1: max_el;
00302       }
00303       else if (len2 >= len1 && len2 >= len3)
00304       {
00305         mmax[1] = len2;
00306         evecs.col (1) = vec2 / Eigen::internal::sqrt (len2);
00307         min_el = len2 <= mmax[min_el]? 1: min_el;
00308         max_el = len2 > mmax[max_el]? 1: max_el;
00309       }
00310       else
00311       {
00312         mmax[1] = len3;
00313         evecs.col (1) = vec3 / Eigen::internal::sqrt (len3);
00314         min_el = len3 <= mmax[min_el]? 1: min_el;
00315         max_el = len3 > mmax[max_el]? 1: max_el;
00316       }
00317       
00318       tmp = scaledMat;
00319       tmp.diagonal ().array () -= evals (0);
00320 
00321       vec1 = tmp.row (0).cross (tmp.row (1));
00322       vec2 = tmp.row (0).cross (tmp.row (2));
00323       vec3 = tmp.row (1).cross (tmp.row (2));
00324 
00325       len1 = vec1.squaredNorm ();
00326       len2 = vec2.squaredNorm ();
00327       len3 = vec3.squaredNorm ();
00328       if (len1 >= len2 && len1 >= len3)
00329       {
00330         mmax[0] = len1;
00331         evecs.col (0) = vec1 / Eigen::internal::sqrt (len1);
00332         min_el = len3 <= mmax[min_el]? 0: min_el;
00333         max_el = len3 > mmax[max_el]? 0: max_el;
00334       }
00335       else if (len2 >= len1 && len2 >= len3)
00336       {
00337         mmax[0] = len2;
00338         evecs.col (0) = vec2 / Eigen::internal::sqrt (len2);
00339         min_el = len3 <= mmax[min_el]? 0: min_el;
00340         max_el = len3 > mmax[max_el]? 0: max_el;    
00341       }
00342       else
00343       {
00344         mmax[0] = len3;
00345         evecs.col (0) = vec3 / Eigen::internal::sqrt (len3);
00346         min_el = len3 <= mmax[min_el]? 0: min_el;
00347         max_el = len3 > mmax[max_el]? 0: max_el;    
00348       }
00349       
00350       unsigned mid_el = 3 - min_el - max_el;
00351       evecs.col (min_el) = evecs.col ((min_el+1)%3).cross ( evecs.col ((min_el+2)%3) ).normalized ();
00352       evecs.col (mid_el) = evecs.col ((mid_el+1)%3).cross ( evecs.col ((mid_el+2)%3) ).normalized ();
00353 #ifdef _WIN32   
00354       delete [] mmax;
00355 #endif
00356     }
00357     // Rescale back to the original size.
00358     evals *= scale;
00359   }
00360 
00368   inline void
00369   getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction,
00370                              Eigen::Affine3f& transformation);
00371   
00379   inline Eigen::Affine3f
00380   getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction);
00381  
00389   inline void
00390   getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction,
00391                              Eigen::Affine3f& transformation);
00392 
00400   inline Eigen::Affine3f
00401   getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction);
00402   
00404   inline void
00405   getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis,
00406                                        Eigen::Affine3f& transformation);
00407   
00409   inline Eigen::Affine3f
00410   getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis);
00411 
00420   inline void
00421   getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis,
00422                                                 const Eigen::Vector3f& origin, Eigen::Affine3f& transformation);
00423 
00431   inline void
00432   getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw);
00433 
00444   inline void
00445   getTranslationAndEulerAngles (const Eigen::Affine3f& t, float& x, float& y, float& z,
00446                                 float& roll, float& pitch, float& yaw);
00447 
00458   inline void
00459   getTransformation (float x, float y, float z, float roll, float pitch, float yaw, Eigen::Affine3f& t);
00460 
00471   inline Eigen::Affine3f
00472   getTransformation (float x, float y, float z, float roll, float pitch, float yaw);
00473   
00479   template <typename Derived> void
00480   saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
00481 
00487   template <typename Derived> void
00488   loadBinary (Eigen::MatrixBase<Derived>& matrix, std::istream& file);
00489 }
00490 
00491 #include "pcl/common/impl/eigen.hpp"
00492 
00493 #endif  //#ifndef PCL_EIGEN_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines