eig3.cpp
Go to the documentation of this file.
```00001
00002 /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
00003    domain Java Matrix library JAMA. */
00004
00005 #include <math.h>
00006
00007 #ifndef MAX
00008 #define MAX(a, b) ((a)>(b)?(a):(b))
00009 #endif
00010
00011 #define n 3
00012
00013 static double hypot2(double x, double y) {
00014   return sqrt(x*x+y*y);
00015 }
00016
00017 // Symmetric Householder reduction to tridiagonal form.
00018
00019 static void tred2(double V[n][n], double d[n], double e[n]) {
00020
00021 //  This is derived from the Algol procedures tred2 by
00022 //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
00023 //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
00024 //  Fortran subroutine in EISPACK.
00025
00026   int i,j,k;
00027   double f,g,h,hh;
00028   for (j = 0; j < n; j++) {
00029     d[j] = V[n-1][j];
00030   }
00031
00032   // Householder reduction to tridiagonal form.
00033
00034   for (i = n-1; i > 0; i--) {
00035
00036     // Scale to avoid under/overflow.
00037
00038     double scale = 0.0;
00039     double h = 0.0;
00040     for (k = 0; k < i; k++) {
00041       scale = scale + fabs(d[k]);
00042     }
00043     if (scale == 0.0) {
00044       e[i] = d[i-1];
00045       for (j = 0; j < i; j++) {
00046         d[j] = V[i-1][j];
00047         V[i][j] = 0.0;
00048         V[j][i] = 0.0;
00049       }
00050     } else {
00051
00052       // Generate Householder vector.
00053
00054       for (k = 0; k < i; k++) {
00055         d[k] /= scale;
00056         h += d[k] * d[k];
00057       }
00058       f = d[i-1];
00059       g = sqrt(h);
00060       if (f > 0) {
00061         g = -g;
00062       }
00063       e[i] = scale * g;
00064       h = h - f * g;
00065       d[i-1] = f - g;
00066       for (j = 0; j < i; j++) {
00067         e[j] = 0.0;
00068       }
00069
00070       // Apply similarity transformation to remaining columns.
00071
00072       for (j = 0; j < i; j++) {
00073         f = d[j];
00074         V[j][i] = f;
00075         g = e[j] + V[j][j] * f;
00076         for (k = j+1; k <= i-1; k++) {
00077           g += V[k][j] * d[k];
00078           e[k] += V[k][j] * f;
00079         }
00080         e[j] = g;
00081       }
00082       f = 0.0;
00083       for (j = 0; j < i; j++) {
00084         e[j] /= h;
00085         f += e[j] * d[j];
00086       }
00087       hh = f / (h + h);
00088       for (j = 0; j < i; j++) {
00089         e[j] -= hh * d[j];
00090       }
00091       for (j = 0; j < i; j++) {
00092         f = d[j];
00093         g = e[j];
00094         for (k = j; k <= i-1; k++) {
00095           V[k][j] -= (f * e[k] + g * d[k]);
00096         }
00097         d[j] = V[i-1][j];
00098         V[i][j] = 0.0;
00099       }
00100     }
00101     d[i] = h;
00102   }
00103
00104   // Accumulate transformations.
00105
00106   for (i = 0; i < n-1; i++) {
00107     V[n-1][i] = V[i][i];
00108     V[i][i] = 1.0;
00109     h = d[i+1];
00110     if (h != 0.0) {
00111       for (k = 0; k <= i; k++) {
00112         d[k] = V[k][i+1] / h;
00113       }
00114       for (j = 0; j <= i; j++) {
00115         g = 0.0;
00116         for (k = 0; k <= i; k++) {
00117           g += V[k][i+1] * V[k][j];
00118         }
00119         for (k = 0; k <= i; k++) {
00120           V[k][j] -= g * d[k];
00121         }
00122       }
00123     }
00124     for (k = 0; k <= i; k++) {
00125       V[k][i+1] = 0.0;
00126     }
00127   }
00128   for (j = 0; j < n; j++) {
00129     d[j] = V[n-1][j];
00130     V[n-1][j] = 0.0;
00131   }
00132   V[n-1][n-1] = 1.0;
00133   e[0] = 0.0;
00134 }
00135
00136 // Symmetric tridiagonal QL algorithm.
00137
00138 static void tql2(double V[n][n], double d[n], double e[n]) {
00139
00140 //  This is derived from the Algol procedures tql2, by
00141 //  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
00142 //  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
00143 //  Fortran subroutine in EISPACK.
00144
00145   int i,j,m,l,k;
00146   double g,p,r,dl1,h,f,tst1,eps;
00147   double c,c2,c3,el1,s,s2;
00148
00149   for (i = 1; i < n; i++) {
00150     e[i-1] = e[i];
00151   }
00152   e[n-1] = 0.0;
00153
00154   f = 0.0;
00155   tst1 = 0.0;
00156   eps = pow(2.0,-52.0);
00157   for (l = 0; l < n; l++) {
00158
00159     // Find small subdiagonal element
00160
00161     tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
00162     m = l;
00163     while (m < n) {
00164       if (fabs(e[m]) <= eps*tst1) {
00165         break;
00166       }
00167       m++;
00168     }
00169
00170     // If m == l, d[l] is an eigenvalue,
00171     // otherwise, iterate.
00172
00173     if (m > l) {
00174       int iter = 0;
00175       do {
00176         iter = iter + 1;  // (Could check iteration count here.)
00177
00178         // Compute implicit shift
00179
00180         g = d[l];
00181         p = (d[l+1] - g) / (2.0 * e[l]);
00182         r = hypot2(p,1.0);
00183         if (p < 0) {
00184           r = -r;
00185         }
00186         d[l] = e[l] / (p + r);
00187         d[l+1] = e[l] * (p + r);
00188         dl1 = d[l+1];
00189         h = g - d[l];
00190         for (i = l+2; i < n; i++) {
00191           d[i] -= h;
00192         }
00193         f = f + h;
00194
00195         // Implicit QL transformation.
00196
00197         p = d[m];
00198         c = 1.0;
00199         c2 = c;
00200         c3 = c;
00201         el1 = e[l+1];
00202         s = 0.0;
00203         s2 = 0.0;
00204         for (i = m-1; i >= l; i--) {
00205           c3 = c2;
00206           c2 = c;
00207           s2 = s;
00208           g = c * e[i];
00209           h = c * p;
00210           r = hypot2(p,e[i]);
00211           e[i+1] = s * r;
00212           s = e[i] / r;
00213           c = p / r;
00214           p = c * d[i] - s * g;
00215           d[i+1] = h + s * (c * g + s * d[i]);
00216
00217           // Accumulate transformation.
00218
00219           for (k = 0; k < n; k++) {
00220             h = V[k][i+1];
00221             V[k][i+1] = s * V[k][i] + c * h;
00222             V[k][i] = c * V[k][i] - s * h;
00223           }
00224         }
00225         p = -s * s2 * c3 * el1 * e[l] / dl1;
00226         e[l] = s * p;
00227         d[l] = c * p;
00228
00229         // Check for convergence.
00230
00231       } while (fabs(e[l]) > eps*tst1);
00232     }
00233     d[l] = d[l] + f;
00234     e[l] = 0.0;
00235   }
00236
00237   // Sort eigenvalues and corresponding vectors.
00238
00239   for (i = 0; i < n-1; i++) {
00240     k = i;
00241     p = d[i];
00242     for (j = i+1; j < n; j++) {
00243       if (d[j] < p) {
00244         k = j;
00245         p = d[j];
00246       }
00247     }
00248     if (k != i) {
00249       d[k] = d[i];
00250       d[i] = p;
00251       for (j = 0; j < n; j++) {
00252         p = V[j][i];
00253         V[j][i] = V[j][k];
00254         V[j][k] = p;
00255       }
00256     }
00257   }
00258 }
00259
00260 void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
00261   int i,j;
00262   double e[n];
00263   for (i = 0; i < n; i++) {
00264     for (j = 0; j < n; j++) {
00265       V[i][j] = A[i][j];
00266     }
00267   }
00268   tred2(V, d, e);
00269   tql2(V, d, e);
00270 }
```

openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21