Re: [eigen] 3x3 symmetric eigenvalues Problem

[Manuel Yguel <manuel.yguel@xxxxxxxxx>]

I actually looked to the code of Gael some months ago and did some
minor improvements. I should have made those public months ago, I
apologize for that.
Here is the code. I will propose a patch this week-end, if you guys
wait until that.
It is divided into 3 files.
- The first is the procedure.

- The second computes the eigenvector from the eigenvalues. I think
this is the one that is important since it solves possible issues with
the eigenvalues
being small and double or triple roots in the previous code.

- The third one computes the roots of the characteristic polynomial
(To Gael: note that it is not necessary to sort the roots.)

With that code here is the result I obtained for the example of Benjamin:
0 0 1
1 0 0
0 1 0

0.00299671 0.997105     2000

Now an open question for the patch: do you think it is worth having
the solver for the characteristic polynomial provided through the
polynomial module?

- kind regards

Manuel

/////////////////////////////////////////////////////////////
///File eigensolverDirectMethod.hpp
/////////////////////////////////////////////////////////////

#ifndef __EIGENSOLVER_DIRECT_METHOD_HPP
#define __EIGENSOLVER_DIRECT_METHOD_HPP

#include "rootsOfCharacteristicPolynomial_SSDP_Matrix33.hpp"
#include "eigenvectorFromEigenvalue.hpp"

namespace eigenSolver {

namespace dim3D {

template< class Matrix, class Poly_t >
inline
void characteristic_polynomial( const Matrix& M, Poly_t& p )
{
  typedef typename Matrix::Scalar   Scalar;

  // The characteristic equation is x^3 + c2*x^2 + c1*x + c0 = 0.  The
  // eigenvalues are the roots to this equation.
  p(0) =  - M(0,0)*M(1,1)*M(2,2) - Scalar(2)*M(0,1)*M(0,2)*M(1,2)
    + M(0,0)*M(1,2)*M(1,2) + M(1,1)*M(0,2)*M(0,2) + M(2,2)*M(0,1)*M(0,1);
  p(1) = M(0,0)*M(1,1) - M(0,1)*M(0,1) + M(0,0)*M(2,2)
    - M(0,2)*M(0,2) + M(1,1)*M(2,2) - M(1,2)*M(1,2);
  p(2) = -M(0,0) - M(1,1) - M(2,2);

  if( 4 <= Poly_t::SizeAtCompileTime ){
    p(3) = Scalar(1); }
}

template< class Root_finder = Roots_of_characteristic_polynomial_SSDP_matrix33 >
struct SSDP_directMethod
{
  template< class Matrix, class EVectors, class EValues >
    void operator()( const Matrix& M, EVectors& evecs, EValues& evals,
        const typename Matrix::Scalar precision_for_isotropic_case
        = Eigen::NumTraits<typename Matrix::Scalar>::dummy_precision(),
        const typename Matrix::Scalar significantSquaredNorm2
        = Eigen::NumTraits<typename Matrix::Scalar>::dummy_precision() ) const
    {
      typedef typename Matrix::Scalar   Scalar;
      typedef Eigen::Matrix<Scalar,3,1> Poly_t;
      typedef Eigen::Matrix<Scalar,3,3> Matrix_t;

      //Scale the matrix: greatest eigenvalue is bounded by 1.
      const Scalar scale = M.trace();
      const Matrix_t TMP = M / scale;

      Poly_t p;
      // The characteristic equation is x^3 + c2*x^2 + c1*x + c0 = 0.  The
      // eigenvalues are the roots to this equation, all guaranteed to be
      // real-valued, because the matrix is symmetric.
      characteristic_polynomial( TMP, p );

      //Find the roots of the polynomial (sorted in ascending order)
      Root_finder rootFinder;
      rootFinder( p, evals );

      //Find the eigenvectors
      eigenvectorsSSD( TMP, evals, evecs,
          precision_for_isotropic_case, significantSquaredNorm2 );

      //Scale back the eigenvalues
      evals *= scale;
    }
};

} //< namespace dim3D

} //< namespace eigenSolver

#endif //< __EIGENSOLVER_DIRECT_METHOD_HPP


//////////////////////////////////////////////////////////////////////
///File eigenvectorFromEigenvalue.hpp
//////////////////////////////////////////////////////////////////////
#ifndef __EIGENVECTOR_FROM_EIGENVALUE_HPP
#define __EIGENVECTOR_FROM_EIGENVALUE_HPP

#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <cassert>

