GestureRecognitionToolkit  Version: 0.2.5
The Gesture Recognition Toolkit (GRT) is a cross-platform, open-source, c++ machine learning library for real-time gesture recognition.
SVD.cpp
1 /*
2  GRT MIT License
3  Copyright (c) <2012> <Nicholas Gillian, Media Lab, MIT>
4 
5  Permission is hereby granted, free of charge, to any person obtaining a copy of this software
6  and associated documentation files (the "Software"), to deal in the Software without restriction,
7  including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
8  and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
9  subject to the following conditions:
10 
11  The above copyright notice and this permission notice shall be included in all copies or substantial
12  portions of the Software.
13 
14  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
15  LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
16  IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
17  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
18  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
19  */
20 #define GRT_DLL_EXPORTS
21 #include "SVD.h"
22 
23 GRT_BEGIN_NAMESPACE
24 
25 bool SVD::solve(MatrixFloat &a){
26 
27  //Setup the memory
28  m = a.getNumRows();
29  n = a.getNumCols();
30  u = a;
31  v.resize(n,n);
32  w.resize(n);
33 
34  eps = std::numeric_limits< Float >::epsilon();
35  if( !decompose() ) return false;
36  if( !reorder() ) return false;
37 
38  tsh = 0.5*grt_sqrt(m+n+1.)*w[0]*eps;
39 
40  return true;
41 }
42 
43 bool SVD::solveVector(VectorFloat &b, VectorFloat &x, Float thresh) {
44  UINT i,j,jj;
45  Float s;
46  if(b.size() != m || x.size() != n){
47  return false;
48  }
49  VectorFloat tmp(n);
50  tsh = (thresh >= 0. ? thresh : 0.5*grt_sqrt(m+n+1.)*w[0]*eps);
51  for (j=0;j<n;j++) {
52  s=0.0;
53  if (w[j] > tsh) {
54  for (i=0;i<m;i++) s += u[i][j]*b[i];
55  s /= w[j];
56  }
57  tmp[j]=s;
58  }
59  for (j=0;j<n;j++) {
60  s=0.0;
61  for (jj=0;jj<n;jj++) s += v[j][jj]*tmp[jj];
62  x[j]=s;
63  }
64  return true;
65 }
66 
67 bool SVD::solve(MatrixFloat &b, MatrixFloat &x, Float thresh){
68  UINT i,j,m=b.getNumCols();
69  if (b.getNumRows() != n || x.getNumRows() != n || b.getNumCols() != x.getNumCols()){
70  return false;
71  }
72  VectorFloat xx(n);
73  for (j=0;j<m;j++) {
74  for (i=0;i<n;i++) xx[i] = b[i][j];
75  solveVector(xx,xx,thresh);
76  for (i=0;i<n;i++) x[i][j] = xx[i];
77  }
78  return true;
79 }
80 UINT SVD::rank(Float thresh) {
81  UINT j,nr=0;
82  tsh = (thresh >= 0. ? thresh : 0.5*grt_sqrt(m+n+1.)*w[0]*eps);
83  for (j=0;j<n;j++) if (w[j] > tsh) nr++;
84  return nr;
85 }
86 
87 UINT SVD::nullity(Float thresh) {
88  UINT j,nn=0;
89  tsh = (thresh >= 0. ? thresh : 0.5*grt_sqrt(m+n+1.)*w[0]*eps);
90  for (j=0;j<n;j++) if (w[j] <= tsh) nn++;
91  return nn;
92 }
93 
94 MatrixFloat SVD::range(Float thresh){
95  UINT i,j,nr=0;
96  MatrixFloat rnge(m,rank(thresh));
97  for (j=0;j<n;j++) {
98  if (w[j] > tsh) {
99  for (i=0;i<m;i++) rnge[i][nr] = u[i][j];
100  nr++;
101  }
102  }
103  return rnge;
104 }
105 
106 MatrixFloat SVD::nullspace(Float thresh){
107  UINT j,jj,nn=0;
108  MatrixFloat nullsp(n,nullity(thresh));
109  for (j=0;j<n;j++) {
110  if (w[j] <= tsh) {
111  for (jj=0;jj<n;jj++) nullsp[jj][nn] = v[jj][j];
112  nn++;
113  }
114  }
115  return nullsp;
116 }
117 
118 Float SVD::inv_condition() {
119  return (w[0] <= 0. || w[n-1] <= 0.) ? 0. : w[n-1]/w[0];
120 }
121 
122 bool SVD::decompose() {
123  bool flag;
124  int i,its,j,jj,k,l,nm,N,M;
125  Float anorm,c,f,g,h,s,scale,x,y,z;
126  VectorFloat rv1(n);
127  g = scale = anorm = 0.0;
128  N = int(n);
129  M = int(m);
130  l = 0;
131  for (i=0;i<N;i++) {
132  l=i+2;
133  rv1[i]=scale*g;
134  g=s=scale=0.0;
135  if (i < M) {
136  for (k=i;k<M;k++) scale += fabs(u[k][i]);
137  if (scale != 0.0) {
138  for (k=i;k<M;k++) {
139  u[k][i] /= scale;
140  s+= u[k][i]*u[k][i];
141  }
142  f=u[i][i];
143  g = -SIGN(grt_sqrt(s),f);
144  h=f*g-s;
145  u[i][i]=f-g;
146  for (j=l-1;j<N;j++) {
147  for (s=0.0,k=i;k<M;k++) s += u[k][i] * u[k][j];
148  f=s/h;
149  for (k=i;k<M;k++) u[k][j] += f * u[k][i];
150  }
151  for (k=i;k<M;k++) u[k][i] *= scale;
152  }
153  }
154  w[i]=scale *g;
155  g=s=scale=0.0;
156  if (i+1 <= M && i+1 != N) {
157  for (k=l-1;k<N;k++) scale += fabs(u[i][k]);
158  if (scale != 0.0) {
159  for (k=l-1;k<N;k++) {
160  u[i][k] /= scale;
161  s += u[i][k]*u[i][k];
162  }
163  f=u[i][l-1];
164  g = -SIGN(grt_sqrt(s),f);
165  h=f*g-s;
166  u[i][l-1]=f-g;
167  for (k=l-1;k<N;k++) rv1[k]=u[i][k]/h;
168  for (j=l-1;j<M;j++) {
169  for (s=0.0,k=l-1;k<N;k++) s += u[j][k]*u[i][k];
170  for (k=l-1;k<N;k++) u[j][k] += s*rv1[k];
171  }
172  for (k=l-1;k<N;k++) u[i][k]*= scale;
173  }
174  }
175  anorm=grt_max(anorm,(fabs(w[i])+fabs(rv1[i])));
176  }
177  for (i=N-1;i>=0;i--) {
178  if (i < N-1) {
179  if (g != 0.0) {
180  for (j=l;j<N;j++)
181  v[j][i]=u[i][j]/u[i][l]/g;
182  for (j=l;j<N;j++) {
183  for (s=0.0,k=l;k<N;k++) s += u[i][k]*v[k][j];
184  for (k=l;k<N;k++) v[k][j] += s*v[k][i];
185  }
186  }
187  for (j=l;j<N;j++) v[i][j]=v[j][i]=0.0;
188  }
189  v[i][i]=1.0;
190  g=rv1[i];
191  l=i;
192  }
193  for (i=grt_min(M,N)-1;i>=0;i--) {
194  l=i+1;
195  g=w[i];
196  for (j=l;j<N;j++) u[i][j]=0.0;
197  if (g != 0.0) {
198  g=1.0/g;
199  for (j=l;j<N;j++) {
200  for (s=0.0,k=l;k<M;k++) s += u[k][i]*u[k][j];
201  f=(s/u[i][i])*g;
202  for (k=i;k<M;k++) u[k][j] += f*u[k][i];
203  }
204  for (j=i;j<M;j++) u[j][i] *= g;
205  } else for (j=i;j<M;j++) u[j][i] =0.0;
206  ++u[i][i];
207  }
208  for (k=N-1;k>=0;k--) {
209  for (its=0;its<MAX_NUM_SVD_ITER;its++) {
210  flag=true;
211  for (l=k;l>=0;l--) {
212  nm=l-1;
213  if (l == 0 || fabs(rv1[l]) <= eps*anorm) {
214  flag=false;
215  break;
216  }
217  if (fabs(w[nm]) <= eps*anorm) break;
218  }
219  if (flag) {
220  c=0.0;
221  s=1.0;
222  for (i=l;i<k+1;i++) {
223  f=s*rv1[i];
224  rv1[i]=c*rv1[i];
225  if (fabs(f) <= eps*anorm) break;
226  g=w[i];
227  h=pythag(f,g);
228  w[i]=h;
229  h=1.0/h;
230  c=g*h;
231  s = -f*h;
232  for (j=0;j<M;j++) {
233  y=u[j][nm];
234  z=u[j][i];
235  u[j][nm]=y*c+z*s;
236  u[j][i]=z*c-y*s;
237  }
238  }
239  }
240  z=w[k];
241  if (l == k) {
242  if (z < 0.0) {
243  w[k] = -z;
244  for (j=0;j<N;j++) v[j][k] = -v[j][k];
245  }
246  break;
247  }
248  if (its == MAX_NUM_SVD_ITER-1){
249  return false;
250  }
251  x=w[l];
252  nm=k-1;
253  y=w[nm];
254  g=rv1[nm];
255  h=rv1[k];
256  f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
257  g=pythag(f,1.0);
258  f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
259  c=s=1.0;
260  for (j=l;j<=nm;j++) {
261  i=j+1;
262  g=rv1[i];
263  y=w[i];
264  h=s*g;
265  g=c*g;
266  z=pythag(f,h);
267  rv1[j]=z;
268  c=f/z;
269  s=h/z;
270  f=x*c+g*s;
271  g=g*c-x*s;
272  h=y*s;
273  y *= c;
274  for (jj=0;jj<N;jj++) {
275  x=v[jj][j];
276  z=v[jj][i];
277  v[jj][j]=x*c+z*s;
278  v[jj][i]=z*c-x*s;
279  }
280  z=pythag(f,h);
281  w[j]=z;
282  if (z) {
283  z=1.0/z;
284  c=f*z;
285  s=h*z;
286  }
287  f=c*g+s*y;
288  x=c*y-s*g;
289  for (jj=0;jj<M;jj++) {
290  y=u[jj][j];
291  z=u[jj][i];
292  u[jj][j]=y*c+z*s;
293  u[jj][i]=z*c-y*s;
294  }
295  }
296  rv1[l]=0.0;
297  rv1[k]=f;
298  w[k]=x;
299  }
300  }
301 
302  return true;
303 }
304 
305 bool SVD::reorder() {
306  UINT i,j,k,s,inc=1;
307  Float sw;
308  VectorFloat su(m);
309  VectorFloat sv(n);
310  do { inc *= 3; inc++; } while (inc <= n);
311  do {
312  inc /= 3;
313  for (i=inc;i<n;i++) {
314  sw = w[i];
315  for (k=0;k<m;k++) su[k] = u[k][i];
316  for (k=0;k<n;k++) sv[k] = v[k][i];
317  j = i;
318  while (w[j-inc] < sw) {
319  w[j] = w[j-inc];
320  for (k=0;k<m;k++) u[k][j] = u[k][j-inc];
321  for (k=0;k<n;k++) v[k][j] = v[k][j-inc];
322  j -= inc;
323  if (j < inc) break;
324  }
325  w[j] = sw;
326  for (k=0;k<m;k++) u[k][j] = su[k];
327  for (k=0;k<n;k++) v[k][j] = sv[k];
328  }
329  } while (inc > 1);
330  for (k=0;k<n;k++) {
331  s=0;
332  for (i=0;i<m;i++) if (u[i][k] < 0.) s++;
333  for (j=0;j<n;j++) if (v[j][k] < 0.) s++;
334  if (s > (m+n)/2) {
335  for (i=0;i<m;i++) u[i][k] = -u[i][k];
336  for (j=0;j<n;j++) v[j][k] = -v[j][k];
337  }
338  }
339  return true;
340 }
341 
342 Float SVD::pythag(const Float a, const Float b) {
343  Float absa=fabs(a);
344  Float absb=fabs(b);
345  return (absa > absb ? absa*grt_sqrt(1.0+grt_sqr(absb/absa)) : (absb == 0.0 ? 0.0 : absb*grt_sqrt(1.0+grt_sqr(absa/absb))));
346 }
347 
348 GRT_END_NAMESPACE
virtual bool resize(const unsigned int size)
Definition: Vector.h:133
unsigned int getNumRows() const
Definition: Matrix.h:574
unsigned int getNumCols() const
Definition: Matrix.h:581
virtual bool resize(const unsigned int r, const unsigned int c)
Definition: Matrix.h:245