UpperHessenbergEigen.h
Go to the documentation of this file.
1 // The code was adapted from Eigen/src/Eigenvaleus/EigenSolver.h
2 //
3 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
4 // Copyright (C) 2010,2012 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 UPPER_HESSENBERG_EIGEN_H
12 #define UPPER_HESSENBERG_EIGEN_H
13 
14 #include <Eigen/Core>
15 #include <Eigen/Eigenvalues>
16 #include <stdexcept>
17 
18 namespace Spectra {
19 
20 template <typename Scalar = double>
22 {
23 private:
27 
30 
31  typedef std::complex<Scalar> Complex;
34 
35  Index m_n; // Size of the matrix
36  Eigen::RealSchur<Matrix> m_realSchur; // Schur decomposition solver
37  Matrix m_matT; // Schur T matrix
38  Matrix m_eivec; // Storing eigenvectors
39  ComplexVector m_eivalues; // Eigenvalues
40 
41  bool m_computed;
42 
44  {
45  using std::abs;
46 
47  const Index size = m_eivec.cols();
49 
50  // inefficient! this is already computed in RealSchur
51  Scalar norm(0);
52  for (Index j = 0; j < size; ++j)
53  {
54  norm += m_matT.row(j).segment((std::max)(j - 1, Index(0)), size - (std::max)(j - 1, Index(0))).cwiseAbs().sum();
55  }
56 
57  // Backsubstitute to find vectors of upper triangular form
58  if (norm == Scalar(0))
59  return;
60 
61  for (Index n = size - 1; n >= 0; n--)
62  {
63  Scalar p = m_eivalues.coeff(n).real();
64  Scalar q = m_eivalues.coeff(n).imag();
65 
66  // Scalar vector
67  if (q == Scalar(0))
68  {
69  Scalar lastr(0), lastw(0);
70  Index l = n;
71 
72  m_matT.coeffRef(n, n) = Scalar(1);
73  for (Index i = n - 1; i >= 0; i--)
74  {
75  Scalar w = m_matT.coeff(i, i) - p;
76  Scalar r = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n).segment(l, n - l + 1));
77 
78  if (m_eivalues.coeff(i).imag() < Scalar(0))
79  {
80  lastw = w;
81  lastr = r;
82  }
83  else
84  {
85  l = i;
86  if (m_eivalues.coeff(i).imag() == Scalar(0))
87  {
88  if (w != Scalar(0))
89  m_matT.coeffRef(i, n) = -r / w;
90  else
91  m_matT.coeffRef(i, n) = -r / (eps * norm);
92  }
93  else // Solve real equations
94  {
95  Scalar x = m_matT.coeff(i, i + 1);
96  Scalar y = m_matT.coeff(i + 1, i);
97  Scalar denom = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag();
98  Scalar t = (x * lastr - lastw * r) / denom;
99  m_matT.coeffRef(i, n) = t;
100  if (abs(x) > abs(lastw))
101  m_matT.coeffRef(i + 1, n) = (-r - w * t) / x;
102  else
103  m_matT.coeffRef(i + 1, n) = (-lastr - y * t) / lastw;
104  }
105 
106  // Overflow control
107  Scalar t = abs(m_matT.coeff(i, n));
108  if ((eps * t) * t > Scalar(1))
109  m_matT.col(n).tail(size - i) /= t;
110  }
111  }
112  }
113  else if (q < Scalar(0) && n > 0)
114  { // Complex vector
115  Scalar lastra(0), lastsa(0), lastw(0);
116  Index l = n - 1;
117 
118  // Last vector component imaginary so matrix is triangular
119  if (abs(m_matT.coeff(n, n - 1)) > abs(m_matT.coeff(n - 1, n)))
120  {
121  m_matT.coeffRef(n - 1, n - 1) = q / m_matT.coeff(n, n - 1);
122  m_matT.coeffRef(n - 1, n) = -(m_matT.coeff(n, n) - p) / m_matT.coeff(n, n - 1);
123  }
124  else
125  {
126  Complex cc = Complex(Scalar(0), -m_matT.coeff(n - 1, n)) / Complex(m_matT.coeff(n - 1, n - 1) - p, q);
127  m_matT.coeffRef(n - 1, n - 1) = Eigen::numext::real(cc);
128  m_matT.coeffRef(n - 1, n) = Eigen::numext::imag(cc);
129  }
130  m_matT.coeffRef(n, n - 1) = Scalar(0);
131  m_matT.coeffRef(n, n) = Scalar(1);
132  for (Index i = n - 2; i >= 0; i--)
133  {
134  Scalar ra = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n - 1).segment(l, n - l + 1));
135  Scalar sa = m_matT.row(i).segment(l, n - l + 1).dot(m_matT.col(n).segment(l, n - l + 1));
136  Scalar w = m_matT.coeff(i, i) - p;
137 
138  if (m_eivalues.coeff(i).imag() < Scalar(0))
139  {
140  lastw = w;
141  lastra = ra;
142  lastsa = sa;
143  }
144  else
145  {
146  l = i;
147  if (m_eivalues.coeff(i).imag() == Scalar(0))
148  {
149  Complex cc = Complex(-ra, -sa) / Complex(w, q);
150  m_matT.coeffRef(i, n - 1) = Eigen::numext::real(cc);
151  m_matT.coeffRef(i, n) = Eigen::numext::imag(cc);
152  }
153  else
154  {
155  // Solve complex equations
156  Scalar x = m_matT.coeff(i, i + 1);
157  Scalar y = m_matT.coeff(i + 1, i);
158  Scalar vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q;
159  Scalar vi = (m_eivalues.coeff(i).real() - p) * Scalar(2) * q;
160  if ((vr == Scalar(0)) && (vi == Scalar(0)))
161  vr = eps * norm * (abs(w) + abs(q) + abs(x) + abs(y) + abs(lastw));
162 
163  Complex cc = Complex(x * lastra - lastw * ra + q * sa, x * lastsa - lastw * sa - q * ra) / Complex(vr, vi);
164  m_matT.coeffRef(i, n - 1) = Eigen::numext::real(cc);
165  m_matT.coeffRef(i, n) = Eigen::numext::imag(cc);
166  if (abs(x) > (abs(lastw) + abs(q)))
167  {
168  m_matT.coeffRef(i + 1, n - 1) = (-ra - w * m_matT.coeff(i, n - 1) + q * m_matT.coeff(i, n)) / x;
169  m_matT.coeffRef(i + 1, n) = (-sa - w * m_matT.coeff(i, n) - q * m_matT.coeff(i, n - 1)) / x;
170  }
171  else
172  {
173  cc = Complex(-lastra - y * m_matT.coeff(i, n - 1), -lastsa - y * m_matT.coeff(i, n)) / Complex(lastw, q);
174  m_matT.coeffRef(i + 1, n - 1) = Eigen::numext::real(cc);
175  m_matT.coeffRef(i + 1, n) = Eigen::numext::imag(cc);
176  }
177  }
178 
179  // Overflow control
180  Scalar t = std::max(abs(m_matT.coeff(i, n - 1)), abs(m_matT.coeff(i, n)));
181  if ((eps * t) * t > Scalar(1))
182  m_matT.block(i, n - 1, size - i, 2) /= t;
183  }
184  }
185 
186  // We handled a pair of complex conjugate eigenvalues, so need to skip them both
187  n--;
188  }
189  }
190 
191  // Back transformation to get eigenvectors of original matrix
192  Vector m_tmp(size);
193  for (Index j = size - 1; j >= 0; j--)
194  {
195  m_tmp.noalias() = m_eivec.leftCols(j + 1) * m_matT.col(j).segment(0, j + 1);
196  m_eivec.col(j) = m_tmp;
197  }
198  }
199 
200 public:
202  m_n(0), m_computed(false)
203  {}
204 
205  UpperHessenbergEigen(ConstGenericMatrix& mat) :
206  m_n(mat.rows()), m_computed(false)
207  {
208  compute(mat);
209  }
210 
211  void compute(ConstGenericMatrix& mat)
212  {
213  using std::abs;
214  using std::sqrt;
215 
216  if (mat.rows() != mat.cols())
217  throw std::invalid_argument("UpperHessenbergEigen: matrix must be square");
218 
219  m_n = mat.rows();
220  // Scale matrix prior to the Schur decomposition
221  const Scalar scale = mat.cwiseAbs().maxCoeff();
222 
223  // Reduce to real Schur form
224  Matrix Q = Matrix::Identity(m_n, m_n);
225  m_realSchur.computeFromHessenberg(mat / scale, Q, true);
226  if (m_realSchur.info() != Eigen::Success)
227  throw std::runtime_error("UpperHessenbergEigen: eigen decomposition failed");
228 
229  m_matT = m_realSchur.matrixT();
230  m_eivec = m_realSchur.matrixU();
231 
232  // Compute eigenvalues from matT
233  m_eivalues.resize(m_n);
234  Index i = 0;
235  while (i < m_n)
236  {
237  // Real eigenvalue
238  if (i == m_n - 1 || m_matT.coeff(i + 1, i) == Scalar(0))
239  {
240  m_eivalues.coeffRef(i) = m_matT.coeff(i, i);
241  ++i;
242  }
243  else // Complex eigenvalues
244  {
245  Scalar p = Scalar(0.5) * (m_matT.coeff(i, i) - m_matT.coeff(i + 1, i + 1));
246  Scalar z;
247  // Compute z = sqrt(abs(p * p + m_matT.coeff(i+1, i) * m_matT.coeff(i, i+1)));
248  // without overflow
249  {
250  Scalar t0 = m_matT.coeff(i + 1, i);
251  Scalar t1 = m_matT.coeff(i, i + 1);
252  Scalar maxval = std::max(abs(p), std::max(abs(t0), abs(t1)));
253  t0 /= maxval;
254  t1 /= maxval;
255  Scalar p0 = p / maxval;
256  z = maxval * sqrt(abs(p0 * p0 + t0 * t1));
257  }
258  m_eivalues.coeffRef(i) = Complex(m_matT.coeff(i + 1, i + 1) + p, z);
259  m_eivalues.coeffRef(i + 1) = Complex(m_matT.coeff(i + 1, i + 1) + p, -z);
260  i += 2;
261  }
262  }
263 
264  // Compute eigenvectors
266 
267  // Scale eigenvalues back
268  m_eivalues *= scale;
269 
270  m_computed = true;
271  }
272 
273  const ComplexVector& eigenvalues() const
274  {
275  if (!m_computed)
276  throw std::logic_error("UpperHessenbergEigen: need to call compute() first");
277 
278  return m_eivalues;
279  }
280 
281  ComplexMatrix eigenvectors()
282  {
283  using std::abs;
284 
285  if (!m_computed)
286  throw std::logic_error("UpperHessenbergEigen: need to call compute() first");
287 
288  Index n = m_eivec.cols();
289  ComplexMatrix matV(n, n);
290  for (Index j = 0; j < n; ++j)
291  {
292  // imaginary part of real eigenvalue is already set to exact zero
293  if (Eigen::numext::imag(m_eivalues.coeff(j)) == Scalar(0) || j + 1 == n)
294  {
295  // we have a real eigen value
296  matV.col(j) = m_eivec.col(j).template cast<Complex>();
297  matV.col(j).normalize();
298  }
299  else
300  {
301  // we have a pair of complex eigen values
302  for (Index i = 0; i < n; ++i)
303  {
304  matV.coeffRef(i, j) = Complex(m_eivec.coeff(i, j), m_eivec.coeff(i, j + 1));
305  matV.coeffRef(i, j + 1) = Complex(m_eivec.coeff(i, j), -m_eivec.coeff(i, j + 1));
306  }
307  matV.col(j).normalize();
308  matV.col(j + 1).normalize();
309  ++j;
310  }
311  }
312 
313  return matV;
314  }
315 };
316 
317 } // namespace Spectra
318 
319 #endif // UPPER_HESSENBERG_EIGEN_H
SCALAR Scalar
Definition: bench_gemm.cpp:46
#define max(a, b)
Definition: datatypes.h:20
float real
Definition: datatypes.h:10
Scalar * y
Performs a real Schur decomposition of a square matrix.
Definition: RealSchur.h:54
void compute(ConstGenericMatrix &mat)
RealSchur & computeFromHessenberg(const HessMatrixType &matrixH, const OrthMatrixType &matrixQ, bool computeU)
Computes Schur decomposition of a Hessenberg matrix H = Z T Z^T.
int n
Vector3f p0
const Eigen::Ref< const Matrix > ConstGenericMatrix
Eigen::Ref< Matrix > GenericMatrix
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Eigen::Matrix< Complex, Eigen::Dynamic, Eigen::Dynamic > ComplexMatrix
static double epsilon
Definition: testRot3.cpp:37
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
static const Line3 l(Rot3(), 1, 1)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
std::complex< Scalar > Complex
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar & coeff(Index rowId, Index colId) const
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
UpperHessenbergEigen(ConstGenericMatrix &mat)
EIGEN_DEVICE_FUNC const Scalar & q
RowVector3d w
const MatrixType & matrixT() const
Returns the quasi-triangular matrix in the Schur decomposition.
Definition: RealSchur.h:144
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Eigen::Matrix< Complex, Eigen::Dynamic, 1 > ComplexVector
Eigen::RealSchur< Matrix > m_realSchur
const MatrixType & matrixU() const
Returns the orthogonal matrix in the Schur decomposition.
Definition: RealSchur.h:127
The quaternion class used to represent 3D orientations and rotations.
float * p
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
EIGEN_DEVICE_FUNC const ImagReturnType imag() const
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
ComputationInfo info() const
Reports whether previous computation was successful.
Definition: RealSchur.h:195
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
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
const ComplexVector & eigenvalues() const
std::ptrdiff_t j
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43