vmmlib  1.7.0
 All Classes Namespaces Functions Pages
svd.hpp
1 /*
2  * Copyright (c) 2006-2014, Visualization and Multimedia Lab,
3  * University of Zurich <http://vmml.ifi.uzh.ch>,
4  * Eyescale Software GmbH,
5  * Blue Brain Project, EPFL
6  *
7  * This file is part of VMMLib <https://github.com/VMML/vmmlib/>
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * Redistributions of source code must retain the above copyright notice, this
13  * list of conditions and the following disclaimer. Redistributions in binary
14  * form must reproduce the above copyright notice, this list of conditions and
15  * the following disclaimer in the documentation and/or other materials provided
16  * with the distribution. Neither the name of the Visualization and Multimedia
17  * Lab, University of Zurich nor the names of its contributors may be used to
18  * endorse or promote products derived from this software without specific prior
19  * written permission.
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 #ifndef __VMML__SINGULAR_VALUE_DECOMPOSITION__HPP__
33 #define __VMML__SINGULAR_VALUE_DECOMPOSITION__HPP__
34 
35 #include <vmmlib/matrix.hpp>
36 #include <vmmlib/vector.hpp>
37 
38 #include <cmath>
39 
40 namespace vmml
41 {
42 
43 /*
44  * Given a matrix a[1..m][1..n], this routine computes its singular value
45  * decomposition, A = U·W·V T. The matrix U replaces a on output. The diagonal
46  * matrix of singular values W is output as a vector w[1..n]. The transpose V T
47  * (not the matrix V ! ) is output as v[1..n][1..n].
48  */
49 //static double rv1[16];
50 
51 template < size_t M, size_t N, typename T >
52 void svdecompose(
53  matrix< M, N, T >& a,
54  vector< N, T >& w,
55  matrix< N, N, T >& v
56  )
57 {
58  int m = M;
59  int n = N;
60  int flag, i, its, j, jj, k, l, nm = 0;
61  T anorm, c, f, g, h, s, scale, x, y, z;
62 
63  //T* rv1 = (T*)calloc( n, sizeof(T)); // vector(1,n);
64  vector< N, T > rv1;
65 
66  g = scale = anorm = 0.0; // Householder reduction to bidiagonal form.
67  for ( i = 0; i < n; ++i )
68  {
69  l = i + 1;
70  rv1[i] = scale * g;
71  g = s = scale = 0.0;
72  if ( i < m )
73  {
74  for ( k = i; k < m; ++k )
75  scale += fabs(a[k][i]);
76  if ( scale )
77  {
78  for ( k = i; k < m; ++k )
79  {
80  a[k][i] /= scale;
81  s += a[k][i] * a[k][i];
82  }
83  f = a[i][i];
84  g = -math::sign( static_cast< T >(sqrt(s)), f);
85  h = f * g - s;
86  a[i][i] = f - g;
87  for ( j = l; j < n; ++j )
88  {
89  for ( s = 0.0, k = i; k < m; ++k )
90  s += a[k][i] * a[k][j];
91  f = s / h;
92  for ( k = i; k < m; ++k )
93  a[k][j] += f * a[k][i];
94  }
95  for ( k = i; k < m; ++k )
96  a[k][i] *= scale;
97  }
98  }
99  w[i] = scale * g;
100  g = s = scale = 0.0;
101  if ( i < m && i != n-1 )
102  {
103  for ( k = l; k < n; ++k )
104  scale += fabs(a[i][k]);
105  if ( scale )
106  {
107  for ( k = l; k < n; ++k )
108  {
109  a[i][k] /= scale;
110  s += a[i][k] * a[i][k];
111  }
112  f = a[i][l];
113  g = -math::sign( static_cast< T >( sqrt(s)), f);
114  h = f * g - s;
115  a[i][l] = f - g;
116  for ( k = l; k < n; ++k )
117  rv1[k] = a[i][k] / h;
118  for ( j = l; j < m; ++j )
119  {
120  for ( s = 0.0, k = l; k < n; ++k )
121  s += a[j][k] * a[i][k];
122  for ( k = l; k < n; ++k )
123  a[j][k] += s * rv1[k];
124  }
125  for ( k = l; k < n; ++k )
126  a[i][k] *= scale;
127  }
128  }
129  anorm = (std::max)( anorm, static_cast< T >( ( fabs( w[i] ) + fabs( rv1[i] ) ) ) );
130  }
131  for ( i = n-1; i >= 0; --i )
132  { // Accumulation of right-hand transformations.
133  if ( i < n )
134  {
135  if ( g )
136  {
137  for ( j = l; j < n; ++j ) // Double division to avoid possible underflow.
138  v[j][i] = (a[i][j] / a[i][l]) / g;
139  for ( j = l; j < n; ++j )
140  {
141  for ( s = 0.0, k = l; k < n; ++k )
142  s += a[i][k] * v[k][j];
143  for ( k = l; k < n; ++k )
144  v[k][j] += s * v[k][i];
145  }
146  }
147  for ( j = l; j < n; ++j )
148  v[i][j] = v[j][i] = 0.0;
149  }
150  v[i][i] = 1.0;
151  g = rv1[i];
152  l = i;
153  }
154  i = ( m < n ) ? m - 1 : n - 1;
155  for ( ; i >= 0; --i ) // IMIN
156  { // Accumulation of left-hand transformations.
157  l = i + 1;
158  g = w[i];
159  for ( j = l; j < n; ++j )
160  a[i][j] = 0.0;
161  if ( g )
162  {
163  g = 1.0 / g;
164  for ( j = l; j < n; ++j )
165  {
166  for ( s = 0.0, k = l; k < m; ++k )
167  s += a[k][i] * a[k][j];
168  f = (s / a[i][i]) * g;
169  for ( k = i; k < m; ++k )
170  a[k][j] += f * a[k][i];
171  }
172  for ( j = i; j < m; ++j )
173  a[j][i] *= g;
174  }
175  else
176  for ( j = i; j < m; ++j )
177  a[j][i] = 0.0;
178  ++a[i][i];
179  }
180  for ( k = n-1; k >= 0; --k )
181  { // Diagonalization of the bidiagonal form: Loop over singular values,
182  // and over allowed iterations.
183  for ( its = 0; its < 30; ++its )
184  {
185  flag = 1;
186  for ( l = k; l >= 0; --l )
187  { // Test for splitting.
188  nm = l - 1; // Note that rv1[1] is always zero.
189  if ( ( fabs( rv1[l] ) + anorm ) == anorm )
190  {
191  flag = 0;
192  break;
193  }
194  if ( ( fabs( w[ nm ] ) + anorm ) == anorm )
195  break;
196  }
197  if ( flag )
198  {
199  c = 0.0; // Cancellation of rv1[l], if l > 1.
200  s = 1.0;
201  for ( i = l; i <= k; ++i )
202  {
203  f = s * rv1[i];
204  rv1[i] = c * rv1[i];
205  if ( ( fabs(f) + anorm ) == anorm )
206  break;
207  g = w[i];
208  h = math::pythag(f, g);
209  w[i] = h;
210  h = 1.0 / h;
211  c = g * h;
212  s = -f * h;
213  for ( j = 0; j < m; ++j )
214  {
215  y = a[j][nm];
216  z = a[j][i];
217  a[j][nm] = y * c + z * s;
218  a[j][i] = z * c - y * s;
219  }
220  }
221  }
222  z = w[k];
223  if ( l == k )
224  { // Convergence.
225  if ( z < 0.0 )
226  { // Singular value is made nonnegative.
227  w[k] = -z;
228  for ( j = 0; j < n; ++j )
229  v[j][k] = -v[j][k];
230  }
231  break;
232  }
233  if ( its == 30 )
234  {
235  //fprintf(stderr, "Warning: no convergence in 30 svdcmp iterations\n");
236  std::cerr << "SingularValueDecomposition - Warning: no convergence in 30 iterations." << std::endl;
237  }
238  x = w[l]; // Shift from bottom 2-by-2 minor.
239  nm = k - 1;
240  y = w[nm];
241  g = rv1[nm];
242  h = rv1[k];
243  f = ( (y-z) * (y+z) + (g-h) * (g+h) ) / (2.0 * h * y );
244  g = math::pythag( f, static_cast< T >( 1.0 ) );
245  f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + math::sign( g, f ) ) ) - h ) ) / x;
246  c = s = 1.0; // Next QR transformation:
247 
248  for ( j = l; j <= nm; ++j )
249  {
250  i = j + 1;
251  g = rv1[i];
252  y = w[i];
253  h = s * g;
254  g = c * g;
255  z = math::pythag( f, h );
256  rv1[j] = z;
257  c = f / z;
258  s = h / z;
259  f = x * c + g * s;
260  g = g * c - x * s;
261  h = y * s;
262  y *= c;
263  for ( jj = 0; jj < n; ++jj )
264  {
265  x = v[jj][j];
266  z = v[jj][i];
267  v[jj][j] = x * c + z * s;
268  v[jj][i] = z * c - x * s;
269  }
270  z = math::pythag( f, h );
271  w[j] = z; // Rotation can be arbitrary if z = 0.
272  if ( z )
273  {
274  z = 1.0 / z;
275  c = f * z;
276  s = h * z;
277  }
278  f = c * g + s * y;
279  x = c * y - s * g;
280  for ( jj = 0; jj < m; ++jj )
281  {
282  y = a[jj][j];
283  z = a[jj][i];
284  a[jj][j] = y * c + z * s;
285  a[jj][i] = z * c - y * s;
286  }
287  }
288  rv1[l] = 0.0;
289  rv1[k] = f;
290  w[k] = x;
291  }
292 
293  }
294  //free(rv1);
295  for ( i = 0; i < 3; ++i )
296  for ( j = 0; j < 3; ++j )
297  if ( i < j && w[i] < w[j] )
298  {
299  double t = w[i];
300  double u = w[j];
301  w[i] = u;
302  w[j] = t;
303 
304  for ( k = 0; k < 3; ++k )
305  {
306  t = v[i][k];
307  u = v[j][k];
308  v[i][k] = u * std::pow( -1., i );
309  v[j][k] = t * std::pow( -1., j );
310  t = a[k][i];
311  u = a[k][j];
312  a[k][i] = u * std::pow( -1., i );
313  a[k][j] = t * std::pow( -1., j );
314  }
315  }
316 
317  }
318 
319 }
320 
321 #endif