namespace eigenSolver {

namespace dim3D {

namespace internal {

/** Computes an eigenvector for a a 3x3 matrix M and
 * an eigenvalue l, by solving a linear system.
 * @param[in] M_lId: the matrix \f$ M - \lambda I \f$. Should be
 *  non zero.
 * @param[out] evect: the computed normalized eigenvector.
 * @param[in] significantSquaredNorm2: the threshold that should be lower
 *  than the squared norm of the un-normalized eigenvector computed as
 *  the solution of the linear system \f$ (M - l*I) v = 0\f$ where one
 *  coordinate of v is fixed to 1. Note that to be effective this threshold
 *  must be set with respect to the norms of the columns of M. In particular
 *  this threshold should be lower than the square of the product of the two
 *  smallest norms among the column norms.
 *
 *  <i><b>Precondition:</b></i>
 *  <dd> M_lId should be a non zero matrix (far from zero for stability) </dd>
 */
template< class Matrix, class Vector >
inline
void eigenvector(
    const Matrix& M_lId,
    Vector& evect,
    const typename Matrix::Scalar significantSquaredNorm2
    = Eigen::NumTraits<typename Matrix::Scalar>::dummy_precision(),
    const typename Matrix::Index i=0, const typename Matrix::Index j=1 )
{
  typedef typename Matrix::Scalar Scalar;

  evect = M_lId.col(i).cross(M_lId.col(j));
  Scalar norm2 = evect.squaredNorm();
  if( norm2 > significantSquaredNorm2 ){
    evect /= std::sqrt( norm2 ); }
  else
  {/* Either:
    * - at least one of the two columns is zero,
    * - the two columns are colinear.
    */

    norm2 = M_lId.col(i).squaredNorm();
    if( norm2 < significantSquaredNorm2 ){//< col(i) is zero
      evect.setZero(); evect(i) = Scalar(1); }
    else
    {
      norm2 = M_lId.col(j).squaredNorm();
      if( norm2 < significantSquaredNorm2 ){//< col(j) is zero
        evect.setZero(); evect(j) = Scalar(1); }
      else //< The two columns are colinear
      {
        typename Matrix::Index maxIndex_col_i;
        const Scalar val = M_lId.col(i).maxCoeff(&maxIndex_col_i);
        const Scalar inv_lambda = val / M_lId.col(j)(maxIndex_col_i);
        evect.setZero();
        evect(i) = Scalar(1); evect(j) = -inv_lambda;
        evect.normalize();
      }
    }
  }
}

}//< namespace internal

/** Computes an eigenvector for a 3x3 symmetric semi-definite (SSD) matrix M
 * for which the eigenvalue l is provided, by solving a linear system.
 * @param[in] M: the SSD matrix.
 * @param[in] l: the eigenvalue.
 * @param[out] evect: the computed normalized eigenvector.
 * @param[in] significantSquaredNorm2: the threshold that should be lower
 *  than the squared norm of the un-normalized eigenvector computed as
 *  the solution of the linear system \f$ (M - l*I) v = 0\f$ where one
 *  coordinate of v is fixed to 1. Note that to be effective this threshold
 *  must be set with respect to the norms of the columns of M. In particular
 *  this threshold should be lower than the square of the product of the two
 *  smallest norms among the column norms.
 *
 *  <i><b>Precondition:</b></i>
 *  <dd> M should be a symmetric semi-definite matrix </dd>
 */
template< class Matrix, class Scalar, class Vector >
inline
void eigenvectorSSD(
    const Matrix& M, const Scalar l,
    Vector& evect,
    const Scalar significantSquaredNorm2 =
Eigen::NumTraits<Scalar>::dummy_precision() )
{
  Matrix tmp = M;
  tmp.diagonal().array() -= l;
  if( tmp.diagonal().squaredNorm() < significantSquaredNorm2 ){
    evect = Vector::UnitX(); }
  internal::eigenvector( tmp, evect, significantSquaredNorm2 );
}

/** Computes the three eigenvectors of a 3x3 symmetric semi-definite (SSD)
 * matrix M for which the eigenvalues are provided sorted in increasing order.
 * The method used is to solve a linear system.
 * @param[in] M: the SSD matrix.
 * @param[in] evals: the eigenvalues.
 * @param[out] evecs: the matrix of computed normalized eigenvectors, one
 * eigenvector by column. \code evecs.col(i) \endcode corresponds to the
 * eigenvector associated to eigenvalue \code evals(i) \endcode
 * @param[in] precision_for_isotropic_case: if the lowest and the greatest
 * eigenvalues are closest that this threshold, the matrix is considered
 * as isotropic i.e. of the form \f$ \lambda I \f$
 * @param[in] significantSquaredNorm2: the threshold that should be lower
 *  than the squared norm of the un-normalized eigenvector computed as
 *  the solution of the linear system \f$ (M - \lambda*I) v = 0\f$ where one
 *  coordinate of v is fixed to 1. Note that to be effective this threshold
 *  must be set with respect to the norms of the columns of M. In particular
 *  this threshold should be lower than the square of the product of the two
 *  smallest norms among the column norms. *
 *
 *  <i><b>Precondition:</b></i>
 *  <dd> M should be a symmetric semi-definite matrix </dd>
 */
template< class Matrix, class EValues, class EVectors >
inline
void eigenvectorsSSD(
    const Matrix& M, const EValues& evals,
    EVectors& evecs,
    const typename Matrix::Scalar precision_for_isotropic_case
    = Eigen::NumTraits<typename Matrix::Scalar>::dummy_precision(),
    const typename Matrix::Scalar significantSquaredNorm2
    = Eigen::NumTraits<typename Matrix::Scalar>::dummy_precision() )
{
  typedef typename Matrix::Scalar Scalar;
  if(std::abs(evals(2)-evals(0))<=precision_for_isotropic_case){
    evecs.setIdentity(); }
  else
  {
    Matrix tmp = M;
    tmp.diagonal ().array () -= evals (2);
    {
      typename Matrix::ColXpr ev(evecs.col(2));
      internal::eigenvector( tmp, ev, significantSquaredNorm2, 2, 0 );
    }

    tmp = M;
    tmp.diagonal ().array () -= evals (0);
    {
      typename Matrix::ColXpr ev(evecs.col(0));
      internal::eigenvector( tmp, ev, significantSquaredNorm2, 0, 1 );
    }
    evecs.col(1) = evecs.col(2).cross( evecs.col(0) );
  }
}

} //< namespace dim3D

} //< namespace eigenSolver

