TridiagEigen.h
Go to the documentation of this file.
1 // The code was adapted from Eigen/src/Eigenvaleus/SelfAdjointEigenSolver.h
2 //
3 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
4 // Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
5 // Copyright (C) 2016-2019 Yixuan Qiu <yixuan.qiu@cos.name>
6 //
7 // This Source Code Form is subject to the terms of the Mozilla
8 // Public License v. 2.0. If a copy of the MPL was not distributed
9 // with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
10 
11 #ifndef TRIDIAG_EIGEN_H
12 #define TRIDIAG_EIGEN_H
13 
14 #include <Eigen/Core>
15 #include <Eigen/Jacobi>
16 #include <stdexcept>
17 
18 #include "../Util/TypeTraits.h"
19 
20 namespace Spectra {
21 
22 template <typename Scalar = double>
24 {
25 private:
27  // For convenience in adapting the tridiagonal_qr_step() function
28  typedef Scalar RealScalar;
29 
32 
35 
36  Index m_n;
37  Vector m_main_diag; // Main diagonal elements of the matrix
38  Vector m_sub_diag; // Sub-diagonal elements of the matrix
39  Matrix m_evecs; // To store eigenvectors
40 
41  bool m_computed;
42  const Scalar m_near_0; // a very small value, ~= 1e-307 for the "double" type
43 
44  // Adapted from Eigen/src/Eigenvaleus/SelfAdjointEigenSolver.h
45  static void tridiagonal_qr_step(RealScalar* diag,
46  RealScalar* subdiag, Index start,
47  Index end, Scalar* matrixQ,
48  Index n)
49  {
50  using std::abs;
51 
52  RealScalar td = (diag[end - 1] - diag[end]) * RealScalar(0.5);
53  RealScalar e = subdiag[end - 1];
54  // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still
55  // underflow thus leading to inf/NaN values when using the following commented code:
56  // RealScalar e2 = numext::abs2(subdiag[end-1]);
57  // RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
58  // This explain the following, somewhat more complicated, version:
59  RealScalar mu = diag[end];
60  if (td == Scalar(0))
61  mu -= abs(e);
62  else
63  {
64  RealScalar e2 = Eigen::numext::abs2(subdiag[end - 1]);
65  RealScalar h = Eigen::numext::hypot(td, e);
66  if (e2 == RealScalar(0))
67  mu -= (e / (td + (td > RealScalar(0) ? RealScalar(1) : RealScalar(-1)))) * (e / h);
68  else
69  mu -= e2 / (td + (td > RealScalar(0) ? h : -h));
70  }
71 
72  RealScalar x = diag[start] - mu;
73  RealScalar z = subdiag[start];
74  Eigen::Map<Matrix> q(matrixQ, n, n);
75  for (Index k = start; k < end; ++k)
76  {
78  rot.makeGivens(x, z);
79 
80  const RealScalar s = rot.s();
81  const RealScalar c = rot.c();
82 
83  // do T = G' T G
84  RealScalar sdk = s * diag[k] + c * subdiag[k];
85  RealScalar dkp1 = s * subdiag[k] + c * diag[k + 1];
86 
87  diag[k] = c * (c * diag[k] - s * subdiag[k]) - s * (c * subdiag[k] - s * diag[k + 1]);
88  diag[k + 1] = s * sdk + c * dkp1;
89  subdiag[k] = c * sdk - s * dkp1;
90 
91  if (k > start)
92  subdiag[k - 1] = c * subdiag[k - 1] - s * z;
93 
94  x = subdiag[k];
95 
96  if (k < end - 1)
97  {
98  z = -s * subdiag[k + 1];
99  subdiag[k + 1] = c * subdiag[k + 1];
100  }
101 
102  // apply the givens rotation to the unit matrix Q = Q * G
103  if (matrixQ)
104  q.applyOnTheRight(k, k + 1, rot);
105  }
106  }
107 
108 public:
110  m_n(0), m_computed(false),
111  m_near_0(TypeTraits<Scalar>::min() * Scalar(10))
112  {}
113 
114  TridiagEigen(ConstGenericMatrix& mat) :
115  m_n(mat.rows()), m_computed(false),
116  m_near_0(TypeTraits<Scalar>::min() * Scalar(10))
117  {
118  compute(mat);
119  }
120 
121  void compute(ConstGenericMatrix& mat)
122  {
123  using std::abs;
124 
125  m_n = mat.rows();
126  if (m_n != mat.cols())
127  throw std::invalid_argument("TridiagEigen: matrix must be square");
128 
129  m_main_diag.resize(m_n);
130  m_sub_diag.resize(m_n - 1);
131  m_evecs.resize(m_n, m_n);
132  m_evecs.setIdentity();
133 
134  // Scale matrix to improve stability
135  const Scalar scale = std::max(mat.diagonal().cwiseAbs().maxCoeff(),
136  mat.diagonal(-1).cwiseAbs().maxCoeff());
137  // If scale=0, mat is a zero matrix, so we can early stop
138  if (scale < m_near_0)
139  {
140  // m_main_diag contains eigenvalues
141  m_main_diag.setZero();
142  // m_evecs has been set identity
143  // m_evecs.setIdentity();
144  m_computed = true;
145  return;
146  }
147  m_main_diag.noalias() = mat.diagonal() / scale;
148  m_sub_diag.noalias() = mat.diagonal(-1) / scale;
149 
150  Scalar* diag = m_main_diag.data();
151  Scalar* subdiag = m_sub_diag.data();
152 
153  Index end = m_n - 1;
154  Index start = 0;
155  Index iter = 0; // total number of iterations
156  int info = 0; // 0 for success, 1 for failure
157 
158  const Scalar considerAsZero = TypeTraits<Scalar>::min();
160 
161  while (end > 0)
162  {
163  for (Index i = start; i < end; i++)
164  if (abs(subdiag[i]) <= considerAsZero ||
165  abs(subdiag[i]) <= (abs(diag[i]) + abs(diag[i + 1])) * precision)
166  subdiag[i] = 0;
167 
168  // find the largest unreduced block
169  while (end > 0 && subdiag[end - 1] == Scalar(0))
170  end--;
171 
172  if (end <= 0)
173  break;
174 
175  // if we spent too many iterations, we give up
176  iter++;
177  if (iter > 30 * m_n)
178  {
179  info = 1;
180  break;
181  }
182 
183  start = end - 1;
184  while (start > 0 && subdiag[start - 1] != Scalar(0))
185  start--;
186 
187  tridiagonal_qr_step(diag, subdiag, start, end, m_evecs.data(), m_n);
188  }
189 
190  if (info > 0)
191  throw std::runtime_error("TridiagEigen: eigen decomposition failed");
192 
193  // Scale eigenvalues back
194  m_main_diag *= scale;
195 
196  m_computed = true;
197  }
198 
199  const Vector& eigenvalues() const
200  {
201  if (!m_computed)
202  throw std::logic_error("TridiagEigen: need to call compute() first");
203 
204  // After calling compute(), main_diag will contain the eigenvalues.
205  return m_main_diag;
206  }
207 
208  const Matrix& eigenvectors() const
209  {
210  if (!m_computed)
211  throw std::logic_error("TridiagEigen: need to call compute() first");
212 
213  return m_evecs;
214  }
215 };
216 
217 } // namespace Spectra
218 
219 #endif // TRIDIAG_EIGEN_H
Scalar & c()
Definition: Jacobi.h:45
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
SCALAR Scalar
Definition: bench_gemm.cpp:33
#define max(a, b)
Definition: datatypes.h:20
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
const Vector & eigenvalues() const
Definition: TridiagEigen.h:199
const Matrix & eigenvectors() const
Definition: TridiagEigen.h:208
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
#define min(a, b)
Definition: datatypes.h:19
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
double mu
Eigen::Ref< Matrix > GenericMatrix
Definition: TridiagEigen.h:33
void compute(ConstGenericMatrix &mat)
Definition: TridiagEigen.h:121
int n
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
Rotation given by a cosine-sine pair.
iterator iter(handle obj)
Definition: pytypes.h:1547
const Eigen::Ref< const Matrix > ConstGenericMatrix
Definition: TridiagEigen.h:34
Eigen::Index Index
Definition: TridiagEigen.h:26
static double epsilon
Definition: testRot3.cpp:39
else if n * info
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: TridiagEigen.h:31
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:33
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
const Scalar m_near_0
Definition: TridiagEigen.h:42
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Abs2ReturnType abs2() const
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
const double h
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: TridiagEigen.h:30
TridiagEigen(ConstGenericMatrix &mat)
Definition: TridiagEigen.h:114
const mpreal hypot(const mpreal &x, const mpreal &y, mp_rnd_t rnd_mode=mpreal::get_default_rnd())
Definition: mpreal.h:2280
cout precision(2)
static void tridiagonal_qr_step(RealScalar *diag, RealScalar *subdiag, Index start, Index end, Scalar *matrixQ, Index n)
Definition: TridiagEigen.h:45
Scalar & s()
Definition: Jacobi.h:47
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
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 x
#define abs(x)
Definition: datatypes.h:17
void makeGivens(const Scalar &p, const Scalar &q, Scalar *r=0)
Definition: Jacobi.h:148


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:51:16