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>
computeRoots(const Matrix & m,Roots & 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>
eigen33(const Matrix & mat,Matrix & evecs,Vector & evals)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
main()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 }
196