Hugintrunk  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
eig_jacobi.cpp
Go to the documentation of this file.
1 // -*- c-basic-offset: 4 -*-
12 #include <stdlib.h>
13 #include <stdio.h>
14 #include <math.h>
15 
16 #include "eig_jacobi.h"
17 
18 
19 //void sortd( int a, double * b, int * c); /* black box sorting routine */
20 
21 namespace hugin_utils
22 {
23 
24  void sortd(int length, double * a, int *ind )
25  {
26 
27  int i, ii, ij, j, m, m1, n2;
28  double t;
29 
30  for ( i = 0 ; i < length; i++ ) ind[ i ] = i;
31  m = 1;
32  n2 = length / 2;
33  m = 2 * m;
34  while ( m <= n2 ) m = 2 * m;
35  m = m - 1;
36  three:;
37  m1 = m + 1;
38  for ( j = m1-1 ; j < length; j++ ) {
39  t = a[ ind[ j ] ];
40  ij = ind[ j ];
41  i = j - m;
42  ii = ind[ i ];
43  four:;
44  if ( t > a[ ii ] ) {
45  ind[ i+m ] = ii;
46  i = i - m;
47  if ( i >= 0 ) {
48  ii = ind[ i ];
49  goto four;
50  }
51  }
52  ind[ i+m ] = ij;
53  }
54  m = m / 2;
55  if ( m > 0 ) goto three;
56  return;
57  }
58 
59 
60  /* the line below is a marker for where sed will insert global
61  parameters like MAX - size of matrix
62  MAXSWEEP - MAXIMUN number of sweeps
63  EPSILON - tolerance to deem offdiagonal elements zero
64  choice of test matrix
65  */
66  //INSERTS
67 
68  void eig_jacobi( int n, double a[3][3], double v[3][3], double *d,int* ind,int* maxsweep,int* maxannil,double* epsilon)
69  {
70  /* Implements jacobi eigenvalue/vector algorithm on a symmetric matrix
71  stored as a 2 dimensional matrix a[n][n] and computes the eigenvectors
72  in another globally allocated matrix v[n][n].
73  intput:
74  n - size of matrix problem
75  outputs:
76  v - eigenvector matrix
77  d[MAX] - a vector of unsorted eigenvalues
78  ind[MAX] - a vector of indicies sorting d[] into descending order
79  maxanil - number of rotations applied
80  inputs/outputs
81  a - input matrix (the input is changed)
82  maxsweep - on input max number of sweeps
83  - on output actual number of sweeps
84  epsilon - on input tolerance to consider offdiagonal elements as zero
85  - on output sum of offdiagonal elements
86  */
87 
88  int i, j, k, l, p, q, sweep, annil;
89  double alpha, beta, c, s, mu1, mu2, mu3;
90  double pp, qq, pq, offdiag;
91  double t1, t2;
92 
93 
94  /* get off diagonal contribution */
95  if( n < 1 ) {
96  printf( "In jacobi(), size of matrix = %d\n", n );
97  }
98 
99  /* compute initial offdiagonal sum */
100  offdiag = 0.0;
101  for( k = 0; k < n; k++ ) {
102  for( l = k+1; l < n; l++ ) {
103  offdiag = offdiag + a[k][l] * a[k][l];
104  }
105  }
106  /* compute tolerance for completion */
107  mu1 = sqrt( offdiag ) / n ;
108  mu2 = mu1;
109  /* initiallize eigenvector matrix as identity matrix */
110  for( i = 0; i < n; i++ ) {
111  d[ i ] = a[ i ][ i ];
112  for( j = 0; j < n; j++ ) {
113  if( i == j ) {
114  v[ i ][ j ] = 1.0;
115  } else {
116  v[ i ][ j ] = 0.0;
117  }
118  }
119  }
120  annil = 0;
121  sweep = 0;
122  /* test if matrix is initially diagonal */
123  if( mu1 <= ( *epsilon * mu1 ) ) goto done;
124  /* major loop of sweeps */
125  for( sweep = 1; sweep < *maxsweep; sweep++ ) {
126  /* sweep */
127  for( p = 0; p < n; p++ ) {
128  for( q = p+1; q < n; q++ ) {
129  if( fabs( a[ p ][ q ] ) > mu2 ) {
130  annil++;
131  /* calculate rotation to zero out a[ i ][ j ] */
132  alpha = 0.5 * ( d[ p ] - d[ q ] );
133  beta = sqrt( a[ p ][ q ] * a[ p ][ q ] + alpha * alpha );
134  c = sqrt( 0.5 + fabs( alpha ) / ( 2.0 * beta ) );
135  if( alpha == 0.0 ) {
136  s = c;
137  } else {
138  s = - ( alpha * a[ p ][ q ] ) / ( 2.0 * beta * fabs( alpha ) * c );
139  }
140  /* apply rotation */
141  pp = d[ p ];
142  pq = a[ p ][ q ];
143  qq = d[ q ];
144  d[ p ] = c * c * pp + s * s * qq - 2.0 * s * c * pq;
145  d[ q ] = s * s * pp + c * c * qq + 2.0 * s * c * pq;
146  a[ p ][ q ] = ( c * c - s * s ) * pq + s * c * ( pp - qq );
147  /* update columns */
148  for( k = 0; k < p; k++ ) {
149  t1 = a[ k ][ p ];
150  t2 = a[ k ][ q ];
151  a[ k ][ p ] = c * t1 - s * t2;
152  a[ k ][ q ] = c * t2 + s * t1;
153  }
154  /* update triangular area */
155  for( k = p+1; k < q; k++ ) {
156  t1 = a[ p ][ k ];
157  t2 = a[ k ][ q ];
158  a[ p ][ k ] = c * t1 - s * t2;
159  a[ k ][ q ] = c * t2 + s * t1;
160  }
161  /* update rows */
162  for( k = q+1; k < n; k++ ) {
163  t1 = a[ p ][ k ];
164  t2 = a[ q ][ k ];
165  a[ p ][ k ] = c * t1 - s * t2;
166  a[ q ][ k ] = c * t2 + s * t1;
167  }
168  /* update matrix of eigenvectors */
169  for( k = 0; k < n; k++ ) {
170  t1 = v[ p ][ k ];
171  t2 = v[ q ][ k ];
172  v[ p ][ k ] = c * t1 - s * t2;
173  v[ q ][ k ] = s * t1 + c * t2;
174  }
175  } /* abs( a[ p ][ q ] ) > mu2 */
176  } /* p */
177  } /* q */
178  /* test for small off diagonal contribution */
179  offdiag = 0.0;
180  for( k = 0; k < n; k++ ) {
181  for( l = k+1; l < n; l++ ) {
182  offdiag = offdiag + a[k][l] * a[k][l];
183  }
184  }
185  mu3 = sqrt( offdiag ) / n;
186  /* has offdiagonal sum gotten larger ? if so there's a problem
187  may be not positive definite ?
188  */
189  if( mu2 < mu3 ) {
190  printf( "Offdiagonal sum is increasing muold= %f munow= %f\n", mu2, mu3 );
191  exit( -1 );
192  } else {
193  mu2 = mu3;
194  }
195  /* has offdiagonal sum gone below input goal ? */
196  if( mu2 <= ( *epsilon * mu1 ) ) goto done;
197  } /* sweep */
198  done:;
199  *maxsweep = sweep;
200  *maxannil = annil;
201  *epsilon = offdiag;
202  sortd( n, d, ind );
203  }
204 
205 } // namespace
void sortd(int length, double *a, int *ind)
Definition: eig_jacobi.cpp:24
lu decomposition and linear LMS solver
void eig_jacobi(int n, double a[3][3], double v[3][3], double *d, int *ind, int *maxsweep, int *maxannil, double *epsilon)
Implements jacobi eigenvalue/vector algorithm on a symmetric matrix stored as a 2 dimensional matrix ...
Definition: eig_jacobi.cpp:68