eig33.cpp
Go to the documentation of this file.
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 different 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();
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 
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 }
Matrix3f m
EIGEN_DEVICE_FUNC SelfAdjointEigenSolver & computeDirect(const MatrixType &matrix, int options=ComputeEigenvectors)
Computes eigendecomposition of given matrix using a closed-form algorithm.
SCALAR Scalar
Definition: bench_gemm.cpp:46
internal::traits< Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > >::Scalar Scalar
#define max(a, b)
Definition: datatypes.h:20
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
EIGEN_DEVICE_FUNC SelfAdjointEigenSolver & compute(const EigenBase< InputType > &matrix, int options=ComputeEigenvectors)
Computes eigendecomposition of given matrix.
Computes eigenvalues and eigenvectors of selfadjoint matrices.
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
void eigen33(const Matrix &mat, Matrix &evecs, Vector &evals)
Definition: eig33.cpp:86
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Definition: BFloat16.h:88
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
int main()
Definition: eig33.cpp:165
double best(int TIMER=CPU_TIMER) const
Definition: BenchTimer.h:111
SelfAdjointEigenSolver< PlainMatrixType > eig(mat, computeVectors?ComputeEigenvectors:EigenvaluesOnly)
EIGEN_DEVICE_FUNC const Scalar & q
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Matrix< Scalar, Dynamic, 1 > Vec
Definition: gemv_common.h:17
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
Matrix< Scalar, Dynamic, Dynamic > Mat
Definition: gemm_common.h:15
void computeRoots(const Matrix &m, Roots &roots)
Definition: eig33.cpp:49
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
#define BENCH(TIMER, TRIES, REP, CODE)
Definition: BenchTimer.h:174
The matrix class, also used for vectors and row-vectors.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:11