ComplexSchur.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Claire Maurice
00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00006 // Copyright (C) 2010,2012 Jitse Niesen <jitse@maths.leeds.ac.uk>
00007 //
00008 // This Source Code Form is subject to the terms of the Mozilla
00009 // Public License v. 2.0. If a copy of the MPL was not distributed
00010 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00011 
00012 #ifndef EIGEN_COMPLEX_SCHUR_H
00013 #define EIGEN_COMPLEX_SCHUR_H
00014 
00015 #include "./HessenbergDecomposition.h"
00016 
00017 namespace Eigen { 
00018 
00019 namespace internal {
00020 template<typename MatrixType, bool IsComplex> struct complex_schur_reduce_to_hessenberg;
00021 }
00022 
00051 template<typename _MatrixType> class ComplexSchur
00052 {
00053   public:
00054     typedef _MatrixType MatrixType;
00055     enum {
00056       RowsAtCompileTime = MatrixType::RowsAtCompileTime,
00057       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
00058       Options = MatrixType::Options,
00059       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
00060       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
00061     };
00062 
00064     typedef typename MatrixType::Scalar Scalar;
00065     typedef typename NumTraits<Scalar>::Real RealScalar;
00066     typedef typename MatrixType::Index Index;
00067 
00074     typedef std::complex<RealScalar> ComplexScalar;
00075 
00081     typedef Matrix<ComplexScalar, RowsAtCompileTime, ColsAtCompileTime, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> ComplexMatrixType;
00082 
00094     ComplexSchur(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime)
00095       : m_matT(size,size),
00096         m_matU(size,size),
00097         m_hess(size),
00098         m_isInitialized(false),
00099         m_matUisUptodate(false),
00100         m_maxIters(-1)
00101     {}
00102 
00112     ComplexSchur(const MatrixType& matrix, bool computeU = true)
00113       : m_matT(matrix.rows(),matrix.cols()),
00114         m_matU(matrix.rows(),matrix.cols()),
00115         m_hess(matrix.rows()),
00116         m_isInitialized(false),
00117         m_matUisUptodate(false),
00118         m_maxIters(-1)
00119     {
00120       compute(matrix, computeU);
00121     }
00122 
00137     const ComplexMatrixType& matrixU() const
00138     {
00139       eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
00140       eigen_assert(m_matUisUptodate && "The matrix U has not been computed during the ComplexSchur decomposition.");
00141       return m_matU;
00142     }
00143 
00161     const ComplexMatrixType& matrixT() const
00162     {
00163       eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
00164       return m_matT;
00165     }
00166 
00189     ComplexSchur& compute(const MatrixType& matrix, bool computeU = true);
00190     
00208     template<typename HessMatrixType, typename OrthMatrixType>
00209     ComplexSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ,  bool computeU=true);
00210 
00215     ComputationInfo info() const
00216     {
00217       eigen_assert(m_isInitialized && "ComplexSchur is not initialized.");
00218       return m_info;
00219     }
00220 
00226     ComplexSchur& setMaxIterations(Index maxIters)
00227     {
00228       m_maxIters = maxIters;
00229       return *this;
00230     }
00231 
00233     Index getMaxIterations()
00234     {
00235       return m_maxIters;
00236     }
00237 
00243     static const int m_maxIterationsPerRow = 30;
00244 
00245   protected:
00246     ComplexMatrixType m_matT, m_matU;
00247     HessenbergDecomposition<MatrixType> m_hess;
00248     ComputationInfo m_info;
00249     bool m_isInitialized;
00250     bool m_matUisUptodate;
00251     Index m_maxIters;
00252 
00253   private:  
00254     bool subdiagonalEntryIsNeglegible(Index i);
00255     ComplexScalar computeShift(Index iu, Index iter);
00256     void reduceToTriangularForm(bool computeU);
00257     friend struct internal::complex_schur_reduce_to_hessenberg<MatrixType, NumTraits<Scalar>::IsComplex>;
00258 };
00259 
00263 template<typename MatrixType>
00264 inline bool ComplexSchur<MatrixType>::subdiagonalEntryIsNeglegible(Index i)
00265 {
00266   RealScalar d = numext::norm1(m_matT.coeff(i,i)) + numext::norm1(m_matT.coeff(i+1,i+1));
00267   RealScalar sd = numext::norm1(m_matT.coeff(i+1,i));
00268   if (internal::isMuchSmallerThan(sd, d, NumTraits<RealScalar>::epsilon()))
00269   {
00270     m_matT.coeffRef(i+1,i) = ComplexScalar(0);
00271     return true;
00272   }
00273   return false;
00274 }
00275 
00276 
00278 template<typename MatrixType>
00279 typename ComplexSchur<MatrixType>::ComplexScalar ComplexSchur<MatrixType>::computeShift(Index iu, Index iter)
00280 {
00281   using std::abs;
00282   if (iter == 10 || iter == 20) 
00283   {
00284     // exceptional shift, taken from http://www.netlib.org/eispack/comqr.f
00285     return abs(numext::real(m_matT.coeff(iu,iu-1))) + abs(numext::real(m_matT.coeff(iu-1,iu-2)));
00286   }
00287 
00288   // compute the shift as one of the eigenvalues of t, the 2x2
00289   // diagonal block on the bottom of the active submatrix
00290   Matrix<ComplexScalar,2,2> t = m_matT.template block<2,2>(iu-1,iu-1);
00291   RealScalar normt = t.cwiseAbs().sum();
00292   t /= normt;     // the normalization by sf is to avoid under/overflow
00293 
00294   ComplexScalar b = t.coeff(0,1) * t.coeff(1,0);
00295   ComplexScalar c = t.coeff(0,0) - t.coeff(1,1);
00296   ComplexScalar disc = sqrt(c*c + RealScalar(4)*b);
00297   ComplexScalar det = t.coeff(0,0) * t.coeff(1,1) - b;
00298   ComplexScalar trace = t.coeff(0,0) + t.coeff(1,1);
00299   ComplexScalar eival1 = (trace + disc) / RealScalar(2);
00300   ComplexScalar eival2 = (trace - disc) / RealScalar(2);
00301 
00302   if(numext::norm1(eival1) > numext::norm1(eival2))
00303     eival2 = det / eival1;
00304   else
00305     eival1 = det / eival2;
00306 
00307   // choose the eigenvalue closest to the bottom entry of the diagonal
00308   if(numext::norm1(eival1-t.coeff(1,1)) < numext::norm1(eival2-t.coeff(1,1)))
00309     return normt * eival1;
00310   else
00311     return normt * eival2;
00312 }
00313 
00314 
00315 template<typename MatrixType>
00316 ComplexSchur<MatrixType>& ComplexSchur<MatrixType>::compute(const MatrixType& matrix, bool computeU)
00317 {
00318   m_matUisUptodate = false;
00319   eigen_assert(matrix.cols() == matrix.rows());
00320 
00321   if(matrix.cols() == 1)
00322   {
00323     m_matT = matrix.template cast<ComplexScalar>();
00324     if(computeU)  m_matU = ComplexMatrixType::Identity(1,1);
00325     m_info = Success;
00326     m_isInitialized = true;
00327     m_matUisUptodate = computeU;
00328     return *this;
00329   }
00330 
00331   internal::complex_schur_reduce_to_hessenberg<MatrixType, NumTraits<Scalar>::IsComplex>::run(*this, matrix, computeU);
00332   computeFromHessenberg(m_matT, m_matU, computeU);
00333   return *this;
00334 }
00335 
00336 template<typename MatrixType>
00337 template<typename HessMatrixType, typename OrthMatrixType>
00338 ComplexSchur<MatrixType>& ComplexSchur<MatrixType>::computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ, bool computeU)
00339 {
00340   m_matT = matrixH;
00341   if(computeU)
00342     m_matU = matrixQ;
00343   reduceToTriangularForm(computeU);
00344   return *this;
00345 }
00346 namespace internal {
00347 
00348 /* Reduce given matrix to Hessenberg form */
00349 template<typename MatrixType, bool IsComplex>
00350 struct complex_schur_reduce_to_hessenberg
00351 {
00352   // this is the implementation for the case IsComplex = true
00353   static void run(ComplexSchur<MatrixType>& _this, const MatrixType& matrix, bool computeU)
00354   {
00355     _this.m_hess.compute(matrix);
00356     _this.m_matT = _this.m_hess.matrixH();
00357     if(computeU)  _this.m_matU = _this.m_hess.matrixQ();
00358   }
00359 };
00360 
00361 template<typename MatrixType>
00362 struct complex_schur_reduce_to_hessenberg<MatrixType, false>
00363 {
00364   static void run(ComplexSchur<MatrixType>& _this, const MatrixType& matrix, bool computeU)
00365   {
00366     typedef typename ComplexSchur<MatrixType>::ComplexScalar ComplexScalar;
00367 
00368     // Note: m_hess is over RealScalar; m_matT and m_matU is over ComplexScalar
00369     _this.m_hess.compute(matrix);
00370     _this.m_matT = _this.m_hess.matrixH().template cast<ComplexScalar>();
00371     if(computeU)  
00372     {
00373       // This may cause an allocation which seems to be avoidable
00374       MatrixType Q = _this.m_hess.matrixQ(); 
00375       _this.m_matU = Q.template cast<ComplexScalar>();
00376     }
00377   }
00378 };
00379 
00380 } // end namespace internal
00381 
00382 // Reduce the Hessenberg matrix m_matT to triangular form by QR iteration.
00383 template<typename MatrixType>
00384 void ComplexSchur<MatrixType>::reduceToTriangularForm(bool computeU)
00385 {  
00386   Index maxIters = m_maxIters;
00387   if (maxIters == -1)
00388     maxIters = m_maxIterationsPerRow * m_matT.rows();
00389 
00390   // The matrix m_matT is divided in three parts. 
00391   // Rows 0,...,il-1 are decoupled from the rest because m_matT(il,il-1) is zero. 
00392   // Rows il,...,iu is the part we are working on (the active submatrix).
00393   // Rows iu+1,...,end are already brought in triangular form.
00394   Index iu = m_matT.cols() - 1;
00395   Index il;
00396   Index iter = 0; // number of iterations we are working on the (iu,iu) element
00397   Index totalIter = 0; // number of iterations for whole matrix
00398 
00399   while(true)
00400   {
00401     // find iu, the bottom row of the active submatrix
00402     while(iu > 0)
00403     {
00404       if(!subdiagonalEntryIsNeglegible(iu-1)) break;
00405       iter = 0;
00406       --iu;
00407     }
00408 
00409     // if iu is zero then we are done; the whole matrix is triangularized
00410     if(iu==0) break;
00411 
00412     // if we spent too many iterations, we give up
00413     iter++;
00414     totalIter++;
00415     if(totalIter > maxIters) break;
00416 
00417     // find il, the top row of the active submatrix
00418     il = iu-1;
00419     while(il > 0 && !subdiagonalEntryIsNeglegible(il-1))
00420     {
00421       --il;
00422     }
00423 
00424     /* perform the QR step using Givens rotations. The first rotation
00425        creates a bulge; the (il+2,il) element becomes nonzero. This
00426        bulge is chased down to the bottom of the active submatrix. */
00427 
00428     ComplexScalar shift = computeShift(iu, iter);
00429     JacobiRotation<ComplexScalar> rot;
00430     rot.makeGivens(m_matT.coeff(il,il) - shift, m_matT.coeff(il+1,il));
00431     m_matT.rightCols(m_matT.cols()-il).applyOnTheLeft(il, il+1, rot.adjoint());
00432     m_matT.topRows((std::min)(il+2,iu)+1).applyOnTheRight(il, il+1, rot);
00433     if(computeU) m_matU.applyOnTheRight(il, il+1, rot);
00434 
00435     for(Index i=il+1 ; i<iu ; i++)
00436     {
00437       rot.makeGivens(m_matT.coeffRef(i,i-1), m_matT.coeffRef(i+1,i-1), &m_matT.coeffRef(i,i-1));
00438       m_matT.coeffRef(i+1,i-1) = ComplexScalar(0);
00439       m_matT.rightCols(m_matT.cols()-i).applyOnTheLeft(i, i+1, rot.adjoint());
00440       m_matT.topRows((std::min)(i+2,iu)+1).applyOnTheRight(i, i+1, rot);
00441       if(computeU) m_matU.applyOnTheRight(i, i+1, rot);
00442     }
00443   }
00444 
00445   if(totalIter <= maxIters)
00446     m_info = Success;
00447   else
00448     m_info = NoConvergence;
00449 
00450   m_isInitialized = true;
00451   m_matUisUptodate = computeU;
00452 }
00453 
00454 } // end namespace Eigen
00455 
00456 #endif // EIGEN_COMPLEX_SCHUR_H


turtlebot_exploration_3d
Author(s): Bona , Shawn
autogenerated on Thu Jun 6 2019 20:57:57