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

Revision 86, 11.6 KB checked in by dave, 11 years ago (diff)

lots of code added - pca implementation for eigenfaces and lda started

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 "SVD.h"
60
61#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
62#define MAX(x,y) ((x)>(y)?(x):(y))
63
64static double PYTHAG(double a, double b)
65{
66    double at = fabs(a), bt = fabs(b), ct, result;
67
68    if (at > bt)       { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
69    else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
70    else result = 0.0;
71    return(result);
72}
73
74Vector<float> SVD(Matrix<float> &m)
75{
76        Vector<float> w(m.GetRows());
77        Matrix<float> v(m.GetRows(),m.GetCols());
78        dsvd(m, m.GetRows(), m.GetCols(), w.GetRawData(), v);
79        return w;
80}
81
82int dsvd(Matrix<float> &a, int m, int n, float *w, Matrix<float> &v)
83{
84    int flag, i, its, j, jj, k, l, nm;
85    double c, f, h, s, x, y, z;
86    double anorm = 0.0, g = 0.0, scale = 0.0;
87    double *rv1;
88 
89    if (m < n)
90    {
91        fprintf(stderr, "#rows must be > #cols \n");
92        return(0);
93    }
94 
95    rv1 = (double *)malloc((unsigned int) n*sizeof(double));
96
97/* Householder reduction to bidiagonal form */
98    for (i = 0; i < n; i++)
99    {
100        /* left-hand reduction */
101        l = i + 1;
102        rv1[i] = scale * g;
103        g = s = scale = 0.0;
104        if (i < m)
105        {
106            for (k = i; k < m; k++)
107                scale += fabs((double)a[k][i]);
108            if (scale)
109            {
110                for (k = i; k < m; k++)
111                {
112                    a[k][i] = (float)((double)a[k][i]/scale);
113                    s += ((double)a[k][i] * (double)a[k][i]);
114                }
115                f = (double)a[i][i];
116                g = -SIGN(sqrt(s), f);
117                h = f * g - s;
118                a[i][i] = (float)(f - g);
119                if (i != n - 1)
120                {
121                    for (j = l; j < n; j++)
122                    {
123                        for (s = 0.0, k = i; k < m; k++)
124                            s += ((double)a[k][i] * (double)a[k][j]);
125                        f = s / h;
126                        for (k = i; k < m; k++)
127                            a[k][j] += (float)(f * (double)a[k][i]);
128                    }
129                }
130                for (k = i; k < m; k++)
131                    a[k][i] = (float)((double)a[k][i]*scale);
132            }
133        }
134        w[i] = (float)(scale * g);
135   
136        /* right-hand reduction */
137        g = s = scale = 0.0;
138        if (i < m && i != n - 1)
139        {
140            for (k = l; k < n; k++)
141                scale += fabs((double)a[i][k]);
142            if (scale)
143            {
144                for (k = l; k < n; k++)
145                {
146                    a[i][k] = (float)((double)a[i][k]/scale);
147                    s += ((double)a[i][k] * (double)a[i][k]);
148                }
149                f = (double)a[i][l];
150                g = -SIGN(sqrt(s), f);
151                h = f * g - s;
152                a[i][l] = (float)(f - g);
153                for (k = l; k < n; k++)
154                    rv1[k] = (double)a[i][k] / h;
155                if (i != m - 1)
156                {
157                    for (j = l; j < m; j++)
158                    {
159                        for (s = 0.0, k = l; k < n; k++)
160                            s += ((double)a[j][k] * (double)a[i][k]);
161                        for (k = l; k < n; k++)
162                            a[j][k] += (float)(s * rv1[k]);
163                    }
164                }
165                for (k = l; k < n; k++)
166                    a[i][k] = (float)((double)a[i][k]*scale);
167            }
168        }
169        anorm = MAX(anorm, (fabs((double)w[i]) + fabs(rv1[i])));
170    }
171 
172    /* accumulate the right-hand transformation */
173    for (i = n - 1; i >= 0; i--)
174    {
175        if (i < n - 1)
176        {
177            if (g)
178            {
179                for (j = l; j < n; j++)
180                    v[j][i] = (float)(((double)a[i][j] / (double)a[i][l]) / g);
181                    /* double division to avoid underflow */
182                for (j = l; j < n; j++)
183                {
184                    for (s = 0.0, k = l; k < n; k++)
185                        s += ((double)a[i][k] * (double)v[k][j]);
186                    for (k = l; k < n; k++)
187                        v[k][j] += (float)(s * (double)v[k][i]);
188                }
189            }
190            for (j = l; j < n; j++)
191                v[i][j] = v[j][i] = 0.0;
192        }
193        v[i][i] = 1.0;
194        g = rv1[i];
195        l = i;
196    }
197 
198    /* accumulate the left-hand transformation */
199    for (i = n - 1; i >= 0; i--)
200    {
201        l = i + 1;
202        g = (double)w[i];
203        if (i < n - 1)
204            for (j = l; j < n; j++)
205                a[i][j] = 0.0;
206        if (g)
207        {
208            g = 1.0 / g;
209            if (i != n - 1)
210            {
211                for (j = l; j < n; j++)
212                {
213                    for (s = 0.0, k = l; k < m; k++)
214                        s += ((double)a[k][i] * (double)a[k][j]);
215                    f = (s / (double)a[i][i]) * g;
216                    for (k = i; k < m; k++)
217                        a[k][j] += (float)(f * (double)a[k][i]);
218                }
219            }
220            for (j = i; j < m; j++)
221                a[j][i] = (float)((double)a[j][i]*g);
222        }
223        else
224        {
225            for (j = i; j < m; j++)
226                a[j][i] = 0.0;
227        }
228        ++a[i][i];
229    }
230
231    /* diagonalize the bidiagonal form */
232    for (k = n - 1; k >= 0; k--)
233    {                             /* loop over singular values */
234        for (its = 0; its < 30; its++)
235        {                         /* loop over allowed iterations */
236            flag = 1;
237            for (l = k; l >= 0; l--)
238            {                     /* test for splitting */
239                nm = l - 1;
240                if (fabs(rv1[l]) + anorm == anorm)
241                {
242                    flag = 0;
243                    break;
244                }
245                if (fabs((double)w[nm]) + anorm == anorm)
246                    break;
247            }
248            if (flag)
249            {
250                c = 0.0;
251                s = 1.0;
252                for (i = l; i <= k; i++)
253                {
254                    f = s * rv1[i];
255                    if (fabs(f) + anorm != anorm)
256                    {
257                        g = (double)w[i];
258                        h = PYTHAG(f, g);
259                        w[i] = (float)h;
260                        h = 1.0 / h;
261                        c = g * h;
262                        s = (- f * h);
263                        for (j = 0; j < m; j++)
264                        {
265                            y = (double)a[j][nm];
266                            z = (double)a[j][i];
267                            a[j][nm] = (float)(y * c + z * s);
268                            a[j][i] = (float)(z * c - y * s);
269                        }
270                    }
271                }
272            }
273            z = (double)w[k];
274            if (l == k)
275            {                  /* convergence */
276                if (z < 0.0)
277                {              /* make singular value nonnegative */
278                    w[k] = (float)(-z);
279                    for (j = 0; j < n; j++)
280                        v[j][k] = (-v[j][k]);
281                }
282                break;
283            }
284            if (its >= 30) {
285                free((void*) rv1);
286                fprintf(stderr, "No convergence after 30,000! iterations \n");
287                return(0);
288            }
289   
290            /* shift from bottom 2 x 2 minor */
291            x = (double)w[l];
292            nm = k - 1;
293            y = (double)w[nm];
294            g = rv1[nm];
295            h = rv1[k];
296            f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
297            g = PYTHAG(f, 1.0);
298            f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
299         
300            /* next QR transformation */
301            c = s = 1.0;
302            for (j = l; j <= nm; j++)
303            {
304                i = j + 1;
305                g = rv1[i];
306                y = (double)w[i];
307                h = s * g;
308                g = c * g;
309                z = PYTHAG(f, h);
310                rv1[j] = z;
311                c = f / z;
312                s = h / z;
313                f = x * c + g * s;
314                g = g * c - x * s;
315                h = y * s;
316                y = y * c;
317                for (jj = 0; jj < n; jj++)
318                {
319                    x = (double)v[jj][j];
320                    z = (double)v[jj][i];
321                    v[jj][j] = (float)(x * c + z * s);
322                    v[jj][i] = (float)(z * c - x * s);
323                }
324                z = PYTHAG(f, h);
325                w[j] = (float)z;
326                if (z)
327                {
328                    z = 1.0 / z;
329                    c = f * z;
330                    s = h * z;
331                }
332                f = (c * g) + (s * y);
333                x = (c * y) - (s * g);
334                for (jj = 0; jj < m; jj++)
335                {
336                    y = (double)a[jj][j];
337                    z = (double)a[jj][i];
338                    a[jj][j] = (float)(y * c + z * s);
339                    a[jj][i] = (float)(z * c - y * s);
340                }
341            }
342            rv1[l] = 0.0;
343            rv1[k] = f;
344            w[k] = (float)x;
345        }
346    }
347    free((void*) rv1);
348    return(1);
349}
350
Note: See TracBrowser for help on using the repository browser.