ground/gcs/src/libs/eigen/bench/eig33.cpp

   1 // This file is part of Eigen, a lightweight C++ template library
   2 // for linear algebra.
   3 //
   4 // Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
   5 //
   6 // This Source Code Form is subject to the terms of the Mozilla
   7 // Public License v. 2.0. If a copy of the MPL was not distributed
   8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
   9
  10 // The computeRoots function included in this is based on materials
  11 // covered by the following copyright and license:
  12 //
  13 // Geometric Tools, LLC
  14 // Copyright (c) 1998-2010
  15 // Distributed under the Boost Software License, Version 1.0.
  16 //
  17 // Permission is hereby granted, free of charge, to any person or organization
  18 // obtaining a copy of the software and accompanying documentation covered by
  19 // this license (the "Software") to use, reproduce, display, distribute,
  20 // execute, and transmit the Software, and to prepare derivative works of the
  21 // Software, and to permit third-parties to whom the Software is furnished to
  22 // do so, all subject to the following:
  23 //
  24 // The copyright notices in the Software and this entire statement, including
  25 // the above license grant, this restriction and the following disclaimer,
  26 // must be included in all copies of the Software, in whole or in part, and
  27 // all derivative works of the Software, unless such copies or derivative
  28 // works are solely in the form of machine-executable object code generated by
  29 // a source language processor.
  30 //
  31 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  32 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  33 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
  34 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
  35 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
  36 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  37 // DEALINGS IN THE SOFTWARE.
  38
  39 #include <iostream>
  40 #include <Eigen/Core>
  41 #include <Eigen/Eigenvalues>
  42 #include <Eigen/Geometry>
  43 #include <bench/BenchTimer.h>
  44
  45 using namespace Eigen;
  46 using namespace std;
  47
  48 template<typename Matrix, typename Roots>
  49 inline void computeRoots(const Matrix& m, Roots& roots)
  50 {
  51   typedef typename Matrix::Scalar Scalar;
  52   const Scalar s_inv3 = 1.0/3.0;
  53   const Scalar s_sqrt3 = std::sqrt(Scalar(3.0));
  54
  55   // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
  56   // eigenvalues are the roots to this equation, all guaranteed to be
  57   // real-valued, because the matrix is symmetric.
  58   Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(0,1)*m(0,2)*m(1,2) - m(0,0)*m(1,2)*m(1,2) - m(1,1)*m(0,2)*m(0,2) - m(2,2)*m(0,1)*m(0,1);
  59   Scalar c1 = m(0,0)*m(1,1) - m(0,1)*m(0,1) + m(0,0)*m(2,2) - m(0,2)*m(0,2) + m(1,1)*m(2,2) - m(1,2)*m(1,2);
  60   Scalar c2 = m(0,0) + m(1,1) + m(2,2);
  61
  62   // Construct the parameters used in classifying the roots of the equation
  63   // and in solving the equation for the roots in closed form.
  64   Scalar c2_over_3 = c2*s_inv3;
  65   Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
  66   if (a_over_3 > Scalar(0))
  67     a_over_3 = Scalar(0);
  68
  69   Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
  70
  71   Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
  72   if (q > Scalar(0))
  73     q = Scalar(0);
  74
  75   // Compute the eigenvalues by solving for the roots of the polynomial.
  76   Scalar rho = std::sqrt(-a_over_3);
  77   Scalar theta = std::atan2(std::sqrt(-q),half_b)*s_inv3;
  78   Scalar cos_theta = std::cos(theta);
  79   Scalar sin_theta = std::sin(theta);
  80   roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta;
  81   roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
  82   roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
  83 }
  84
  85 template<typename Matrix, typename Vector>
  86 void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals)
  87 {
  88   typedef typename Matrix::Scalar Scalar;
  89   // Scale the matrix so its entries are in [-1,1].  The scaling is applied
  90   // only when at least one matrix entry has magnitude larger than 1.
  91
  92   Scalar shift = mat.trace()/3;
  93   Matrix scaledMat = mat;
  94   scaledMat.diagonal().array() -= shift;
  95   Scalar scale = scaledMat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff();
  96   scale = std::max(scale,Scalar(1));
  97   scaledMat/=scale;
  98
  99   // Compute the eigenvalues
 100 //   scaledMat.setZero();
 101   computeRoots(scaledMat,evals);
 102
 103   // compute the eigen vectors
 104   // **here we assume 3 differents eigenvalues**
 105
 106   // "optimized version" which appears to be slower with gcc!
 107 //     Vector base;
 108 //     Scalar alpha, beta;
 109 //     base <<   scaledMat(1,0) * scaledMat(2,1),
 110 //               scaledMat(1,0) * scaledMat(2,0),
 111 //              -scaledMat(1,0) * scaledMat(1,0);
 112 //     for(int k=0; k<2; ++k)
 113 //     {
 114 //       alpha = scaledMat(0,0) - evals(k);
 115 //       beta  = scaledMat(1,1) - evals(k);
 116 //       evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized();
 117 //     }
 118 //     evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized();
 119
 120 //   // naive version
 121 //   Matrix tmp;
 122 //   tmp = scaledMat;
 123 //   tmp.diagonal().array() -= evals(0);
 124 //   evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized();
 125 //
 126 //   tmp = scaledMat;
 127 //   tmp.diagonal().array() -= evals(1);
 128 //   evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized();
 129 //
 130 //   tmp = scaledMat;
 131 //   tmp.diagonal().array() -= evals(2);
 132 //   evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized();
 133
 134   // a more stable version:
 135   if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
 136   {
 137     evecs.setIdentity();
 138   }
 139   else
 140   {
 141     Matrix tmp;
 142     tmp = scaledMat;
 143     tmp.diagonal ().array () -= evals (2);
 144     evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();
 145
 146     tmp = scaledMat;
 147     tmp.diagonal ().array () -= evals (1);
 148     evecs.col(1) = tmp.row (0).cross(tmp.row (1));
 149     Scalar n1 = evecs.col(1).norm();
 150     if(n1<=Eigen::NumTraits<Scalar>::epsilon())
 151       evecs.col(1) = evecs.col(2).unitOrthogonal();
 152     else
 153       evecs.col(1) /= n1;
 154
 155     // make sure that evecs[1] is orthogonal to evecs[2]
 156     evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized();
 157     evecs.col(0) = evecs.col(2).cross(evecs.col(1));
 158   }
 159
 160   // Rescale back to the original size.
 161   evals *= scale;
 162   evals.array()+=shift;
 163 }
 164
 165 int main()
 166 {
 167   BenchTimer t;
 168   int tries = 10;
 169   int rep = 400000;
 170   typedef Matrix3d Mat;
 171   typedef Vector3d Vec;
 172   Mat A = Mat::Random(3,3);
 173   A = A.adjoint() * A;
 174 //   Mat Q = A.householderQr().householderQ();
 175 //   A = Q * Vec(2.2424567,2.2424566,7.454353).asDiagonal() * Q.transpose();
 176
 177   SelfAdjointEigenSolver<Mat> eig(A);
 178   BENCH(t, tries, rep, eig.compute(A));
 179   std::cout << "Eigen iterative:  " << t.best() << "s\n";
 180
 181   BENCH(t, tries, rep, eig.computeDirect(A));
 182   std::cout << "Eigen direct   :  " << t.best() << "s\n";
 183
 184   Mat evecs;
 185   Vec evals;
 186   BENCH(t, tries, rep, eigen33(A,evecs,evals));
 187   std::cout << "Direct: " << t.best() << "s\n\n";
 188
 189 //   std::cerr << "Eigenvalue/eigenvector diffs:\n";
 190 //   std::cerr << (evals - eig.eigenvalues()).transpose() << "\n";
 191 //   for(int k=0;k<3;++k)
 192 //     if(evecs.col(k).dot(eig.eigenvectors().col(k))<0)
 193 //       evecs.col(k) = -evecs.col(k);
 194 //   std::cerr << evecs - eig.eigenvectors() << "\n\n";
 195 }