eig_jacobi.cpp

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00012 #include <stdlib.h>
00013 #include <stdio.h>
00014 #include <math.h>
00015 
00016 #include "eig_jacobi.h"
00017 
00018 
00019 //void sortd( int a, double * b, int * c);  /* black box sorting routine */
00020 
00021 namespace hugin_utils
00022 {
00023 
00024     void sortd(int length, double * a, int *ind )
00025     {
00026 
00027         int i, ii, ij, j, m, m1, n2;
00028         double t;
00029         
00030         for ( i = 0 ; i < length; i++ ) ind[ i ] = i;
00031         m = 1;
00032         n2 = length / 2;
00033         m = 2 * m;
00034         while ( m <= n2 ) m = 2 * m;
00035         m = m - 1;
00036     three:;
00037         m1 = m + 1;
00038         for ( j = m1-1 ; j < length; j++ ) {
00039             t = a[ ind[ j ] ];
00040             ij = ind[ j ];
00041             i = j - m;
00042             ii = ind[ i ];
00043     four:;
00044             if ( t > a[ ii ] ) {
00045                 ind[ i+m ] = ii;
00046                 i = i - m;
00047                 if ( i >= 0 ) {
00048                     ii = ind[ i ];
00049                     goto four;
00050                 }
00051             }
00052             ind[ i+m ] = ij;
00053         }
00054         m = m / 2;
00055         if ( m > 0 ) goto three;
00056         return;
00057     }
00058 
00059 
00060     /* the line below is a marker for where sed will insert global
00061        parameters like MAX      - size of matrix
00062                        MAXSWEEP - MAXIMUN number of sweeps
00063                        EPSILON  - tolerance to deem offdiagonal elements zero
00064                        choice of test matrix
00065     */
00066     //INSERTS
00067 
00068     void eig_jacobi( int n, double a[3][3], double v[3][3], double *d,int* ind,int* maxsweep,int* maxannil,double* epsilon)
00069     {
00070     /* Implements jacobi eigenvalue/vector algorithm on a symmetric matrix
00071        stored as a 2 dimensional matrix a[n][n] and computes the eigenvectors
00072        in another globally allocated matrix v[n][n].
00073     intput:
00074        n        - size of matrix problem
00075     outputs:
00076         v        - eigenvector matrix
00077         d[MAX]   - a vector of unsorted eigenvalues
00078        ind[MAX] - a vector of indicies sorting d[] into descending order
00079        maxanil  - number of rotations applied
00080     inputs/outputs
00081         a        - input matrix (the input is changed)
00082         maxsweep - on input max number of sweeps
00083                 - on output actual number of sweeps
00084        epsilon  - on input tolerance to consider offdiagonal elements as zero
00085                 - on output sum of offdiagonal elements
00086     */
00087 
00088       int i, j, k, l, p, q, sweep, annil;
00089       double alpha, beta, c, s, mu1, mu2, mu3;
00090       double pp, qq, pq, offdiag;
00091       double t1, t2;
00092 
00093 
00094     /* get off diagonal contribution */
00095       if( n < 1 ) {
00096         printf( "In jacobi(), size of matrix = %d\n", n );
00097       }
00098 
00099     /* compute initial offdiagonal sum */
00100       offdiag = 0.0;
00101       for( k = 0; k < n; k++ ) {
00102         for( l = k+1; l < n; l++ ) {
00103           offdiag = offdiag + a[k][l] * a[k][l];
00104         }
00105       }
00106     /* compute tolerance for completion */
00107       mu1 = sqrt( offdiag ) / n ;
00108       mu2 = mu1;
00109     /* initiallize eigenvector matrix as identity matrix */
00110       for( i = 0; i < n; i++ ) {
00111         d[ i ] = a[ i ][ i ];
00112         for( j = 0; j < n; j++ ) {
00113           if( i == j ) {
00114             v[ i ][ j ] = 1.0;
00115           } else {
00116             v[ i ][ j ] = 0.0;
00117           }
00118         }
00119       }
00120       annil = 0;
00121       sweep = 0; 
00122     /* test if matrix is initially diagonal */
00123       if( mu1 <= ( *epsilon * mu1 ) ) goto done;
00124     /* major loop of sweeps */
00125       for( sweep = 1; sweep < *maxsweep; sweep++ ) {
00126     /* sweep */
00127         for( p = 0; p < n; p++ ) {
00128           for( q = p+1; q < n; q++ ) {
00129             if( fabs( a[ p ][ q ] ) > mu2 ) {
00130               annil++;
00131     /* calculate rotation to zero out a[ i ][ j ] */
00132               alpha = 0.5 * ( d[ p ] - d[ q ] );
00133               beta  = sqrt( a[ p ][ q ] * a[ p ][ q ] + alpha * alpha );
00134               c = sqrt( 0.5 +  fabs( alpha ) / ( 2.0 * beta ) );
00135               if( alpha == 0.0 ) {
00136                 s = c;
00137               } else {
00138                 s = - ( alpha * a[ p ][ q ] ) / ( 2.0 * beta * fabs( alpha ) * c ); 
00139               }
00140     /* apply rotation */
00141               pp = d[ p ];
00142               pq = a[ p ][ q ];
00143               qq = d[ q ];
00144               d[ p ] = c * c * pp + s * s * qq - 2.0 * s * c * pq;
00145               d[ q ] = s * s * pp + c * c * qq + 2.0 * s * c * pq;
00146               a[ p ][ q ] = ( c * c - s * s ) * pq + s * c * ( pp - qq );
00147     /* update columns */
00148               for( k = 0; k < p; k++ ) {
00149                 t1 = a[ k ][ p ];
00150                 t2 = a[ k ][ q ];
00151                 a[ k ][ p ] = c * t1 - s * t2;
00152                 a[ k ][ q ] = c * t2 + s * t1;
00153               }
00154     /* update triangular area */
00155               for( k = p+1; k < q; k++ ) {
00156                 t1 = a[ p ][ k ];
00157                 t2 = a[ k ][ q ];
00158                 a[ p ][ k ] = c * t1 - s * t2;
00159                 a[ k ][ q ] = c * t2 + s * t1;
00160               }
00161     /* update rows */
00162               for( k = q+1; k < n; k++ ) {
00163                 t1 = a[ p ][ k ];
00164                 t2 = a[ q ][ k ];
00165                 a[ p ][ k ] = c * t1 - s * t2;
00166                 a[ q ][ k ] = c * t2 + s * t1;
00167               }
00168     /* update matrix of eigenvectors */
00169               for( k = 0; k < n; k++ ) {
00170                 t1 = v[ p ][ k ];
00171                 t2 = v[ q ][ k ];
00172                 v[ p ][ k ] = c * t1 - s * t2;
00173                 v[ q ][ k ] = s * t1 + c * t2;
00174               }
00175             } /* abs( a[ p ][ q ] ) > mu2 */ 
00176           } /* p */
00177         } /* q */
00178     /* test for small off diagonal contribution */
00179         offdiag = 0.0;
00180         for( k = 0; k < n; k++ ) {
00181           for( l = k+1; l < n; l++ ) {
00182             offdiag = offdiag + a[k][l] * a[k][l];
00183           }
00184         }
00185         mu3 = sqrt( offdiag ) / n;
00186     /* has offdiagonal sum gotten larger ? if so there's a problem
00187          may be not positive definite ? 
00188     */
00189         if( mu2 < mu3 ) {
00190           printf( "Offdiagonal sum is increasing muold= %f munow= %f\n", mu2, mu3 );
00191           exit( -1 );
00192         } else {
00193           mu2 = mu3;
00194         }
00195     /* has offdiagonal sum gone below input goal ? */
00196         if( mu2 <= ( *epsilon * mu1 ) ) goto done;
00197       } /* sweep */
00198     done:;
00199       *maxsweep = sweep;
00200       *maxannil = annil;
00201       *epsilon  = offdiag;
00202       sortd( n, d, ind ); 
00203     }
00204 
00205 } // namespace

Generated on Sun Apr 20 01:25:40 2014 for Hugintrunk by  doxygen 1.3.9.1