SelfAdjointEigenSolver.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
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 http://mozilla.org/MPL/2.0/.
10 
11 #ifndef EIGEN_SELFADJOINTEIGENSOLVER_H
12 #define EIGEN_SELFADJOINTEIGENSOLVER_H
13 
14 #include "./Tridiagonalization.h"
15 
16 namespace Eigen {
17 
18 template<typename _MatrixType>
19 class GeneralizedSelfAdjointEigenSolver;
20 
21 namespace internal {
22 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues;
23 }
24 
68 template<typename _MatrixType> class SelfAdjointEigenSolver
69 {
70  public:
71 
72  typedef _MatrixType MatrixType;
73  enum {
74  Size = MatrixType::RowsAtCompileTime,
75  ColsAtCompileTime = MatrixType::ColsAtCompileTime,
76  Options = MatrixType::Options,
77  MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
78  };
79 
81  typedef typename MatrixType::Scalar Scalar;
82  typedef typename MatrixType::Index Index;
83 
91 
92  friend struct internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>;
93 
99  typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVectorType;
100  typedef Tridiagonalization<MatrixType> TridiagonalizationType;
101 
113  : m_eivec(),
114  m_eivalues(),
115  m_subdiag(),
116  m_isInitialized(false)
117  { }
118 
132  : m_eivec(size, size),
133  m_eivalues(size),
134  m_subdiag(size > 1 ? size - 1 : 1),
135  m_isInitialized(false)
136  {}
137 
153  SelfAdjointEigenSolver(const MatrixType& matrix, int options = ComputeEigenvectors)
154  : m_eivec(matrix.rows(), matrix.cols()),
155  m_eivalues(matrix.cols()),
156  m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
157  m_isInitialized(false)
158  {
159  compute(matrix, options);
160  }
161 
192  SelfAdjointEigenSolver& compute(const MatrixType& matrix, int options = ComputeEigenvectors);
193 
208  SelfAdjointEigenSolver& computeDirect(const MatrixType& matrix, int options = ComputeEigenvectors);
209 
228  const MatrixType& eigenvectors() const
229  {
230  eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
231  eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
232  return m_eivec;
233  }
234 
251  {
252  eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
253  return m_eivalues;
254  }
255 
274  MatrixType operatorSqrt() const
275  {
276  eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
277  eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
278  return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint();
279  }
280 
299  MatrixType operatorInverseSqrt() const
300  {
301  eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
302  eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
303  return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint();
304  }
305 
311  {
312  eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
313  return m_info;
314  }
315 
321  static const int m_maxIterations = 30;
322 
323  #ifdef EIGEN2_SUPPORT
324  SelfAdjointEigenSolver(const MatrixType& matrix, bool computeEigenvectors)
325  : m_eivec(matrix.rows(), matrix.cols()),
326  m_eivalues(matrix.cols()),
327  m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
328  m_isInitialized(false)
329  {
330  compute(matrix, computeEigenvectors);
331  }
332 
333  SelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
334  : m_eivec(matA.cols(), matA.cols()),
335  m_eivalues(matA.cols()),
336  m_subdiag(matA.cols() > 1 ? matA.cols() - 1 : 1),
337  m_isInitialized(false)
338  {
339  static_cast<GeneralizedSelfAdjointEigenSolver<MatrixType>*>(this)->compute(matA, matB, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
340  }
341 
342  void compute(const MatrixType& matrix, bool computeEigenvectors)
343  {
344  compute(matrix, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
345  }
346 
347  void compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
348  {
349  compute(matA, matB, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
350  }
351  #endif // EIGEN2_SUPPORT
352 
353  protected:
354  MatrixType m_eivec;
355  RealVectorType m_eivalues;
356  typename TridiagonalizationType::SubDiagonalType m_subdiag;
357  ComputationInfo m_info;
358  bool m_isInitialized;
359  bool m_eigenvectorsOk;
360 };
361 
378 namespace internal {
379 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
380 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n);
381 }
382 
383 template<typename MatrixType>
384 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
385 ::compute(const MatrixType& matrix, int options)
386 {
387  eigen_assert(matrix.cols() == matrix.rows());
388  eigen_assert((options&~(EigVecMask|GenEigMask))==0
389  && (options&EigVecMask)!=EigVecMask
390  && "invalid option parameter");
391  bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
392  Index n = matrix.cols();
393  m_eivalues.resize(n,1);
394 
395  if(n==1)
396  {
397  m_eivalues.coeffRef(0,0) = internal::real(matrix.coeff(0,0));
398  if(computeEigenvectors)
399  m_eivec.setOnes(n,n);
400  m_info = Success;
401  m_isInitialized = true;
402  m_eigenvectorsOk = computeEigenvectors;
403  return *this;
404  }
405 
406  // declare some aliases
407  RealVectorType& diag = m_eivalues;
408  MatrixType& mat = m_eivec;
409 
410  // map the matrix coefficients to [-1:1] to avoid over- and underflow.
411  RealScalar scale = matrix.cwiseAbs().maxCoeff();
412  if(scale==RealScalar(0)) scale = RealScalar(1);
413  mat = matrix / scale;
414  m_subdiag.resize(n-1);
415  internal::tridiagonalization_inplace(mat, diag, m_subdiag, computeEigenvectors);
416 
417  Index end = n-1;
418  Index start = 0;
419  Index iter = 0; // total number of iterations
420 
421  while (end>0)
422  {
423  for (Index i = start; i<end; ++i)
424  if (internal::isMuchSmallerThan(internal::abs(m_subdiag[i]),(internal::abs(diag[i])+internal::abs(diag[i+1]))))
425  m_subdiag[i] = 0;
426 
427  // find the largest unreduced block
428  while (end>0 && m_subdiag[end-1]==0)
429  {
430  end--;
431  }
432  if (end<=0)
433  break;
434 
435  // if we spent too many iterations, we give up
436  iter++;
437  if(iter > m_maxIterations * n) break;
438 
439  start = end - 1;
440  while (start>0 && m_subdiag[start-1]!=0)
441  start--;
442 
443  internal::tridiagonal_qr_step<MatrixType::Flags&RowMajorBit ? RowMajor : ColMajor>(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
444  }
445 
446  if (iter <= m_maxIterations * n)
447  m_info = Success;
448  else
449  m_info = NoConvergence;
450 
451  // Sort eigenvalues and corresponding vectors.
452  // TODO make the sort optional ?
453  // TODO use a better sort algorithm !!
454  if (m_info == Success)
455  {
456  for (Index i = 0; i < n-1; ++i)
457  {
458  Index k;
459  m_eivalues.segment(i,n-i).minCoeff(&k);
460  if (k > 0)
461  {
462  std::swap(m_eivalues[i], m_eivalues[k+i]);
463  if(computeEigenvectors)
464  m_eivec.col(i).swap(m_eivec.col(k+i));
465  }
466  }
467  }
468 
469  // scale back the eigen values
470  m_eivalues *= scale;
471 
472  m_isInitialized = true;
473  m_eigenvectorsOk = computeEigenvectors;
474  return *this;
475 }
476 
477 
478 namespace internal {
479 
480 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues
481 {
482  static inline void run(SolverType& eig, const typename SolverType::MatrixType& A, int options)
483  { eig.compute(A,options); }
484 };
485 
486 template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3,false>
487 {
488  typedef typename SolverType::MatrixType MatrixType;
489  typedef typename SolverType::RealVectorType VectorType;
490  typedef typename SolverType::Scalar Scalar;
491 
492  static inline void computeRoots(const MatrixType& m, VectorType& roots)
493  {
494  using std::sqrt;
495  using std::atan2;
496  using std::cos;
497  using std::sin;
498  const Scalar s_inv3 = Scalar(1.0)/Scalar(3.0);
499  const Scalar s_sqrt3 = sqrt(Scalar(3.0));
500 
501  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
502  // eigenvalues are the roots to this equation, all guaranteed to be
503  // real-valued, because the matrix is symmetric.
504  Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(1,0)*m(2,0)*m(2,1) - m(0,0)*m(2,1)*m(2,1) - m(1,1)*m(2,0)*m(2,0) - m(2,2)*m(1,0)*m(1,0);
505  Scalar c1 = m(0,0)*m(1,1) - m(1,0)*m(1,0) + m(0,0)*m(2,2) - m(2,0)*m(2,0) + m(1,1)*m(2,2) - m(2,1)*m(2,1);
506  Scalar c2 = m(0,0) + m(1,1) + m(2,2);
507 
508  // Construct the parameters used in classifying the roots of the equation
509  // and in solving the equation for the roots in closed form.
510  Scalar c2_over_3 = c2*s_inv3;
511  Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
512  if (a_over_3 > Scalar(0))
513  a_over_3 = Scalar(0);
514 
515  Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
516 
517  Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
518  if (q > Scalar(0))
519  q = Scalar(0);
520 
521  // Compute the eigenvalues by solving for the roots of the polynomial.
522  Scalar rho = sqrt(-a_over_3);
523  Scalar theta = atan2(sqrt(-q),half_b)*s_inv3;
524  Scalar cos_theta = cos(theta);
525  Scalar sin_theta = sin(theta);
526  roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
527  roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
528  roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
529 
530  // Sort in increasing order.
531  if (roots(0) >= roots(1))
532  std::swap(roots(0),roots(1));
533  if (roots(1) >= roots(2))
534  {
535  std::swap(roots(1),roots(2));
536  if (roots(0) >= roots(1))
537  std::swap(roots(0),roots(1));
538  }
539  }
540 
541  static inline void run(SolverType& solver, const MatrixType& mat, int options)
542  {
543  using std::sqrt;
544  eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows());
545  eigen_assert((options&~(EigVecMask|GenEigMask))==0
546  && (options&EigVecMask)!=EigVecMask
547  && "invalid option parameter");
548  bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
549 
550  MatrixType& eivecs = solver.m_eivec;
551  VectorType& eivals = solver.m_eivalues;
552 
553  // map the matrix coefficients to [-1:1] to avoid over- and underflow.
554  Scalar scale = mat.cwiseAbs().maxCoeff();
555  MatrixType scaledMat = mat / scale;
556 
557  // compute the eigenvalues
558  computeRoots(scaledMat,eivals);
559 
560  // compute the eigen vectors
561  if(computeEigenvectors)
562  {
563  Scalar safeNorm2 = Eigen::NumTraits<Scalar>::epsilon();
564  safeNorm2 *= safeNorm2;
565  if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
566  {
567  eivecs.setIdentity();
568  }
569  else
570  {
571  scaledMat = scaledMat.template selfadjointView<Lower>();
572  MatrixType tmp;
573  tmp = scaledMat;
574 
575  Scalar d0 = eivals(2) - eivals(1);
576  Scalar d1 = eivals(1) - eivals(0);
577  int k = d0 > d1 ? 2 : 0;
578  d0 = d0 > d1 ? d1 : d0;
579 
580  tmp.diagonal().array () -= eivals(k);
581  VectorType cross;
582  Scalar n;
583  n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();
584 
585  if(n>safeNorm2)
586  eivecs.col(k) = cross / sqrt(n);
587  else
588  {
589  n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm();
590 
591  if(n>safeNorm2)
592  eivecs.col(k) = cross / sqrt(n);
593  else
594  {
595  n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm();
596 
597  if(n>safeNorm2)
598  eivecs.col(k) = cross / sqrt(n);
599  else
600  {
601  // the input matrix and/or the eigenvaues probably contains some inf/NaN,
602  // => exit
603  // scale back to the original size.
604  eivals *= scale;
605 
606  solver.m_info = NumericalIssue;
607  solver.m_isInitialized = true;
608  solver.m_eigenvectorsOk = computeEigenvectors;
609  return;
610  }
611  }
612  }
613 
614  tmp = scaledMat;
615  tmp.diagonal().array() -= eivals(1);
616 
618  eivecs.col(1) = eivecs.col(k).unitOrthogonal();
619  else
620  {
621  n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm();
622  if(n>safeNorm2)
623  eivecs.col(1) = cross / sqrt(n);
624  else
625  {
626  n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm();
627  if(n>safeNorm2)
628  eivecs.col(1) = cross / sqrt(n);
629  else
630  {
631  n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm();
632  if(n>safeNorm2)
633  eivecs.col(1) = cross / sqrt(n);
634  else
635  {
636  // we should never reach this point,
637  // if so the last two eigenvalues are likely to ve very closed to each other
638  eivecs.col(1) = eivecs.col(k).unitOrthogonal();
639  }
640  }
641  }
642 
643  // make sure that eivecs[1] is orthogonal to eivecs[2]
644  Scalar d = eivecs.col(1).dot(eivecs.col(k));
645  eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized();
646  }
647 
648  eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized();
649  }
650  }
651  // Rescale back to the original size.
652  eivals *= scale;
653 
654  solver.m_info = Success;
655  solver.m_isInitialized = true;
656  solver.m_eigenvectorsOk = computeEigenvectors;
657  }
658 };
659 
660 // 2x2 direct eigenvalues decomposition, code from Hauke Heibel
661 template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,2,false>
662 {
663  typedef typename SolverType::MatrixType MatrixType;
664  typedef typename SolverType::RealVectorType VectorType;
665  typedef typename SolverType::Scalar Scalar;
666 
667  static inline void computeRoots(const MatrixType& m, VectorType& roots)
668  {
669  using std::sqrt;
670  const Scalar t0 = Scalar(0.5) * sqrt( abs2(m(0,0)-m(1,1)) + Scalar(4)*m(1,0)*m(1,0));
671  const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1));
672  roots(0) = t1 - t0;
673  roots(1) = t1 + t0;
674  }
675 
676  static inline void run(SolverType& solver, const MatrixType& mat, int options)
677  {
678  eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows());
679  eigen_assert((options&~(EigVecMask|GenEigMask))==0
680  && (options&EigVecMask)!=EigVecMask
681  && "invalid option parameter");
682  bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
683 
684  MatrixType& eivecs = solver.m_eivec;
685  VectorType& eivals = solver.m_eivalues;
686 
687  // map the matrix coefficients to [-1:1] to avoid over- and underflow.
688  Scalar scale = mat.cwiseAbs().maxCoeff();
689  scale = (std::max)(scale,Scalar(1));
690  MatrixType scaledMat = mat / scale;
691 
692  // Compute the eigenvalues
693  computeRoots(scaledMat,eivals);
694 
695  // compute the eigen vectors
696  if(computeEigenvectors)
697  {
698  scaledMat.diagonal().array () -= eivals(1);
699  Scalar a2 = abs2(scaledMat(0,0));
700  Scalar c2 = abs2(scaledMat(1,1));
701  Scalar b2 = abs2(scaledMat(1,0));
702  if(a2>c2)
703  {
704  eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0);
705  eivecs.col(1) /= sqrt(a2+b2);
706  }
707  else
708  {
709  eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0);
710  eivecs.col(1) /= sqrt(c2+b2);
711  }
712 
713  eivecs.col(0) << eivecs.col(1).unitOrthogonal();
714  }
715 
716  // Rescale back to the original size.
717  eivals *= scale;
718 
719  solver.m_info = Success;
720  solver.m_isInitialized = true;
721  solver.m_eigenvectorsOk = computeEigenvectors;
722  }
723 };
724 
725 }
726 
727 template<typename MatrixType>
728 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
729 ::computeDirect(const MatrixType& matrix, int options)
730 {
731  internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>::run(*this,matrix,options);
732  return *this;
733 }
734 
735 namespace internal {
736 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
737 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n)
738 {
739  RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
740  RealScalar e = subdiag[end-1];
741  // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still
742  // underflow thus leading to inf/NaN values when using the following commented code:
743 // RealScalar e2 = abs2(subdiag[end-1]);
744 // RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
745  // This explain the following, somewhat more complicated, version:
746  RealScalar mu = diag[end];
747  if(td==0)
748  mu -= abs(e);
749  else
750  {
751  RealScalar e2 = abs2(subdiag[end-1]);
752  RealScalar h = hypot(td,e);
753  if(e2==0) mu -= (e / (td + (td>0 ? 1 : -1))) * (e / h);
754  else mu -= e2 / (td + (td>0 ? h : -h));
755  }
756 
757  RealScalar x = diag[start] - mu;
758  RealScalar z = subdiag[start];
759  for (Index k = start; k < end; ++k)
760  {
761  JacobiRotation<RealScalar> rot;
762  rot.makeGivens(x, z);
763 
764  // do T = G' T G
765  RealScalar sdk = rot.s() * diag[k] + rot.c() * subdiag[k];
766  RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1];
767 
768  diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]);
769  diag[k+1] = rot.s() * sdk + rot.c() * dkp1;
770  subdiag[k] = rot.c() * sdk - rot.s() * dkp1;
771 
772 
773  if (k > start)
774  subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z;
775 
776  x = subdiag[k];
777 
778  if (k < end - 1)
779  {
780  z = -rot.s() * subdiag[k+1];
781  subdiag[k + 1] = rot.c() * subdiag[k+1];
782  }
783 
784  // apply the givens rotation to the unit matrix Q = Q * G
785  if (matrixQ)
786  {
787  // FIXME if StorageOrder == RowMajor this operation is not very efficient
788  Map<Matrix<Scalar,Dynamic,Dynamic,StorageOrder> > q(matrixQ,n,n);
789  q.applyOnTheRight(k,k+1,rot);
790  }
791  }
792 }
793 
794 } // end namespace internal
795 
796 } // end namespace Eigen
797 
798 #endif // EIGEN_SELFADJOINTEIGENSOLVER_H