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

Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h

@@ -497,7 +497,12 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
   typedef typename SolverType::MatrixType MatrixType;
   typedef typename SolverType::RealVectorType VectorType;
   typedef typename SolverType::Scalar Scalar;
+  typedef typename MatrixType::Index Index;
   
+  /** \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.
+   */
   static inline void computeRoots(const MatrixType& m, VectorType& roots)
   {
     using std::sqrt;
@@ -517,39 +522,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;
+    Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3;
-    if (a_over_3 > Scalar(0))
+    if(a_over_3<Scalar(0))
       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;
+    Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b;
-    if (q > Scalar(0))
+    if(q<Scalar(0))
       q = Scalar(0);
 
     // Compute the eigenvalues by solving for the roots of the polynomial.
-    Scalar rho = sqrt(-a_over_3);
+    Scalar rho = sqrt(a_over_3);
-    Scalar theta = atan2(sqrt(-q),half_b)*s_inv3;
+    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 are already sorted, since cos is monotonically decreasing on [0, pi]
-    roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
+    roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3)
-    roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
+    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;
-    // Sort in increasing order.
-    if (roots(0) >= roots(1))
-      std::swap(roots(0),roots(1));
-    if (roots(1) >= roots(2))
-    {
-      std::swap(roots(1),roots(2));
-      if (roots(0) >= roots(1))
-        std::swap(roots(0),roots(1));
-    }
   }
-  
+
+  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;
+  }
+
   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
@@ -559,116 +573,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.
+    // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow.
-    Scalar scale = mat.cwiseAbs().maxCoeff();
+    Scalar shift = mat.trace() / Scalar(3);
-    MatrixType scaledMat = mat / scale;
+    // 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;
+        Index k(0), l(2);
-        d0 = d0 > d1 ? d0 : d1;
+        if(d0 > d1)
-
-        tmp.diagonal().array () -= eivals(k);
-        VectorType cross;
-        Scalar n;
-        n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();
-
-        if(n>safeNorm2)
         {
-          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)
+          VectorType dummy;
-          {
+          extract_kernel(tmp, eivecs.col(l), dummy);
-            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;
-            }
-          }
         }
 
-        tmp = scaledMat;
+        // Compute last eigenvector from the other two
-        tmp.diagonal().array() -= eivals(1);
+        eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized();
-
-        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();
       }
     }
+
     // Rescale back to the original size.
     eivals *= scale;
+    eivals.array() += shift;
     
     solver.m_info = Success;
     solver.m_isInitialized = true;
@@ -686,7 +656,7 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,2
   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;