mirror/eigen
2
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2024-12-15 07:10:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

bug #1014: More stable direct computation of eigenvalues and -vectors for 3x3 matrices

Browse Source
This commit is contained in:
Christoph Hertzberg 2015-05-17 21:54:32 +02:00
parent c88e1abaf3
commit ebea530782
1 changed files with 75 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
View File

@ -540,6 +540,11 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
typedef typename SolverType::RealVectorType VectorType;
typedef typename SolverType::Scalar Scalar;
/** \internal
* Computes the roots of the characteristic polynomial of \a m.
* For numerical stability m.trace() should be near zero and to avoid over- or underflow m should be normalized.
*/
EIGEN_DEVICE_FUNC
static inline void computeRoots(const MatrixType& m, VectorType& roots)
{
@ -560,40 +565,48 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
// Construct the parameters used in classifying the roots of the equation
// and in solving the equation for the roots in closed form.
Scalar c2_over_3 = c2*s_inv3;
Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
if (a_over_3 > Scalar(0))
a_over_3 = Scalar(0);
Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3;
a_over_3 = numext::maxi(a_over_3, Scalar(0));
Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
if (q > Scalar(0))
q = Scalar(0);
Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b;
q = numext::maxi(q, Scalar(0));
// Compute the eigenvalues by solving for the roots of the polynomial.
Scalar rho = sqrt(-a_over_3);
Scalar theta = atan2(sqrt(-q),half_b)*s_inv3;
Scalar rho = sqrt(a_over_3);
Scalar theta = atan2(sqrt(q),half_b)*s_inv3; // since sqrt(q) > 0, atan2 is in [0, pi] and theta is in [0, pi/3]
Scalar cos_theta = cos(theta);
Scalar sin_theta = sin(theta);
roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
// Sort in increasing order.
if (roots(0) >= roots(1))
numext::swap(roots(0),roots(1));
if (roots(1) >= roots(2))
{
numext::swap(roots(1),roots(2));
if (roots(0) >= roots(1))
numext::swap(roots(0),roots(1));
}
// roots are already sorted, since cos is monotonically decreasing on [0, pi]
roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3)
roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3)
roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta;
}
EIGEN_DEVICE_FUNC
static inline bool extract_kernel(MatrixType& mat, Ref<VectorType> res, Ref<VectorType> representative)
{
using std::abs;
Index i0;
// Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal):
mat.diagonal().cwiseAbs().maxCoeff(&i0);
// mat.col(i0) is a good candidate for an orthogonal vector to the current eigenvector,
// so let's save it:
representative = mat.col(i0);
Scalar n0, n1;
VectorType c0, c1;
n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm();
n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm();
if(n0>n1) res = c0/std::sqrt(n0);
else res = c1/std::sqrt(n1);
return true;
}
EIGEN_DEVICE_FUNC
static inline void run(SolverType& solver, const MatrixType& mat, int options)
{
using std::sqrt;
eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows());
eigen_assert((options&~(EigVecMask|GenEigMask))==0
&& (options&EigVecMask)!=EigVecMask
@ -603,116 +616,72 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
MatrixType& eivecs = solver.m_eivec;
VectorType& eivals = solver.m_eivalues;
// map the matrix coefficients to [-1:1] to avoid over- and underflow.
Scalar scale = mat.cwiseAbs().maxCoeff();
MatrixType scaledMat = mat / scale;
// Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow.
Scalar shift = mat.trace() / Scalar(3);
// TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later
MatrixType scaledMat = mat.template selfadjointView<Lower>();
scaledMat.diagonal().array() -= shift;
Scalar scale = scaledMat.cwiseAbs().maxCoeff();
if(scale > 0) scaledMat /= scale; // TODO for scale==0 we could save the remaining operations
// compute the eigenvalues
computeRoots(scaledMat,eivals);
// compute the eigen vectors
// compute the eigenvectors
if(computeEigenvectors)
{
Scalar safeNorm2 = Eigen::NumTraits<Scalar>::epsilon();
if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
{
// All three eigenvalues are numerically the same
eivecs.setIdentity();
}
else
{
scaledMat = scaledMat.template selfadjointView<Lower>();
MatrixType tmp;
tmp = scaledMat;
// Compute the eigenvector of the most distinct eigenvalue
Scalar d0 = eivals(2) - eivals(1);
Scalar d1 = eivals(1) - eivals(0);
int k = d0 > d1 ? 2 : 0;
d0 = d0 > d1 ? d0 : d1;
tmp.diagonal().array () -= eivals(k);
VectorType cross;
Scalar n;
n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();
if(n>safeNorm2)
Index k(0), l(2);
if(d0 > d1)
{
eivecs.col(k) = cross / sqrt(n);
std::swap(k,l);
d0 = d1;
}
// Compute the eigenvector of index k
{
tmp.diagonal().array () -= eivals(k);
// By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector.
extract_kernel(tmp, eivecs.col(k), eivecs.col(l));
}
// Compute eigenvector of index l
if(d0<=2*Eigen::NumTraits<Scalar>::epsilon()*d1)
{
// If d0 is too small, then the two other eigenvalues are numerically the same,
// and thus we only have to ortho-normalize the near orthogonal vector we saved above.
eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l);
eivecs.col(l).normalize();
}
else
{
n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm();
tmp = scaledMat;
tmp.diagonal().array () -= eivals(l);
if(n>safeNorm2)
{
eivecs.col(k) = cross / sqrt(n);
}
else
{
n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm();
if(n>safeNorm2)
{
eivecs.col(k) = cross / sqrt(n);
}
else
{
// the input matrix and/or the eigenvaues probably contains some inf/NaN,
// => exit
// scale back to the original size.
eivals *= scale;
solver.m_info = NumericalIssue;
solver.m_isInitialized = true;
solver.m_eigenvectorsOk = computeEigenvectors;
return;
}
}
VectorType dummy;
extract_kernel(tmp, eivecs.col(l), dummy);
}
tmp = scaledMat;
tmp.diagonal().array() -= eivals(1);
if(d0<=Eigen::NumTraits<Scalar>::epsilon())
{
eivecs.col(1) = eivecs.col(k).unitOrthogonal();
}
else
{
n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm();
if(n>safeNorm2)
{
eivecs.col(1) = cross / sqrt(n);
}
else
{
n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm();
if(n>safeNorm2)
eivecs.col(1) = cross / sqrt(n);
else
{
n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm();
if(n>safeNorm2)
eivecs.col(1) = cross / sqrt(n);
else
{
// we should never reach this point,
// if so the last two eigenvalues are likely to be very close to each other
eivecs.col(1) = eivecs.col(k).unitOrthogonal();
}
}
}
// make sure that eivecs[1] is orthogonal to eivecs[2]
// FIXME: this step should not be needed
Scalar d = eivecs.col(1).dot(eivecs.col(k));
eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized();
}
eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized();
// Compute last eigenvector from the other two
eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized();
}
}
// Rescale back to the original size.
eivals *= scale;
eivals.array() += shift;
solver.m_info = Success;
solver.m_isInitialized = true;
@ -732,7 +701,7 @@ struct direct_selfadjoint_eigenvalues<SolverType,2,false>
static inline void computeRoots(const MatrixType& m, VectorType& roots)
{
using std::sqrt;
const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*m(1,0)*m(1,0));
const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*numext::abs2(m(1,0)));
const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1));
roots(0) = t1 - t0;
roots(1) = t1 + t0;