rpp_svd.cpp
Go to the documentation of this file.
1 /* ========================================================================
2  * PROJECT: ARToolKitPlus
3  * ========================================================================
4  *
5  * Copyright of the derived and new portions of this work
6  * (C) 2006 Graz University of Technology
7  *
8  * This framework is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This framework is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this framework; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21  *
22  * For further information please contact
23  * Dieter Schmalstieg
24  * <schmalstieg@icg.tu-graz.ac.at>
25  * Graz University of Technology,
26  * Institut for Computer Graphics and Vision,
27  * Inffeldgasse 16a, 8010 Graz, Austria.
28  * ========================================================================
29  ** @author Thomas Pintaric
30  *
31  * $Id$
32  * @file
33  * ======================================================================== */
34 
35 
36 #ifndef _NO_LIBRPP_
37 
38 
39 #include <math.h>
40 #include <assert.h>
41 
42 #include "rpp_types.h"
43 
44 
45 namespace rpp {
46 
47 typedef double SVD_FLOAT;
48 
49 
50 static SVD_FLOAT at,bt,ct;
51 static SVD_FLOAT maxarg1,maxarg2;
52 
53 #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
54  (ct=bt/at,at*sqrt(SVD_FLOAT(1.0f)+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(SVD_FLOAT(1.0f)+ct*ct)): SVD_FLOAT(0.0)))
55 
56 #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
57 
58 #define SIGN(a,b) ((b) >= SVD_FLOAT(0.0f) ? fabs(a) : -fabs(a))
59 
60 
61 int svdcmp( double **a, int m,int n, double *w,double **v)
62 {
63  int flag,i,its,j,jj,k,ii=0,nm=0;
64  SVD_FLOAT c,f,h,s,x,y,z;
65  SVD_FLOAT anorm=0.0,g=0.0,scale=0.0;
66 
67  if (m < n) return -1; // must augment A with extra zero rows
68 
69  assert(n==3);
70  SVD_FLOAT rv1[3]; // was: rv1=G_alloc_vector(n);
71 
72  n--;
73  m--;
74 
75  for (i=0;i<=n;i++) {
76  ii=i+1;
77  rv1[i]=scale*g;
78  g=s=scale=0.0;
79  if (i <= m) {
80  for (k=i;k<=m;k++) scale += (SVD_FLOAT)fabs(a[k][i]);
81  if (scale) {
82  for (k=i;k<=m;k++) {
83  a[k][i] /= (SVD_FLOAT)(scale);
84  s += a[k][i]*a[k][i];
85  }
86  f=a[i][i];
87  g = -SIGN(sqrt(s),f);
88  h=f*g-s;
89  a[i][i]=(SVD_FLOAT)(f-g);
90  if (i != n) {
91  for (j=ii;j<=n;j++) {
92  for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
93  f=s/h;
94  for (k=i;k<=m;k++) a[k][j] += (SVD_FLOAT)(f)*a[k][i];
95  }
96  }
97  for (k=i;k<=m;k++) a[k][i] *= (SVD_FLOAT)(scale);
98  }
99  }
100  w[i]=scale*g;
101  g=s=scale=0.0;
102  if (i <= m && i != n) {
103  for (k=ii;k<=n;k++) scale += fabs(a[i][k]);
104  if (scale) {
105  for (k=ii;k<=n;k++) {
106  a[i][k] /= scale;
107  s += a[i][k]*a[i][k];
108  }
109  f=a[i][ii];
110  g = -SIGN(sqrt(s),f);
111  h=f*g-s;
112  a[i][ii]=f-g;
113  for (k=ii;k<=n;k++) rv1[k]=a[i][k]/h;
114  if (i != m) {
115  for (j=ii;j<=m;j++) {
116  for (s=0.0,k=ii;k<=n;k++) s += a[j][k]*a[i][k];
117  for (k=ii;k<=n;k++) a[j][k] += s*rv1[k];
118  }
119  }
120  for (k=ii;k<=n;k++) a[i][k] *= scale;
121  }
122  }
123  anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
124  }
125  for (i=n;i>=0;i--) {
126  if (i < n) {
127  if (g) {
128  for (j=ii;j<=n;j++)
129  v[j][i]=(a[i][j]/a[i][ii])/g;
130  for (j=ii;j<=n;j++) {
131  for (s=0.0,k=ii;k<=n;k++) s += a[i][k]*v[k][j];
132  for (k=ii;k<=n;k++) v[k][j] += s*v[k][i];
133  }
134  }
135  for (j=ii;j<=n;j++) v[i][j]=v[j][i]=0.0;
136  }
137  v[i][i]=1.0;
138  g=rv1[i];
139  ii=i;
140  }
141  for (i=n;i>=0;i--) {
142  ii=i+1;
143  g=w[i];
144  if (i < n)
145  for (j=ii;j<=n;j++) a[i][j]=0.0;
146  if (g) {
147  g=SVD_FLOAT(1.0)/g;
148  if (i != n) {
149  for (j=ii;j<=n;j++) {
150  for (s=0.0,k=ii;k<=m;k++) s += a[k][i]*a[k][j];
151  f=(s/a[i][i])*g;
152  for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
153  }
154  }
155  for (j=i;j<=m;j++) a[j][i] *= g;
156  } else {
157  for (j=i;j<=m;j++) a[j][i]=0.0;
158  }
159  ++a[i][i];
160  }
161  for (k=n;k>=0;k--) {
162  for (its=1;its<=30;its++) {
163  flag=1;
164  for (ii=k;ii>=0;ii--) {
165  nm=ii-1;
166  if (fabs(rv1[ii])+anorm == anorm) {
167  flag=0;
168  break;
169  }
170  if (fabs(w[nm])+anorm == anorm) break;
171  }
172  if (flag) {
173  c=0.0;
174  s=1.0;
175  for (i=ii;i<=k;i++) {
176  f=s*rv1[i];
177  if (fabs(f)+anorm != anorm) {
178  g=w[i];
179  h=PYTHAG(f,g);
180  w[i]=h;
181  h=SVD_FLOAT(1.0)/h;
182  c=g*h;
183  s=(-f*h);
184  for (j=0;j<=m;j++) {
185  y=a[j][nm];
186  z=a[j][i];
187  a[j][nm]=y*c+z*s;
188  a[j][i]=z*c-y*s;
189  }
190  }
191  }
192  }
193  z=w[k];
194  if (ii == k) {
195  if (z < 0.0) {
196  w[k] = -z;
197  for (j=0;j<=n;j++) v[j][k]=(-v[j][k]);
198  }
199  break;
200  }
201  if (its == 30) return -2; /*No convergence in 30 SVDCMP iterations*/
202  x=w[ii];
203  nm=k-1;
204  y=w[nm];
205  g=rv1[nm];
206  h=rv1[k];
207  f=((y-z)*(y+z)+(g-h)*(g+h))/(SVD_FLOAT(2.0)*h*y);
208  g=PYTHAG(f,SVD_FLOAT(1.0));
209  f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
210  c=s=SVD_FLOAT(1.0);
211  for (j=ii;j<=nm;j++) {
212  i=j+1;
213  g=rv1[i];
214  y=w[i];
215  h=s*g;
216  g=c*g;
217  z=PYTHAG(f,h);
218  rv1[j]=z;
219  c=f/z;
220  s=h/z;
221  f=x*c+g*s;
222  g=g*c-x*s;
223  h=y*s;
224  y=y*c;
225  for (jj=0;jj<=n;jj++) {
226  x=v[jj][j];
227  z=v[jj][i];
228  v[jj][j]=x*c+z*s;
229  v[jj][i]=z*c-x*s;
230  }
231  z=PYTHAG(f,h);
232  w[j]=z;
233  if (z) {
234  z=SVD_FLOAT(1.0)/z;
235  c=f*z;
236  s=h*z;
237  }
238  f=(c*g)+(s*y);
239  x=(c*y)-(s*g);
240  for (jj=0;jj<=m;jj++) {
241  y=a[jj][j];
242  z=a[jj][i];
243  a[jj][j]=y*c+z*s;
244  a[jj][i]=z*c-y*s;
245  }
246  }
247  rv1[ii]=SVD_FLOAT(0.0);
248  rv1[k]=f;
249  w[k]=x;
250  }
251  }
252 
253  //G_free_vector(rv1); // no longer used, rv1 is now on stack
254 
255  return 0;
256 }
257 
258 #undef SIGN
259 #undef MAX
260 #undef PYTHAG
261 
262 /*
263 
264 int G_svbksb(
265  SVD_FLOAT **u,SVD_FLOAT w[],SVD_FLOAT **v,
266  int m,int n,SVD_FLOAT b[],SVD_FLOAT x[])
267 {
268  int j,i;
269  SVD_FLOAT s,*tmp; //,*G_alloc_vector();
270 
271  tmp=G_alloc_vector(n);
272  for (j=0;j<n;j++) {
273  s=0.0;
274  if (w[j]) {
275  for (i=0;i<m;i++) s += u[i][j]*b[i];
276  s /= w[j];
277  }
278  tmp[j]=s;
279  }
280  for (j=0;j<n;j++) {
281  s=0.0;
282  for (i=0;i<n;i++) s += v[j][i]*tmp[i];
283  x[j]=s;
284  }
285  G_free_vector(tmp);
286 
287  return 0;
288 }
289 
290 #define TOL 1e-8
291 
292 int G_svelim( SVD_FLOAT *w, int n)
293 {
294  int i;
295  SVD_FLOAT thresh;
296 
297  thresh=0.0; // remove singularity
298  for(i=0;i<n;i++)
299  if (w[i] > thresh)
300  thresh=w[i];
301  thresh *= TOL;
302  for(i=0;i<n;i++)
303  if (w[i] < thresh)
304  w[i]=0.0;
305 
306  return 0;
307 }
308 
309 #undef TOL
310 
311 */
312 
313 } // namespace rpp
314 
315 
316 #endif //_NO_LIBRPP_
rpp::bt
static SVD_FLOAT bt
Definition: rpp_svd.cpp:50
rpp::SVD_FLOAT
double SVD_FLOAT
Definition: rpp_svd.cpp:47
PYTHAG
#define PYTHAG(a, b)
Definition: rpp_svd.cpp:53
f
f
rpp::maxarg1
static SVD_FLOAT maxarg1
Definition: rpp_svd.cpp:51
s
XmlRpcServer s
rpp
Definition: rpp.cpp:55
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
SIGN
#define SIGN(a, b)
Definition: rpp_svd.cpp:58
rpp::maxarg2
static SVD_FLOAT maxarg2
Definition: rpp_svd.cpp:51
rpp::at
static SVD_FLOAT at
Definition: rpp_svd.cpp:50
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
rpp::svdcmp
int svdcmp(double **a, int m, int n, double *w, double **v)
Definition: rpp_svd.cpp:61
rpp::ct
static SVD_FLOAT ct
Definition: rpp_svd.cpp:50
MAX
#define MAX(a, b)
Definition: rpp_svd.cpp:56
rpp_types.h

tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun Sep 4 2016 03:24:33