LCOV - code coverage report
Current view: directory - src/vxl/core/vnl/algo - vnl_symmetric_eigensystem.txx (source / functions) Found Hit Coverage
Test: vnl.info Lines: 104 31 29.8 %
Date: 2011-09-02 Functions: 22 7 31.8 %

       1                 : // This is core/vnl/algo/vnl_symmetric_eigensystem.txx
       2                 : #ifndef vnl_symmetric_eigensystem_txx_
       3                 : #define vnl_symmetric_eigensystem_txx_
       4                 : //:
       5                 : // \file
       6                 : // \author Andrew W. Fitzgibbon, Oxford RRG
       7                 : // \date Created: 29 Aug 96
       8                 : // \verbatim
       9                 : //   Modifications
      10                 : //    24 Mar 2010  Peter Vanroose  renamed from .cxx to .txx and moved out template instantiations
      11                 : // \endverbatim
      12                 : //
      13                 : //-----------------------------------------------------------------------------
      14                 : 
      15                 : #include "vnl_symmetric_eigensystem.h"
      16                 : #include <vcl_cassert.h>
      17                 : #include <vcl_algorithm.h> // for swap
      18                 : #include <vcl_cmath.h> // for sqrt(double), acos, etc.
      19                 : #include <vcl_iostream.h>
      20                 : #include <vnl/vnl_copy.h>
      21                 : #include <vnl/vnl_math.h>
      22                 : #include <vnl/algo/vnl_netlib.h> // rs_()
      23                 : 
      24                 : //: Find eigenvalues of a symmetric 3x3 matrix
      25                 : // \verbatim
      26                 : // Matrix is   M11  M12  M13
      27                 : //             M12  M22  M23
      28                 : //             M13  M23  M33
      29                 : // \endverbatim
      30                 : template <class T>
      31                 : void vnl_symmetric_eigensystem_compute_eigenvals(
      32                 :   T M11, T M12, T M13,
      33                 :          T M22, T M23,
      34                 :                 T M33,
      35               0 :   T &l1, T &l2, T &l3)
      36                 : {
      37                 :   // Characteristic eqtn |M - xI| = 0
      38                 :   // x^3 + b x^2 + c x + d = 0
      39               0 :   const T b = -M11-M22-M33;
      40               0 :   const T c =  M11*M22 +M11*M33 +M22*M33  -M12*M12 -M13*M13 -M23*M23;
      41               0 :   const T d = M11*M23*M23 +M12*M12*M33 +M13*M13*M22 -2*M12*M13*M23 -M11*M22*M33;
      42                 : 
      43                 :   // Using a numerically tweaked version of the real cubic solver http://www.1728.com/cubic2.htm
      44               0 :   const T b_3 = b/3;
      45               0 :   const T f = b_3*b_3 -  c/3 ;
      46               0 :   const T g = b*c/6 - b_3*b_3*b_3 - d/2;
      47                 : 
      48               0 :   if (f == 0 && g == 0)
      49                 :   {
      50               0 :     l1 = l2 = l3 = - b_3 ;
      51               0 :     return;
      52                 :   }
      53                 : 
      54               0 :   const T f3 = f*f*f;
      55               0 :   const T g2 = g*g;
      56               0 :   const T sqrt_f = -vcl_sqrt(f);
      57                 : 
      58                 :   // deal explicitly with repeated root and treat
      59                 :   // complex conjugate roots as numerically inaccurate repeated roots.
      60                 : 
      61                 :   // first check we are not too numerically inaccurate
      62               0 :   assert((g2 - f3) / vnl_math_sqr(vnl_math_cube(b)) < 1e-8);
      63                 : 
      64               0 :   if (g2 >= f3)
      65                 :   {
      66               0 :     if (g < 0)
      67                 :     {
      68               0 :       l1 = 2 * sqrt_f  - b_3;
      69               0 :       l2 = l3 = - sqrt_f - b_3;
      70                 :     }
      71                 :     else
      72                 :     {
      73               0 :       l1 = l2 = sqrt_f  - b_3;
      74               0 :       l3 = -2 * sqrt_f - b_3;
      75                 :     }
      76               0 :     return;
      77                 :   }
      78                 : 
      79                 : 
      80               0 :   const T sqrt_f3 = sqrt_f * sqrt_f * sqrt_f;
      81               0 :   const T k = vcl_acos(g / sqrt_f3) / 3;
      82               0 :   const T j = 2 * sqrt_f;
      83               0 :   l1 = j * vcl_cos(k) - b_3;
      84               0 :   l2 = j * vcl_cos(k + T(vnl_math::pi * 2.0 / 3.0)) - b_3;
      85               0 :   l3 = j * vcl_cos(k - T(vnl_math::pi * 2.0 / 3.0)) - b_3;
      86                 : 
      87               0 :   if (l2 < l1) vcl_swap(l2, l1);
      88               0 :   if (l3 < l2)
      89                 :   {
      90               0 :     vcl_swap(l2, l3);
      91               0 :     if (l2 < l1) vcl_swap(l2, l1);
      92                 :   }
      93                 : }
      94                 : 
      95                 : template <class T>
      96                 : bool vnl_symmetric_eigensystem_compute(vnl_matrix<T> const & A,
      97                 :                                        vnl_matrix<T>       & V,
      98           41389 :                                        vnl_vector<T>       & D)
      99                 : {
     100           41389 :   A.assert_finite();
     101           41387 :   const long n = A.rows();
     102                 : 
     103                 :   // Set the size of the eigenvalue vector D (output) if it does not match the size of A:
     104           41387 :   if (D.size() != A.rows())
     105               0 :     D.set_size(n);
     106                 : 
     107                 :   // convert to double
     108           41388 :   vnl_matrix<double> Ad(A.rows(), A.cols()); vnl_copy(A, Ad);
     109           41389 :   vnl_vector<double> Dd(D.size());
     110           41389 :   vnl_vector<double> work1(n);
     111           41388 :   vnl_vector<double> work2(n);
     112           41387 :   vnl_vector<double> Vvec(n*n);
     113                 : 
     114           41389 :   long want_eigenvectors = 1;
     115           41389 :   long ierr = 0;
     116                 : 
     117                 :   // No need to transpose A, 'cos it's symmetric...
     118           41389 :   v3p_netlib_rs_(&n, &n, Ad.data_block(), &Dd[0], &want_eigenvectors, &Vvec[0], &work1[0], &work2[0], &ierr);
     119           41389 :   vnl_copy(Dd, D);
     120                 : 
     121           41387 :   if (ierr) {
     122               0 :     vcl_cerr << "vnl_symmetric_eigensystem: ierr = " << ierr << vcl_endl;
     123               0 :     return false;
     124                 :   }
     125                 : 
     126                 :   // Transpose-copy into V, which is first resized if necessary
     127           41387 :   if (V.rows() != A.rows() || V.cols() != A.rows())
     128               0 :     V.set_size(n,n);
     129           41387 :   double *vptr = &Vvec[0];
     130          124258 :   for (int c = 0; c < n; ++c)
     131          248896 :     for (int r = 0; r < n; ++r)
     132          166027 :       V(r,c) = T(*vptr++);
     133                 : 
     134           41387 :   return true;
     135                 : }
     136                 : 
     137                 : //----------------------------------------------------------------------
     138                 : 
     139                 : // - @{ Solve real symmetric eigensystem $A x = \lambda x$ @}
     140                 : template <class T>
     141           41389 : vnl_symmetric_eigensystem<T>::vnl_symmetric_eigensystem(vnl_matrix<T> const& A)
     142           41389 :   : n_(A.rows()), V(n_, n_), D(n_)
     143                 : {
     144           41389 :   vnl_vector<T> Dvec(n_);
     145                 : 
     146           41389 :   vnl_symmetric_eigensystem_compute(A, V, Dvec);
     147                 : 
     148                 :   // Copy Dvec into diagonal of D
     149          124262 :   for (int i = 0; i < n_; ++i)
     150          124262 :     D(i,i) = Dvec[i];
     151           41389 : }
     152                 : 
     153                 : template <class T>
     154             182 : vnl_vector<T> vnl_symmetric_eigensystem<T>::get_eigenvector(int i) const
     155                 : {
     156             182 :   return vnl_vector<T>(V.extract(n_,1,0,i).data_block(), n_);
     157                 : }
     158                 : 
     159                 : template <class T>
     160           82338 : T vnl_symmetric_eigensystem<T>::get_eigenvalue(int i) const
     161                 : {
     162           82338 :   return D(i, i);
     163                 : }
     164                 : 
     165                 : template <class T>
     166               0 : vnl_vector<T> vnl_symmetric_eigensystem<T>::solve(vnl_vector<T> const& b)
     167                 : {
     168                 :   //vnl_vector<T> ret(b.length());
     169                 :   //FastOps::AtB(V, b, &ret);
     170               0 :   vnl_vector<T> ret(b*V); // same as V.transpose()*b
     171                 : 
     172               0 :   vnl_vector<T> tmp(b.size());
     173               0 :   D.solve(ret, &tmp);
     174                 : 
     175               0 :   return V * tmp;
     176                 : }
     177                 : 
     178                 : template <class T>
     179               0 : T vnl_symmetric_eigensystem<T>::determinant() const
     180                 : {
     181               0 :   int const n = D.size();
     182               0 :   T det(1);
     183               0 :   for (int i=0; i<n; ++i)
     184               0 :     det *= D[i];
     185               0 :   return det;
     186                 : }
     187                 : 
     188                 : template <class T>
     189               0 : vnl_matrix<T> vnl_symmetric_eigensystem<T>::pinverse() const
     190                 : {
     191               0 :   unsigned n = D.rows();
     192               0 :   vnl_diag_matrix<T> invD(n);
     193               0 :   for (unsigned i=0; i<n; ++i)
     194               0 :     if (D(i, i) == 0) {
     195               0 :       vcl_cerr << __FILE__ ": pinverse(): eigenvalue " << i << " is zero.\n";
     196               0 :       invD(i, i) = 0;
     197                 :     }
     198                 :     else
     199               0 :       invD(i, i) = 1 / D(i, i);
     200               0 :   return V * invD * V.transpose();
     201                 : }
     202                 : 
     203                 : template <class T>
     204               0 : vnl_matrix<T> vnl_symmetric_eigensystem<T>::square_root() const
     205                 : {
     206               0 :   unsigned n = D.rows();
     207               0 :   vnl_diag_matrix<T> sqrtD(n);
     208               0 :   for (unsigned i=0; i<n; ++i)
     209               0 :     if (D(i, i) < 0) {
     210               0 :       vcl_cerr << __FILE__ ": square_root(): eigenvalue " << i << " is negative (" << D(i, i) << ").\n";
     211               0 :       sqrtD(i, i) = (T)vcl_sqrt((typename vnl_numeric_traits<T>::real_t)(-D(i, i)));
     212                 :                     // gives square root of the absolute value of T.
     213                 :     }
     214                 :     else
     215               0 :       sqrtD(i, i) = (T)vcl_sqrt((typename vnl_numeric_traits<T>::real_t)(D(i, i)));
     216               0 :   return V * sqrtD * V.transpose();
     217                 : }
     218                 : 
     219                 : template <class T>
     220               0 : vnl_matrix<T> vnl_symmetric_eigensystem<T>::inverse_square_root() const
     221                 : {
     222               0 :   unsigned n = D.rows();
     223               0 :   vnl_diag_matrix<T> inv_sqrtD(n);
     224               0 :   for (unsigned i=0; i<n; ++i)
     225               0 :     if (D(i, i) <= 0) {
     226               0 :       vcl_cerr << __FILE__ ": square_root(): eigenvalue " << i << " is non-positive (" << D(i, i) << ").\n";
     227               0 :       inv_sqrtD(i, i) = (T)vcl_sqrt(-1.0/(typename vnl_numeric_traits<T>::real_t)(D(i, i))); // ??
     228                 :     }
     229                 :     else
     230               0 :       inv_sqrtD(i, i) = (T)vcl_sqrt(1.0/(typename vnl_numeric_traits<T>::real_t)(D(i, i)));
     231               0 :   return V * inv_sqrtD * V.transpose();
     232                 : }
     233                 : 
     234                 : //--------------------------------------------------------------------------------
     235                 : 
     236                 : #undef VNL_SYMMETRIC_EIGENSYSTEM_INSTANTIATE
     237                 : #define VNL_SYMMETRIC_EIGENSYSTEM_INSTANTIATE(T) \
     238                 : template class vnl_symmetric_eigensystem<T >; \
     239                 : template void vnl_symmetric_eigensystem_compute_eigenvals(T,T,T,T,T,T,T&,T&,T&); \
     240                 : template bool vnl_symmetric_eigensystem_compute(vnl_matrix<T > const&, vnl_matrix<T > &, vnl_vector<T >&)
     241                 : 
     242                 : #endif // vnl_symmetric_eigensystem_txx_

Generated by: LCOV version 1.7