source: foam/trunk/vision/src/SVD.cpp @ 89

Revision 89, 11.8 KB checked in by dave, 11 years ago (diff)

renamed faceident as it's not so simple these days

Line 
1// Copyright (C) 2009 foam
2//
3// This program is free software; you can redistribute it and/or modify
4// it under the terms of the GNU General Public License as published by
5// the Free Software Foundation; either version 2 of the License, or
6// (at your option) any later version.
7//
8// This program is distributed in the hope that it will be useful,
9// but WITHOUT ANY WARRANTY; without even the implied warranty of
10// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11// GNU General Public License for more details.
12//
13// You should have received a copy of the GNU General Public License
14// along with this program; if not, write to the Free Software
15// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16
17// from http://www.public.iastate.edu/~dicook/JSS/paper/paper.html
18
19/************************************************************
20 *                                                          *
21 *  Permission is hereby granted  to  any  individual   or  *
22 *  institution   for  use,  copying, or redistribution of  *
23 *  this code and associated documentation,  provided       *
24 *  that   such  code  and documentation are not sold  for  *
25 *  profit and the  following copyright notice is retained  *
26 *  in the code and documentation:                          *
27 *     Copyright (c) held by Dianne Cook                    *
28 *  All Rights Reserved.                                    *
29 *                                                          *
30 *  Questions and comments are welcome, and I request       *
31 *  that you share any modifications with me.               *
32 *                                                          *
33 *                Dianne Cook                               *
34 *             dicook@iastate.edu                           *
35 *                                                          *
36 ************************************************************/
37
38/*
39 * svdcomp - SVD decomposition routine.
40 * Takes an mxn matrix a and decomposes it into udv, where u,v are
41 * left and right orthogonal transformation matrices, and d is a
42 * diagonal matrix of singular values.
43 *
44 * This routine is adapted from svdecomp.c in XLISP-STAT 2.1 which is
45 * code from Numerical Recipes adapted by Luke Tierney and David Betz.
46 *
47 * Input to dsvd is as follows:
48 *   a = mxn matrix to be decomposed, gets overwritten with u
49 *   m = row dimension of a
50 *   n = column dimension of a
51 *   w = returns the vector of singular values of a
52 *   v = returns the right orthogonal transformation matrix
53*/
54
55
56#include <stdio.h>
57#include <stdlib.h>
58#include <math.h>
59#include <algorithm>
60#include <list>
61#include "SVD.h"
62
63using namespace std;
64
65#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
66#define MAX(x,y) ((x)>(y)?(x):(y))
67
68///////////////////////////////////////////////////////
69
70Vector<float> SVD(Matrix<float> &m)
71{
72        Vector<float> w(m.GetRows());
73        Matrix<float> v(m.GetRows(),m.GetCols());
74        dsvd(m, m.GetRows(), m.GetCols(), w.GetRawData(), v);
75        m.SortCols(w);
76        return w;
77}
78
79///////////////////////////////////////////////////////
80
81static double PYTHAG(double a, double b)
82{
83    double at = fabs(a), bt = fabs(b), ct, result;
84
85    if (at > bt)       { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
86    else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
87    else result = 0.0;
88    return(result);
89}
90
91int dsvd(Matrix<float> &a, int m, int n, float *w, Matrix<float> &v)
92{
93    int flag, i, its, j, jj, k, l, nm;
94    double c, f, h, s, x, y, z;
95    double anorm = 0.0, g = 0.0, scale = 0.0;
96    double *rv1;
97 
98    if (m < n)
99    {
100        fprintf(stderr, "#rows must be > #cols \n");
101        return(0);
102    }
103 
104    rv1 = (double *)malloc((unsigned int) n*sizeof(double));
105
106/* Householder reduction to bidiagonal form */
107    for (i = 0; i < n; i++)
108    {
109        /* left-hand reduction */
110        l = i + 1;
111        rv1[i] = scale * g;
112        g = s = scale = 0.0;
113        if (i < m)
114        {
115            for (k = i; k < m; k++)
116                scale += fabs((double)a[k][i]);
117            if (scale)
118            {
119                for (k = i; k < m; k++)
120                {
121                    a[k][i] = (float)((double)a[k][i]/scale);
122                    s += ((double)a[k][i] * (double)a[k][i]);
123                }
124                f = (double)a[i][i];
125                g = -SIGN(sqrt(s), f);
126                h = f * g - s;
127                a[i][i] = (float)(f - g);
128                if (i != n - 1)
129                {
130                    for (j = l; j < n; j++)
131                    {
132                        for (s = 0.0, k = i; k < m; k++)
133                            s += ((double)a[k][i] * (double)a[k][j]);
134                        f = s / h;
135                        for (k = i; k < m; k++)
136                            a[k][j] += (float)(f * (double)a[k][i]);
137                    }
138                }
139                for (k = i; k < m; k++)
140                    a[k][i] = (float)((double)a[k][i]*scale);
141            }
142        }
143        w[i] = (float)(scale * g);
144   
145        /* right-hand reduction */
146        g = s = scale = 0.0;
147        if (i < m && i != n - 1)
148        {
149            for (k = l; k < n; k++)
150                scale += fabs((double)a[i][k]);
151            if (scale)
152            {
153                for (k = l; k < n; k++)
154                {
155                    a[i][k] = (float)((double)a[i][k]/scale);
156                    s += ((double)a[i][k] * (double)a[i][k]);
157                }
158                f = (double)a[i][l];
159                g = -SIGN(sqrt(s), f);
160                h = f * g - s;
161                a[i][l] = (float)(f - g);
162                for (k = l; k < n; k++)
163                    rv1[k] = (double)a[i][k] / h;
164                if (i != m - 1)
165                {
166                    for (j = l; j < m; j++)
167                    {
168                        for (s = 0.0, k = l; k < n; k++)
169                            s += ((double)a[j][k] * (double)a[i][k]);
170                        for (k = l; k < n; k++)
171                            a[j][k] += (float)(s * rv1[k]);
172                    }
173                }
174                for (k = l; k < n; k++)
175                    a[i][k] = (float)((double)a[i][k]*scale);
176            }
177        }
178        anorm = MAX(anorm, (fabs((double)w[i]) + fabs(rv1[i])));
179    }
180 
181    /* accumulate the right-hand transformation */
182    for (i = n - 1; i >= 0; i--)
183    {
184        if (i < n - 1)
185        {
186            if (g)
187            {
188                for (j = l; j < n; j++)
189                    v[j][i] = (float)(((double)a[i][j] / (double)a[i][l]) / g);
190                    /* double division to avoid underflow */
191                for (j = l; j < n; j++)
192                {
193                    for (s = 0.0, k = l; k < n; k++)
194                        s += ((double)a[i][k] * (double)v[k][j]);
195                    for (k = l; k < n; k++)
196                        v[k][j] += (float)(s * (double)v[k][i]);
197                }
198            }
199            for (j = l; j < n; j++)
200                v[i][j] = v[j][i] = 0.0;
201        }
202        v[i][i] = 1.0;
203        g = rv1[i];
204        l = i;
205    }
206 
207    /* accumulate the left-hand transformation */
208    for (i = n - 1; i >= 0; i--)
209    {
210        l = i + 1;
211        g = (double)w[i];
212        if (i < n - 1)
213            for (j = l; j < n; j++)
214                a[i][j] = 0.0;
215        if (g)
216        {
217            g = 1.0 / g;
218            if (i != n - 1)
219            {
220                for (j = l; j < n; j++)
221                {
222                    for (s = 0.0, k = l; k < m; k++)
223                        s += ((double)a[k][i] * (double)a[k][j]);
224                    f = (s / (double)a[i][i]) * g;
225                    for (k = i; k < m; k++)
226                        a[k][j] += (float)(f * (double)a[k][i]);
227                }
228            }
229            for (j = i; j < m; j++)
230                a[j][i] = (float)((double)a[j][i]*g);
231        }
232        else
233        {
234            for (j = i; j < m; j++)
235                a[j][i] = 0.0;
236        }
237        ++a[i][i];
238    }
239
240    /* diagonalize the bidiagonal form */
241    for (k = n - 1; k >= 0; k--)
242    {                             /* loop over singular values */
243        for (its = 0; its < 30; its++)
244        {                         /* loop over allowed iterations */
245            flag = 1;
246            for (l = k; l >= 0; l--)
247            {                     /* test for splitting */
248                nm = l - 1;
249                if (fabs(rv1[l]) + anorm == anorm)
250                {
251                    flag = 0;
252                    break;
253                }
254                if (fabs((double)w[nm]) + anorm == anorm)
255                    break;
256            }
257            if (flag)
258            {
259                c = 0.0;
260                s = 1.0;
261                for (i = l; i <= k; i++)
262                {
263                    f = s * rv1[i];
264                    if (fabs(f) + anorm != anorm)
265                    {
266                        g = (double)w[i];
267                        h = PYTHAG(f, g);
268                        w[i] = (float)h;
269                        h = 1.0 / h;
270                        c = g * h;
271                        s = (- f * h);
272                        for (j = 0; j < m; j++)
273                        {
274                            y = (double)a[j][nm];
275                            z = (double)a[j][i];
276                            a[j][nm] = (float)(y * c + z * s);
277                            a[j][i] = (float)(z * c - y * s);
278                        }
279                    }
280                }
281            }
282            z = (double)w[k];
283            if (l == k)
284            {                  /* convergence */
285                if (z < 0.0)
286                {              /* make singular value nonnegative */
287                    w[k] = (float)(-z);
288                    for (j = 0; j < n; j++)
289                        v[j][k] = (-v[j][k]);
290                }
291                break;
292            }
293            if (its >= 30) {
294                free((void*) rv1);
295                fprintf(stderr, "No convergence after 30,000! iterations \n");
296                return(0);
297            }
298   
299            /* shift from bottom 2 x 2 minor */
300            x = (double)w[l];
301            nm = k - 1;
302            y = (double)w[nm];
303            g = rv1[nm];
304            h = rv1[k];
305            f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
306            g = PYTHAG(f, 1.0);
307            f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
308         
309            /* next QR transformation */
310            c = s = 1.0;
311            for (j = l; j <= nm; j++)
312            {
313                i = j + 1;
314                g = rv1[i];
315                y = (double)w[i];
316                h = s * g;
317                g = c * g;
318                z = PYTHAG(f, h);
319                rv1[j] = z;
320                c = f / z;
321                s = h / z;
322                f = x * c + g * s;
323                g = g * c - x * s;
324                h = y * s;
325                y = y * c;
326                for (jj = 0; jj < n; jj++)
327                {
328                    x = (double)v[jj][j];
329                    z = (double)v[jj][i];
330                    v[jj][j] = (float)(x * c + z * s);
331                    v[jj][i] = (float)(z * c - x * s);
332                }
333                z = PYTHAG(f, h);
334                w[j] = (float)z;
335                if (z)
336                {
337                    z = 1.0 / z;
338                    c = f * z;
339                    s = h * z;
340                }
341                f = (c * g) + (s * y);
342                x = (c * y) - (s * g);
343                for (jj = 0; jj < m; jj++)
344                {
345                    y = (double)a[jj][j];
346                    z = (double)a[jj][i];
347                    a[jj][j] = (float)(y * c + z * s);
348                    a[jj][i] = (float)(z * c - y * s);
349                }
350            }
351            rv1[l] = 0.0;
352            rv1[k] = f;
353            w[k] = (float)x;
354        }
355    }
356    free((void*) rv1);
357    return(1);
358}
359
Note: See TracBrowser for help on using the repository browser.