#endif //< __EIGENVECTOR_FROM_EIGENVALUE_HPP




/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// file rootsOfCharacteristicPolynomial_SSDP_Matrix33.hpp
/////////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef  __ROOTS_OF_CHARACTERISTIC_POLYNOMIAL_SSDP_MATRIX33_HPP
#define __ROOTS_OF_CHARACTERISTIC_POLYNOMIAL_SSDP_MATRIX33_HPP

#include <Eigen/Core>

/** \brief Function with state-of-the-art code (inspired by Numerical Recipies).
 * @param[in] poly An array that stores, at least, the non-trivial
coefficients of a
 * monic polynomial.
 * The coefficients of the polynomial should be ordered in the array poly as
 * follows:
 *  \f$ poly(x) = x^3 + c_2 x^2 + c_1 x + c_0 \f$
 * That is we don't care what coefficient is beyond poly[2].
 * @param[out] r The vector of positive real roots sorted in ascending order.
 */
struct Roots_of_characteristic_polynomial_SSDP_matrix33
{
  template<class Poly, class Roots>
    inline
    void operator()( const Poly& poly, Roots& r )
    {
      typedef typename Roots::Scalar Scalar;

      static const Scalar inv3 = Scalar(1.)/Scalar(3.);
      static const Scalar sqrt3 = std::sqrt( Scalar(3.) );
      const Scalar c2_square = poly[2]*poly[2];
      const Scalar c2_over_3 = poly[2]*inv3;

      const Scalar Q = (c2_square - Scalar(3.)*poly[1])/Scalar(9.);
      if( Q < Scalar(0) ){
        r.setConstant( -c2_over_3 ); }
      else
      {
        const Scalar R = (Scalar(2.)*poly[2]*c2_square -
Scalar(9.)*poly[2]*poly[1] + Scalar(27.)*poly[0])/Scalar(54.);

        const Scalar rho = std::sqrt(Q);
        Scalar cos_3_theta = R/(Q*rho);
        if( cos_3_theta > Scalar(1.) ){ cos_3_theta = Scalar(1.); }
        else if( cos_3_theta < Scalar(-1.) ){ cos_3_theta = Scalar(-1.); }
        const Scalar theta = std::acos( cos_3_theta )*inv3;
        const Scalar cos_theta = std::cos( theta );
        const Scalar sin_theta = std::sin( theta );

        r(0) = -Scalar(2)*rho*cos_theta          - c2_over_3;
        r(1) = rho*(cos_theta - sin_theta*sqrt3) - c2_over_3;
        r(2) = rho*(cos_theta + sin_theta*sqrt3) - c2_over_3;
      }
    }
};

#endif //< __ROOTS_OF_CHARACTERISTIC_POLYNOMIAL_SSDP_MATRIX33_HPP