eig3.cpp
Go to the documentation of this file.
1 
2 /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
3  domain Java Matrix library JAMA. */
4 
5 #include <math.h>
6 
7 #ifndef MAX
8 #define MAX(a, b) ((a)>(b)?(a):(b))
9 #endif
10 
11 #define n 3
12 
13 static double hypot2(double x, double y) {
14  return sqrt(x*x+y*y);
15 }
16 
17 // Symmetric Householder reduction to tridiagonal form.
18 
19 static void tred2(double V[n][n], double d[n], double e[n]) {
20 
21 // This is derived from the Algol procedures tred2 by
22 // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
23 // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
24 // Fortran subroutine in EISPACK.
25 
26  int i,j,k;
27  double f,g,h,hh;
28  for (j = 0; j < n; j++) {
29  d[j] = V[n-1][j];
30  }
31 
32  // Householder reduction to tridiagonal form.
33 
34  for (i = n-1; i > 0; i--) {
35 
36  // Scale to avoid under/overflow.
37 
38  double scale = 0.0;
39  double h = 0.0;
40  for (k = 0; k < i; k++) {
41  scale = scale + fabs(d[k]);
42  }
43  if (scale == 0.0) {
44  e[i] = d[i-1];
45  for (j = 0; j < i; j++) {
46  d[j] = V[i-1][j];
47  V[i][j] = 0.0;
48  V[j][i] = 0.0;
49  }
50  } else {
51 
52  // Generate Householder vector.
53 
54  for (k = 0; k < i; k++) {
55  d[k] /= scale;
56  h += d[k] * d[k];
57  }
58  f = d[i-1];
59  g = sqrt(h);
60  if (f > 0) {
61  g = -g;
62  }
63  e[i] = scale * g;
64  h = h - f * g;
65  d[i-1] = f - g;
66  for (j = 0; j < i; j++) {
67  e[j] = 0.0;
68  }
69 
70  // Apply similarity transformation to remaining columns.
71 
72  for (j = 0; j < i; j++) {
73  f = d[j];
74  V[j][i] = f;
75  g = e[j] + V[j][j] * f;
76  for (k = j+1; k <= i-1; k++) {
77  g += V[k][j] * d[k];
78  e[k] += V[k][j] * f;
79  }
80  e[j] = g;
81  }
82  f = 0.0;
83  for (j = 0; j < i; j++) {
84  e[j] /= h;
85  f += e[j] * d[j];
86  }
87  hh = f / (h + h);
88  for (j = 0; j < i; j++) {
89  e[j] -= hh * d[j];
90  }
91  for (j = 0; j < i; j++) {
92  f = d[j];
93  g = e[j];
94  for (k = j; k <= i-1; k++) {
95  V[k][j] -= (f * e[k] + g * d[k]);
96  }
97  d[j] = V[i-1][j];
98  V[i][j] = 0.0;
99  }
100  }
101  d[i] = h;
102  }
103 
104  // Accumulate transformations.
105 
106  for (i = 0; i < n-1; i++) {
107  V[n-1][i] = V[i][i];
108  V[i][i] = 1.0;
109  h = d[i+1];
110  if (h != 0.0) {
111  for (k = 0; k <= i; k++) {
112  d[k] = V[k][i+1] / h;
113  }
114  for (j = 0; j <= i; j++) {
115  g = 0.0;
116  for (k = 0; k <= i; k++) {
117  g += V[k][i+1] * V[k][j];
118  }
119  for (k = 0; k <= i; k++) {
120  V[k][j] -= g * d[k];
121  }
122  }
123  }
124  for (k = 0; k <= i; k++) {
125  V[k][i+1] = 0.0;
126  }
127  }
128  for (j = 0; j < n; j++) {
129  d[j] = V[n-1][j];
130  V[n-1][j] = 0.0;
131  }
132  V[n-1][n-1] = 1.0;
133  e[0] = 0.0;
134 }
135 
136 // Symmetric tridiagonal QL algorithm.
137 
138 static void tql2(double V[n][n], double d[n], double e[n]) {
139 
140 // This is derived from the Algol procedures tql2, by
141 // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
142 // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
143 // Fortran subroutine in EISPACK.
144 
145  int i,j,m,l,k;
146  double g,p,r,dl1,h,f,tst1,eps;
147  double c,c2,c3,el1,s,s2;
148 
149  for (i = 1; i < n; i++) {
150  e[i-1] = e[i];
151  }
152  e[n-1] = 0.0;
153 
154  f = 0.0;
155  tst1 = 0.0;
156  eps = pow(2.0,-52.0);
157  for (l = 0; l < n; l++) {
158 
159  // Find small subdiagonal element
160 
161  tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
162  m = l;
163  while (m < n) {
164  if (fabs(e[m]) <= eps*tst1) {
165  break;
166  }
167  m++;
168  }
169 
170  // If m == l, d[l] is an eigenvalue,
171  // otherwise, iterate.
172 
173  if (m > l) {
174  int iter = 0;
175  do {
176  iter = iter + 1; // (Could check iteration count here.)
177 
178  // Compute implicit shift
179 
180  g = d[l];
181  p = (d[l+1] - g) / (2.0 * e[l]);
182  r = hypot2(p,1.0);
183  if (p < 0) {
184  r = -r;
185  }
186  d[l] = e[l] / (p + r);
187  d[l+1] = e[l] * (p + r);
188  dl1 = d[l+1];
189  h = g - d[l];
190  for (i = l+2; i < n; i++) {
191  d[i] -= h;
192  }
193  f = f + h;
194 
195  // Implicit QL transformation.
196 
197  p = d[m];
198  c = 1.0;
199  c2 = c;
200  c3 = c;
201  el1 = e[l+1];
202  s = 0.0;
203  s2 = 0.0;
204  for (i = m-1; i >= l; i--) {
205  c3 = c2;
206  c2 = c;
207  s2 = s;
208  g = c * e[i];
209  h = c * p;
210  r = hypot2(p,e[i]);
211  e[i+1] = s * r;
212  s = e[i] / r;
213  c = p / r;
214  p = c * d[i] - s * g;
215  d[i+1] = h + s * (c * g + s * d[i]);
216 
217  // Accumulate transformation.
218 
219  for (k = 0; k < n; k++) {
220  h = V[k][i+1];
221  V[k][i+1] = s * V[k][i] + c * h;
222  V[k][i] = c * V[k][i] - s * h;
223  }
224  }
225  p = -s * s2 * c3 * el1 * e[l] / dl1;
226  e[l] = s * p;
227  d[l] = c * p;
228 
229  // Check for convergence.
230 
231  } while (fabs(e[l]) > eps*tst1);
232  }
233  d[l] = d[l] + f;
234  e[l] = 0.0;
235  }
236 
237  // Sort eigenvalues and corresponding vectors.
238 
239  for (i = 0; i < n-1; i++) {
240  k = i;
241  p = d[i];
242  for (j = i+1; j < n; j++) {
243  if (d[j] < p) {
244  k = j;
245  p = d[j];
246  }
247  }
248  if (k != i) {
249  d[k] = d[i];
250  d[i] = p;
251  for (j = 0; j < n; j++) {
252  p = V[j][i];
253  V[j][i] = V[j][k];
254  V[j][k] = p;
255  }
256  }
257  }
258 }
259 
260 void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
261  int i,j;
262  double e[n];
263  for (i = 0; i < n; i++) {
264  for (j = 0; j < n; j++) {
265  V[i][j] = A[i][j];
266  }
267  }
268  tred2(V, d, e);
269  tql2(V, d, e);
270 }
static void tql2(double V[n][n], double d[n], double e[n])
Definition: eig3.cpp:138
static double hypot2(double x, double y)
Definition: eig3.cpp:13
void eigen_decomposition(double A[n][n], double V[n][n], double d[n])
Definition: eig3.cpp:260
#define MAX(a, b)
Definition: eig3.cpp:8
unsigned int c
Definition: gfs2stream.cpp:41
static void tred2(double V[n][n], double d[n], double e[n])
Definition: eig3.cpp:19
#define n
Definition: eig3.cpp:11


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22