mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-05-18 23:57:39 +08:00
RealSchur: split computation in smaller functions.
This commit is contained in:
Jitse Niesen
parent
7dc56b3dfb
commit
9fad1e392b
@ -93,7 +93,12 @@ template<typename _MatrixType> class RealSchur
|
||||
EigenvalueType m_eivalues;
|
||||
bool m_isInitialized;
|
||||
|
||||
void hqr2();
|
||||
Scalar computeNormOfT();
|
||||
int findSmallSubdiagEntry(int n, Scalar norm);
|
||||
void computeShift(Scalar& x, Scalar& y, Scalar& w, int l, int n, Scalar& exshift, int iter);
|
||||
void findTwoSmallSubdiagEntries(Scalar x, Scalar y, Scalar w, int l, int& m, int n, Scalar& p, Scalar& q, Scalar& r);
|
||||
void doFrancisStep(int l, int m, int n, Scalar p, Scalar q, Scalar r, Scalar x, Scalar* workspace);
|
||||
void splitOffTwoRows(int n, Scalar exshift);
|
||||
};
|
||||
|
||||
|
||||
@ -102,66 +107,28 @@ void RealSchur<MatrixType>::compute(const MatrixType& matrix)
|
||||
{
|
||||
assert(matrix.cols() == matrix.rows());
|
||||
|
||||
// Reduce to Hessenberg form
|
||||
// Step 1. Reduce to Hessenberg form
|
||||
// TODO skip Q if skipU = true
|
||||
HessenbergDecomposition<MatrixType> hess(matrix);
|
||||
m_matT = hess.matrixH();
|
||||
m_matU = hess.matrixQ();
|
||||
|
||||
// Reduce to Real Schur form
|
||||
hqr2();
|
||||
|
||||
m_isInitialized = true;
|
||||
}
|
||||
|
||||
|
||||
template<typename MatrixType>
|
||||
void RealSchur<MatrixType>::hqr2()
|
||||
{
|
||||
// Step 2. Reduce to real Schur form
|
||||
typedef Matrix<Scalar, ColsAtCompileTime, 1, Options, MaxColsAtCompileTime, 1> ColumnVectorType;
|
||||
|
||||
// This is derived from the Algol procedure hqr2,
|
||||
// by Martin and Wilkinson, Handbook for Auto. Comp.,
|
||||
// Vol.ii-Linear Algebra, and the corresponding
|
||||
// Fortran subroutine in EISPACK.
|
||||
|
||||
// Initialize
|
||||
const int size = m_matU.cols();
|
||||
int n = size-1;
|
||||
Scalar exshift = 0.0;
|
||||
Scalar p=0, q=0, r=0;
|
||||
|
||||
ColumnVectorType workspaceVector(size);
|
||||
ColumnVectorType workspaceVector(m_matU.cols());
|
||||
Scalar* workspace = &workspaceVector.coeffRef(0);
|
||||
|
||||
// Compute matrix norm
|
||||
// FIXME to be efficient the following would requires a triangular reduxion code
|
||||
// Scalar norm = m_matT.upper().cwiseAbs().sum() + m_matT.corner(BottomLeft,n,n).diagonal().cwiseAbs().sum();
|
||||
Scalar norm = 0.0;
|
||||
for (int j = 0; j < size; ++j)
|
||||
{
|
||||
norm += m_matT.row(j).segment(std::max(j-1,0), size-std::max(j-1,0)).cwiseAbs().sum();
|
||||
}
|
||||
int n = m_matU.cols() - 1;
|
||||
Scalar exshift = 0.0;
|
||||
Scalar norm = computeNormOfT();
|
||||
|
||||
// Outer loop over eigenvalue index
|
||||
int iter = 0;
|
||||
while (n >= 0)
|
||||
{
|
||||
// Look for single small sub-diagonal element
|
||||
int l = n;
|
||||
while (l > 0)
|
||||
{
|
||||
Scalar s = ei_abs(m_matT.coeff(l-1,l-1)) + ei_abs(m_matT.coeff(l,l));
|
||||
if (s == 0.0)
|
||||
s = norm;
|
||||
if (ei_abs(m_matT.coeff(l,l-1)) < NumTraits<Scalar>::epsilon() * s)
|
||||
break;
|
||||
l--;
|
||||
}
|
||||
int l = findSmallSubdiagEntry(n, norm);
|
||||
|
||||
// Check for convergence
|
||||
// One root found
|
||||
if (l == n)
|
||||
if (l == n) // One root found
|
||||
{
|
||||
m_matT.coeffRef(n,n) = m_matT.coeff(n,n) + exshift;
|
||||
m_eivalues.coeffRef(n) = ComplexScalar(m_matT.coeff(n,n), 0.0);
|
||||
@ -170,169 +137,226 @@ void RealSchur<MatrixType>::hqr2()
|
||||
}
|
||||
else if (l == n-1) // Two roots found
|
||||
{
|
||||
Scalar w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n);
|
||||
p = (m_matT.coeff(n-1,n-1) - m_matT.coeff(n,n)) * Scalar(0.5);
|
||||
q = p * p + w;
|
||||
Scalar z = ei_sqrt(ei_abs(q));
|
||||
m_matT.coeffRef(n,n) = m_matT.coeff(n,n) + exshift;
|
||||
m_matT.coeffRef(n-1,n-1) = m_matT.coeff(n-1,n-1) + exshift;
|
||||
Scalar x = m_matT.coeff(n,n);
|
||||
|
||||
// Scalar pair
|
||||
if (q >= 0)
|
||||
{
|
||||
if (p >= 0)
|
||||
z = p + z;
|
||||
else
|
||||
z = p - z;
|
||||
|
||||
m_eivalues.coeffRef(n-1) = ComplexScalar(x + z, 0.0);
|
||||
m_eivalues.coeffRef(n) = ComplexScalar(z!=0.0 ? x - w / z : m_eivalues.coeff(n-1).real(), 0.0);
|
||||
|
||||
PlanarRotation<Scalar> rot;
|
||||
rot.makeGivens(z, m_matT.coeff(n, n-1));
|
||||
m_matT.block(0, n-1, size, size-n+1).applyOnTheLeft(n-1, n, rot.adjoint());
|
||||
m_matT.block(0, 0, n+1, size).applyOnTheRight(n-1, n, rot);
|
||||
m_matU.applyOnTheRight(n-1, n, rot);
|
||||
}
|
||||
else // Complex pair
|
||||
{
|
||||
m_eivalues.coeffRef(n-1) = ComplexScalar(x + p, z);
|
||||
m_eivalues.coeffRef(n) = ComplexScalar(x + p, -z);
|
||||
}
|
||||
splitOffTwoRows(n, exshift);
|
||||
n = n - 2;
|
||||
iter = 0;
|
||||
}
|
||||
else // No convergence yet
|
||||
{
|
||||
// Form shift
|
||||
Scalar x = m_matT.coeff(n,n);
|
||||
Scalar y = 0.0;
|
||||
Scalar w = 0.0;
|
||||
if (l < n)
|
||||
{
|
||||
y = m_matT.coeff(n-1,n-1);
|
||||
w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n);
|
||||
}
|
||||
|
||||
// Wilkinson's original ad hoc shift
|
||||
if (iter == 10)
|
||||
{
|
||||
exshift += x;
|
||||
for (int i = 0; i <= n; ++i)
|
||||
m_matT.coeffRef(i,i) -= x;
|
||||
Scalar s = ei_abs(m_matT.coeff(n,n-1)) + ei_abs(m_matT.coeff(n-1,n-2));
|
||||
x = y = Scalar(0.75) * s;
|
||||
w = Scalar(-0.4375) * s * s;
|
||||
}
|
||||
|
||||
// MATLAB's new ad hoc shift
|
||||
if (iter == 30)
|
||||
{
|
||||
Scalar s = Scalar((y - x) / 2.0);
|
||||
s = s * s + w;
|
||||
if (s > 0)
|
||||
{
|
||||
s = ei_sqrt(s);
|
||||
if (y < x)
|
||||
s = -s;
|
||||
s = Scalar(x - w / ((y - x) / 2.0 + s));
|
||||
for (int i = 0; i <= n; ++i)
|
||||
m_matT.coeffRef(i,i) -= s;
|
||||
exshift += s;
|
||||
x = y = w = Scalar(0.964);
|
||||
}
|
||||
}
|
||||
|
||||
Scalar p = 0, q = 0, r = 0, x, y, w;
|
||||
computeShift(x, y, w, l, n, exshift, iter);
|
||||
iter = iter + 1; // (Could check iteration count here.)
|
||||
|
||||
// Look for two consecutive small sub-diagonal elements
|
||||
int m = n-2;
|
||||
while (m >= l)
|
||||
{
|
||||
Scalar z = m_matT.coeff(m,m);
|
||||
r = x - z;
|
||||
Scalar s = y - z;
|
||||
p = (r * s - w) / m_matT.coeff(m+1,m) + m_matT.coeff(m,m+1);
|
||||
q = m_matT.coeff(m+1,m+1) - z - r - s;
|
||||
r = m_matT.coeff(m+2,m+1);
|
||||
s = ei_abs(p) + ei_abs(q) + ei_abs(r);
|
||||
p = p / s;
|
||||
q = q / s;
|
||||
r = r / s;
|
||||
if (m == l) {
|
||||
break;
|
||||
}
|
||||
if (ei_abs(m_matT.coeff(m,m-1)) * (ei_abs(q) + ei_abs(r)) <
|
||||
NumTraits<Scalar>::epsilon() * (ei_abs(p) * (ei_abs(m_matT.coeff(m-1,m-1)) + ei_abs(z) +
|
||||
ei_abs(m_matT.coeff(m+1,m+1)))))
|
||||
{
|
||||
break;
|
||||
}
|
||||
m--;
|
||||
}
|
||||
|
||||
for (int i = m+2; i <= n; ++i)
|
||||
{
|
||||
m_matT.coeffRef(i,i-2) = 0.0;
|
||||
if (i > m+2)
|
||||
m_matT.coeffRef(i,i-3) = 0.0;
|
||||
}
|
||||
|
||||
// Double QR step involving rows l:n and columns m:n
|
||||
for (int k = m; k <= n-1; ++k)
|
||||
{
|
||||
int notlast = (k != n-1);
|
||||
if (k != m) {
|
||||
p = m_matT.coeff(k,k-1);
|
||||
q = m_matT.coeff(k+1,k-1);
|
||||
r = notlast ? m_matT.coeff(k+2,k-1) : Scalar(0);
|
||||
x = ei_abs(p) + ei_abs(q) + ei_abs(r);
|
||||
if (x != 0.0)
|
||||
{
|
||||
p = p / x;
|
||||
q = q / x;
|
||||
r = r / x;
|
||||
}
|
||||
}
|
||||
|
||||
if (x == 0.0)
|
||||
break;
|
||||
|
||||
Scalar s = ei_sqrt(p * p + q * q + r * r);
|
||||
|
||||
if (p < 0)
|
||||
s = -s;
|
||||
|
||||
if (s != 0)
|
||||
{
|
||||
if (k != m)
|
||||
m_matT.coeffRef(k,k-1) = -s * x;
|
||||
else if (l != m)
|
||||
m_matT.coeffRef(k,k-1) = -m_matT.coeff(k,k-1);
|
||||
|
||||
p = p + s;
|
||||
|
||||
if (notlast)
|
||||
{
|
||||
Matrix<Scalar, 2, 1> ess(q/p, r/p);
|
||||
m_matT.block(k, k, 3, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace);
|
||||
m_matT.block(0, k, std::min(n,k+3) + 1, 3).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
}
|
||||
else
|
||||
{
|
||||
Matrix<Scalar, 1, 1> ess;
|
||||
ess.coeffRef(0) = q/p;
|
||||
m_matT.block(k, k, 2, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace);
|
||||
m_matT.block(0, k, std::min(n,k+3) + 1, 2).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
m_matU.block(0, k, size, 2).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
}
|
||||
|
||||
} // (s != 0)
|
||||
} // k loop
|
||||
int m;
|
||||
findTwoSmallSubdiagEntries(x, y, w, l, m, n, p, q, r);
|
||||
doFrancisStep(l, m, n, p, q, r, x, workspace);
|
||||
} // check convergence
|
||||
} // while (n >= 0)
|
||||
|
||||
m_isInitialized = true;
|
||||
}
|
||||
|
||||
// Compute matrix norm
|
||||
template<typename MatrixType>
|
||||
inline typename MatrixType::Scalar RealSchur<MatrixType>::computeNormOfT()
|
||||
{
|
||||
const int size = m_matU.cols();
|
||||
// FIXME to be efficient the following would requires a triangular reduxion code
|
||||
// Scalar norm = m_matT.upper().cwiseAbs().sum() + m_matT.corner(BottomLeft,size-1,size-1).diagonal().cwiseAbs().sum();
|
||||
Scalar norm = 0.0;
|
||||
for (int j = 0; j < size; ++j)
|
||||
norm += m_matT.row(j).segment(std::max(j-1,0), size-std::max(j-1,0)).cwiseAbs().sum();
|
||||
return norm;
|
||||
}
|
||||
|
||||
// Look for single small sub-diagonal element
|
||||
template<typename MatrixType>
|
||||
inline int RealSchur<MatrixType>::findSmallSubdiagEntry(int n, Scalar norm)
|
||||
{
|
||||
int l = n;
|
||||
while (l > 0)
|
||||
{
|
||||
Scalar s = ei_abs(m_matT.coeff(l-1,l-1)) + ei_abs(m_matT.coeff(l,l));
|
||||
if (s == 0.0)
|
||||
s = norm;
|
||||
if (ei_abs(m_matT.coeff(l,l-1)) < NumTraits<Scalar>::epsilon() * s)
|
||||
break;
|
||||
l--;
|
||||
}
|
||||
return l;
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline void RealSchur<MatrixType>::splitOffTwoRows(int n, Scalar exshift)
|
||||
{
|
||||
const int size = m_matU.cols();
|
||||
Scalar w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n);
|
||||
Scalar p = (m_matT.coeff(n-1,n-1) - m_matT.coeff(n,n)) * Scalar(0.5);
|
||||
Scalar q = p * p + w;
|
||||
Scalar z = ei_sqrt(ei_abs(q));
|
||||
m_matT.coeffRef(n,n) = m_matT.coeff(n,n) + exshift;
|
||||
m_matT.coeffRef(n-1,n-1) = m_matT.coeff(n-1,n-1) + exshift;
|
||||
Scalar x = m_matT.coeff(n,n);
|
||||
|
||||
// Scalar pair
|
||||
if (q >= 0)
|
||||
{
|
||||
if (p >= 0)
|
||||
z = p + z;
|
||||
else
|
||||
z = p - z;
|
||||
|
||||
m_eivalues.coeffRef(n-1) = ComplexScalar(x + z, 0.0);
|
||||
m_eivalues.coeffRef(n) = ComplexScalar(z!=0.0 ? x - w / z : m_eivalues.coeff(n-1).real(), 0.0);
|
||||
|
||||
PlanarRotation<Scalar> rot;
|
||||
rot.makeGivens(z, m_matT.coeff(n, n-1));
|
||||
m_matT.block(0, n-1, size, size-n+1).applyOnTheLeft(n-1, n, rot.adjoint());
|
||||
m_matT.block(0, 0, n+1, size).applyOnTheRight(n-1, n, rot);
|
||||
m_matU.applyOnTheRight(n-1, n, rot);
|
||||
}
|
||||
else // Complex pair
|
||||
{
|
||||
m_eivalues.coeffRef(n-1) = ComplexScalar(x + p, z);
|
||||
m_eivalues.coeffRef(n) = ComplexScalar(x + p, -z);
|
||||
}
|
||||
}
|
||||
|
||||
// Form shift
|
||||
template<typename MatrixType>
|
||||
inline void RealSchur<MatrixType>::computeShift(Scalar& x, Scalar& y, Scalar& w, int l, int n, Scalar& exshift, int iter)
|
||||
{
|
||||
x = m_matT.coeff(n,n);
|
||||
y = 0.0;
|
||||
w = 0.0;
|
||||
if (l < n)
|
||||
{
|
||||
y = m_matT.coeff(n-1,n-1);
|
||||
w = m_matT.coeff(n,n-1) * m_matT.coeff(n-1,n);
|
||||
}
|
||||
|
||||
// Wilkinson's original ad hoc shift
|
||||
if (iter == 10)
|
||||
{
|
||||
exshift += x;
|
||||
for (int i = 0; i <= n; ++i)
|
||||
m_matT.coeffRef(i,i) -= x;
|
||||
Scalar s = ei_abs(m_matT.coeff(n,n-1)) + ei_abs(m_matT.coeff(n-1,n-2));
|
||||
x = y = Scalar(0.75) * s;
|
||||
w = Scalar(-0.4375) * s * s;
|
||||
}
|
||||
|
||||
// MATLAB's new ad hoc shift
|
||||
if (iter == 30)
|
||||
{
|
||||
Scalar s = Scalar((y - x) / 2.0);
|
||||
s = s * s + w;
|
||||
if (s > 0)
|
||||
{
|
||||
s = ei_sqrt(s);
|
||||
if (y < x)
|
||||
s = -s;
|
||||
s = Scalar(x - w / ((y - x) / 2.0 + s));
|
||||
for (int i = 0; i <= n; ++i)
|
||||
m_matT.coeffRef(i,i) -= s;
|
||||
exshift += s;
|
||||
x = y = w = Scalar(0.964);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Look for two consecutive small sub-diagonal elements
|
||||
template<typename MatrixType>
|
||||
inline void RealSchur<MatrixType>::findTwoSmallSubdiagEntries(Scalar x, Scalar y, Scalar w, int l, int& m, int n, Scalar& p, Scalar& q, Scalar& r)
|
||||
{
|
||||
m = n-2;
|
||||
while (m >= l)
|
||||
{
|
||||
Scalar z = m_matT.coeff(m,m);
|
||||
r = x - z;
|
||||
Scalar s = y - z;
|
||||
p = (r * s - w) / m_matT.coeff(m+1,m) + m_matT.coeff(m,m+1);
|
||||
q = m_matT.coeff(m+1,m+1) - z - r - s;
|
||||
r = m_matT.coeff(m+2,m+1);
|
||||
s = ei_abs(p) + ei_abs(q) + ei_abs(r);
|
||||
p = p / s;
|
||||
q = q / s;
|
||||
r = r / s;
|
||||
if (m == l) {
|
||||
break;
|
||||
}
|
||||
if (ei_abs(m_matT.coeff(m,m-1)) * (ei_abs(q) + ei_abs(r)) <
|
||||
NumTraits<Scalar>::epsilon() * (ei_abs(p) * (ei_abs(m_matT.coeff(m-1,m-1)) + ei_abs(z) +
|
||||
ei_abs(m_matT.coeff(m+1,m+1)))))
|
||||
{
|
||||
break;
|
||||
}
|
||||
m--;
|
||||
}
|
||||
|
||||
for (int i = m+2; i <= n; ++i)
|
||||
{
|
||||
m_matT.coeffRef(i,i-2) = 0.0;
|
||||
if (i > m+2)
|
||||
m_matT.coeffRef(i,i-3) = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Double QR step involving rows l:n and columns m:n
|
||||
template<typename MatrixType>
|
||||
inline void RealSchur<MatrixType>::doFrancisStep(int l, int m, int n, Scalar p, Scalar q, Scalar r, Scalar x, Scalar* workspace)
|
||||
{
|
||||
const int size = m_matU.cols();
|
||||
|
||||
for (int k = m; k <= n-1; ++k)
|
||||
{
|
||||
int notlast = (k != n-1);
|
||||
if (k != m) {
|
||||
p = m_matT.coeff(k,k-1);
|
||||
q = m_matT.coeff(k+1,k-1);
|
||||
r = notlast ? m_matT.coeff(k+2,k-1) : Scalar(0);
|
||||
x = ei_abs(p) + ei_abs(q) + ei_abs(r);
|
||||
if (x != 0.0)
|
||||
{
|
||||
p = p / x;
|
||||
q = q / x;
|
||||
r = r / x;
|
||||
}
|
||||
}
|
||||
|
||||
if (x == 0.0)
|
||||
break;
|
||||
|
||||
Scalar s = ei_sqrt(p * p + q * q + r * r);
|
||||
|
||||
if (p < 0)
|
||||
s = -s;
|
||||
|
||||
if (s != 0)
|
||||
{
|
||||
if (k != m)
|
||||
m_matT.coeffRef(k,k-1) = -s * x;
|
||||
else if (l != m)
|
||||
m_matT.coeffRef(k,k-1) = -m_matT.coeff(k,k-1);
|
||||
|
||||
p = p + s;
|
||||
|
||||
if (notlast)
|
||||
{
|
||||
Matrix<Scalar, 2, 1> ess(q/p, r/p);
|
||||
m_matT.block(k, k, 3, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace);
|
||||
m_matT.block(0, k, std::min(n,k+3) + 1, 3).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
m_matU.block(0, k, size, 3).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
}
|
||||
else
|
||||
{
|
||||
Matrix<Scalar, 1, 1> ess;
|
||||
ess.coeffRef(0) = q/p;
|
||||
m_matT.block(k, k, 2, size-k).applyHouseholderOnTheLeft(ess, p/s, workspace);
|
||||
m_matT.block(0, k, std::min(n,k+3) + 1, 2).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
m_matU.block(0, k, size, 2).applyHouseholderOnTheRight(ess, p/s, workspace);
|
||||
}
|
||||
} // (s != 0)
|
||||
} // k loop
|
||||
}
|
||||
|
||||
#endif // EIGEN_REAL_SCHUR_H
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user