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 (at) 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 = internal::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 = internal::sqrt(-a_over_3); 77 Scalar theta = std::atan2(internal::sqrt(-q),half_b)*s_inv3; 78 Scalar cos_theta = internal::cos(theta); 79 Scalar sin_theta = internal::sin(theta); 80 roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta; 81 roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); 82 roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); 83 84 // Sort in increasing order. 85 if (roots(0) >= roots(1)) 86 std::swap(roots(0),roots(1)); 87 if (roots(1) >= roots(2)) 88 { 89 std::swap(roots(1),roots(2)); 90 if (roots(0) >= roots(1)) 91 std::swap(roots(0),roots(1)); 92 } 93 } 94 95 template<typename Matrix, typename Vector> 96 void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals) 97 { 98 typedef typename Matrix::Scalar Scalar; 99 // Scale the matrix so its entries are in [-1,1]. The scaling is applied 100 // only when at least one matrix entry has magnitude larger than 1. 101 102 Scalar scale = mat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff(); 103 scale = std::max(scale,Scalar(1)); 104 Matrix scaledMat = mat / scale; 105 106 // Compute the eigenvalues 107 // scaledMat.setZero(); 108 computeRoots(scaledMat,evals); 109 110 // compute the eigen vectors 111 // **here we assume 3 differents eigenvalues** 112 113 // "optimized version" which appears to be slower with gcc! 114 // Vector base; 115 // Scalar alpha, beta; 116 // base << scaledMat(1,0) * scaledMat(2,1), 117 // scaledMat(1,0) * scaledMat(2,0), 118 // -scaledMat(1,0) * scaledMat(1,0); 119 // for(int k=0; k<2; ++k) 120 // { 121 // alpha = scaledMat(0,0) - evals(k); 122 // beta = scaledMat(1,1) - evals(k); 123 // evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized(); 124 // } 125 // evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized(); 126 127 // // naive version 128 // Matrix tmp; 129 // tmp = scaledMat; 130 // tmp.diagonal().array() -= evals(0); 131 // evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized(); 132 // 133 // tmp = scaledMat; 134 // tmp.diagonal().array() -= evals(1); 135 // evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized(); 136 // 137 // tmp = scaledMat; 138 // tmp.diagonal().array() -= evals(2); 139 // evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized(); 140 141 // a more stable version: 142 if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon()) 143 { 144 evecs.setIdentity(); 145 } 146 else 147 { 148 Matrix tmp; 149 tmp = scaledMat; 150 tmp.diagonal ().array () -= evals (2); 151 evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized (); 152 153 tmp = scaledMat; 154 tmp.diagonal ().array () -= evals (1); 155 evecs.col(1) = tmp.row (0).cross(tmp.row (1)); 156 Scalar n1 = evecs.col(1).norm(); 157 if(n1<=Eigen::NumTraits<Scalar>::epsilon()) 158 evecs.col(1) = evecs.col(2).unitOrthogonal(); 159 else 160 evecs.col(1) /= n1; 161 162 // make sure that evecs[1] is orthogonal to evecs[2] 163 evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized(); 164 evecs.col(0) = evecs.col(2).cross(evecs.col(1)); 165 } 166 167 // Rescale back to the original size. 168 evals *= scale; 169 } 170 171 int main() 172 { 173 BenchTimer t; 174 int tries = 10; 175 int rep = 400000; 176 typedef Matrix3f Mat; 177 typedef Vector3f Vec; 178 Mat A = Mat::Random(3,3); 179 A = A.adjoint() * A; 180 181 SelfAdjointEigenSolver<Mat> eig(A); 182 BENCH(t, tries, rep, eig.compute(A)); 183 std::cout << "Eigen: " << t.best() << "s\n"; 184 185 Mat evecs; 186 Vec evals; 187 BENCH(t, tries, rep, eigen33(A,evecs,evals)); 188 std::cout << "Direct: " << t.best() << "s\n\n"; 189 190 std::cerr << "Eigenvalue/eigenvector diffs:\n"; 191 std::cerr << (evals - eig.eigenvalues()).transpose() << "\n"; 192 for(int k=0;k<3;++k) 193 if(evecs.col(k).dot(eig.eigenvectors().col(k))<0) 194 evecs.col(k) = -evecs.col(k); 195 std::cerr << evecs - eig.eigenvectors() << "\n\n"; 196 } 197
