source: trunk/projects/slim-curve/src/main/c/GCI_Lsqnonneg.c @ 7607

Revision 7607, 19.5 KB checked in by paulbarber, 9 years ago (diff)

Added single frequency phasor routine for lifetime.
Added Non-negative least squares for spectral unmixing.

Line 
1// Updated file from JG, 2.11.02
2
3/* Non-neg least squares library function
4
5   This code is based on code and descriptions in the book
6     Lawson, C.L. and Hanson, R.J., Solving Least Squares Problems,
7     Prentice-Hall, 1974
8
9   A brief description of the various algorithms used is included
10   within this file, but for a full explanation, please refer to the
11   above-mentioned book or a similar reference.
12
13   Julian Gilbey, Gray Cancer Institute, September 2002
14*/
15
16/* Include files we need */
17#include <stdio.h>   /* for NULL */
18#include <stdlib.h>  /* for malloc/free */
19#include <string.h>  /* for memcpy */
20#include <math.h>    /* for fabs and sqrt */
21#ifndef max
22#define max(a,b) ((a)<(b) ? (b) : (a))
23#endif
24
25/* The greatest number of variables to be determined.  This is to
26   allocate an array in advance to save having to use malloc/free */
27#define MAX_VARS 50
28
29/* The greatest number of equations to be used.  Again, this is
30   to allocate an array in advance */
31#define MAX_EQNS 10000
32
33/* Some tolerence calculations */
34/* Approximate machine epsilon */
35#define EPS 1e-9
36#define TOL (100*EPS)
37
38
39/**********************************************************************
40   Function Householder
41
42   This function takes as input a vector, a pivot element and a zero
43   point, and applies a Householder transformation to the vector to
44   make all of its entries zero from the point specified, pivoting
45   around the specified pivot entry, modifying the vector in the
46   process, as described below.  This function can also take a
47   collection of other vectors and apply this Householder
48   transformation to these vectors.
49
50   Here is a brief description of Householder transformations: we are
51   looking for an orthogonal transformation Q with
52
53      Qv = ( v_0 v_1 ... v_{p-1} -sigma.sqrt(v_p^2+sum_{i=l}^{m-1} v_i^2)
54                 v_{p+1} ... v_{l-1} 0 ... 0)^T =: y
55
56      where sigma = +1 if v_p >= 0 and -1 if v_p < 0.
57
58   Here p is the pivot index and l is the zeroing index, where we
59   assume 0 <= p <= l < m (where v is an m-vector, zero-based).  If
60   these assumptions are not true, we take Q=I.
61
62   Q is given by  Q = I - 2uu^T/|u|^2  for some vector u, but we do
63   not actually need to calculate it.  We do the following:
64
65   Compute: s := -sigma.sqrt(v_p^2+sum_{i=l}^m v_i^2)
66            u_i := 0, i=0, ..., p-1
67            u_p := v_p - s
68            u_i := 0, i=p+1, ..., l-1
69            u_i := v_i, i=l, ..., m-1
70            b := su_p  (so b = -|u|^2/2)
71            Q := ( I_m + b^{-1}uu^T  if b != 0
72                 ( I_m               if b = 0
73
74   After this, we place the non-zero components of u and y into the
75   storage previously occupied by v; we let v_p := y_p and store u_p
76   separately.
77
78   To apply this transformation to the set of m-vectors c_j, getting
79   d_j = Qc_j, we do:
80
81            t_j := b^{-1}(u^T c_j)
82            d_j := c_j + t_j u
83
84   if b != 0, and d_j := c_j if b = 0.
85
86   This function computes u, b, y:=Qv and d_j := Qc_j for all c_j we
87   are given.  If u and y have already been computed, we can apply
88   this Q to new c_j vectors.
89
90   Arguments: mode: 1 means calculate u, b, y and then Qc_j
91                    2 means u and y are precalculated, just calculate Qc_j
92              v, the vector to pivot
93              NB this vector is modified during execution of this
94                  function, as described above
95              p, the pivot
96              l, the zeroing point
97              m, the length of the vector v
98              u_p (double *), the calculated value of u_p
99              C, double **C, array of pointers to the c_j vectors
100              nC, number of c_j vectors
101              If C=NULL or nC=0, the code will not attempt to find any
102                  Qc_j.
103              NB the calculated d_j := Qc_j vectors will overwrite the
104                  C array
105
106   Here is the collected algorithm, as outlined above:
107
108   0  // Start here if mode = 1
109   1  s := sqrt(v_p^2 + sum_{i=l}^{m-1} v_i^2)
110      (Use the calculation trick described below under Givens
111      transformations to avoid calculation errors)
112   2  if v_p > 0, s := -s
113   3  u_p := v_p - s,  v_p := s
114   4  // Start here if mode = 2
115   5  b := v_p.u_p
116   6  if b=0 or no C, stop
117   7  for each c_j do steps 8-10
118   8  t_j := (c_j[p].u_p + sum_{i=l}^{m-1} c_j[i].v_i) / b
119   9  c_j[p] := c_j[p] + t_j.u_p
120   10 for i=l,...,m-1,  c_j[i] := c_j[i] + t_j.v_i
121
122*/
123 
124void Householder(int mode, double *v, int p, int l, int m, double *u_p,
125                                 double *C[], int nC)
126{
127        double vmax, vmax_inv, vtmp, s, b, tj;
128        double *cj;
129        int i, j;
130
131        if (0>p || p>=l || l>m) return;
132
133        /* find the maximum v_i */
134        vmax = fabs(v[p]);
135        if (mode == 1) {
136                for (i=l; i<m; i++)
137                        vmax = max(vmax, fabs(v[i]));
138                if (vmax == 0) return;  /* oops; not much to do here, really */
139                vmax_inv = 1/vmax;  /* only calculate this once; does it matter? */
140
141                /* we now calculate s, which is step 1 in the algorithm */
142                vtmp = v[p]*vmax_inv;
143                s = vtmp*vtmp;
144                for (i=l; i<m; i++) {
145                        vtmp = v[i]*vmax_inv;
146                        s += vtmp*vtmp;
147                }
148                s = vmax*sqrt(s);
149                /* this is step 1 done */
150                if (v[p]>0) s = -s;
151                *u_p = v[p]-s;
152                v[p] = s;
153        } else {
154                if (vmax == 0) return;  /* same test as before, only now we are
155                                                                   testing only the v_p element, which is
156                                                                   actually y_p */
157        }
158
159        /* Now calculate the Qc_j vectors */
160        if (nC <= 0 || C==NULL) return;
161        b = v[p] * (*u_p);
162        if (b >= 0) return;
163
164        for (j=0; j<nC; j++) {
165                cj = C[j];
166                tj = cj[p] * (*u_p);
167                for (i=l; i<m; i++)
168                        tj += cj[i]*v[i];
169
170                if (tj != 0) {
171                        tj /= b;
172                        cj[p] += tj*(*u_p);
173                        for (i=l; i<m; i++)
174                                cj[i] += tj*v[i];
175                }
176        }
177}
178
179
180/**********************************************************************
181   Functions GivensCalc, GivensApply
182
183   The GivensCalc function takes as input a 2-vector, and computes
184   the Givens rotation for this vector.
185
186   If the vector is v = (v_1 v_2)^T != 0, then the Givens rotation is
187   the 2x2 (special) orthogonal matrix
188
189                G = (  c  s )
190                    ( -s  c )
191
192   (with c^2 + s^2 = 1) such that Gv = (r 0)^T, where
193   r=|v|=sqrt(v_1^2 + v_2^2).
194
195   This function calculates c, s and r, avoiding overflow and
196   underflow issues, and returns these values.  (We can allow r to
197   overwrite v_1 or v_2 if we wish.)  The calculation is as follows:
198
199   To compute r=sqrt(x^2 + y^2):
200            t := max {|x|, |y|}
201            u := min {|x|, |y|}
202            r := ( t.sqrt(1+(u/t)^2)  if t != 0
203                 ( 0                  if t = 0
204
205   The function GivensApply applies a Givens rotation to a
206   2-vector.  No rocket science there.
207
208   Here, then, are the algorithms we use:
209
210   Calculate a Givens rotation for the vector (v_1 v_2)^T
211   (GivensCalc):
212
213   Arguments: v_1, v_2, the input vector
214              c, s, r, pointers to the answers.  r may point to v_1 or v_2
215
216   1  if |v_1| <= |v_2| go to step 8
217   2  w := v_2/v_1
218   3  q := sqrt(1+w^2)
219   4  c := 1/q
220   5  if v_1 < 0, c := -c
221   6  s := wc
222   7  r := |v_1|.q  and stop
223   8  if v_2 != 0, go to step 10
224   9  [v_1=v_2=0] c := 1, s:= 0, r := 0, stop
225   10 w := v_1/v_2
226   11 q := sqrt(1+w^2)
227   12 s := 1/q
228   13 if v_1 < 0, s := -s
229   14 c := ws
230   15 r := |v_2|.q  and stop
231
232   Apply the Givens rotation G to the vector (z_1 z_2)^T
233   (GivensApply):
234
235   Arguments: c, s, components of the Givens rotation matrix
236              z_1, z_2, (double *) the vector to apply it to; these
237                will be overwritten!
238
239   1  w := z_1.c + z_2.s
240   2  z_2 := -z_1.s + z_2.c
241   3  z_1 := w
242
243*/
244
245void GivensCalc(double v_1, double v_2, double *c, double *s, double *r)
246{
247        double w, q;
248
249        if (fabs(v_1) > fabs(v_2)) {
250                w = v_2/v_1;
251                q = sqrt(1+w*w);
252                *c = 1/q;
253                if (v_1 < 0) *c = - (*c);
254                *s = w*(*c);
255                *r = fabs(v_1)*q;
256        } else {
257                if (v_2 == 0) {
258                        *c = 1; *s = 0; *r = 0;
259                } else {
260                        w = v_1/v_2;
261                        q = sqrt(1+w*w);
262                        *s = 1/q;
263                        if (v_2 < 0) *s = - (*s);
264                        *c = w*(*s);
265                        *r = fabs(v_2)*q;
266                }
267        }
268}
269
270
271void GivensApply(double c, double s, double *z_1, double *z_2)
272{
273        double w;
274
275        w = (*z_1)*c + (*z_2)*s;
276        *z_2 = -(*z_1)*s + (*z_2)*c;
277        *z_1 = w;
278}
279
280
281/**********************************************************************
282   Function GCI_lsqnonneg
283
284   This function solves the non-negative least squares problem:
285   minimise |Ax-b| subject to x >= 0 (where |v| is the 2-norm of the
286   vector v).
287
288   Arguments: A, an m x n matrix, in the form double A[n][m], so
289                 the columns of A are A[0], A[1], ..., A[n-1]
290              b, the m-vector
291
292              !!! NB: A and B will both be modified unless preserve is
293              !!!     non-zero (see below).
294
295              x, the solution will be placed here
296              m ) the dimensions of A
297              n )
298              preserve, copy A and b before solving the problem
299              rnorm, (double *) the value of |Ax-b| with the
300                determined x if the function was successful or if the
301                iteration count was exceeded.  This can be NULL.
302              lambda, an n-vector which will contain the dual vector
303                 on completion (that is, the Lagrange multipliers).
304                 This can be NULL.
305
306   On exit:   
307              The return value will be 0 on success, and negative if
308                a problem occurred:
309                -1: m > MAX_EQNS or m <= 0
310                -2: n > MAX_VARS or n <= 0
311                -3: iteration count exceeded: more than 3*n iterations
312                    performed
313                -4: memory allocation problems
314
315   The algorithm is similar to the simplex algorithm, and uses
316   Lagrange multipliers.
317
318   1  Set P:={}, Z:={1,2,...,n}, x:=0
319   2  Compute w := A^T (b-Ax)
320   3  If Z={} or if w_j <= 0 for all j in Z, stop
321   4  Find t in Z such that w_t = max { w_j : j in Z }
322   5  Move index t from Z to P
323   6  Let A_P denote the m x n matrix defined by
324
325                          ( column j of A   if j in P
326        column j of A_P := (
327                          ( 0               if j in Z
328
329      Compute the n-vector z as a solution of the regular least squares
330      problem minimise | A_P z - b |.  Note that only the components
331      z_j, j in P, are determined by this problem.  Define z_j:=0 for j in Z.
332   7  If z_j>0 for all j in P, then set x:=z and goto step 2
333   8  Find an index q in P such that
334        x_q/(x_q-z_q) = min { x_j/(x_j-z_j) : z_j <= 0, j in P }
335   9  Set alpha := x_q/(x_q-z_q)
336   10 Set x := x + alpha(z-x)
337   11 Move from set P to set Z all j in P for which x_j=0.  Go to step 6.
338
339   On termination, x satisfies x_j>0 for j in P and x_j=0 for j in Z, and
340   x is a solution to the least squares problem minimise |A_P z - b|.
341
342   Step 6 (finding the solution to the least squares problem
343   |A_P z - b|) is performed using a QR factorisation of A_P, which in
344   turn is calculated using Householder matrices and Givens rotations,
345   and clever tricks for keeping track of the QR factorisation of A_P
346   as P changes, as we describe below.
347*/
348
349#define Amatrix(row,col) A[col][row]
350
351int GCI_lsqnonneg(double **A_orig, double *b_orig, double *x, int m, int n,
352                                  int preserve, double *rnorm_orig, double *lambda)
353{
354        double *AA[MAX_VARS], bb[MAX_EQNS], **A, *b;  /* to preserve originals */
355        double rnorm2, *rnorm;  /* in case rnorm_orig is NULL */
356        double ww[MAX_VARS], z[MAX_EQNS], *w;
357        double *C[MAX_VARS];  /* for passing to Householder */
358        double wmax, asave, u_p, unorm, sum;  /* miscellany */
359        int iter, itmax;
360        int index[MAX_VARS], p;
361        int i, ii, j, jj, k, l, q, zmax, index_zmax;
362
363        /* Meanings of variables:
364           index[] is an array containing information on the sets Z and P.
365       index[0..p-1] is the set P
366       index[p..n-1] is the set Z
367
368       z[] is working space
369        */
370
371        /* Check variables */
372        if (m > MAX_EQNS || m <= 0) return -1;
373        if (n > MAX_VARS || n <= 0) return -2;
374        w = (lambda == NULL) ? ww : lambda;
375        rnorm = (rnorm_orig == NULL) ? &rnorm2 : rnorm_orig;
376        if (preserve) {
377                /* We allocate one long array and split it into pieces */
378                if ((AA[0] = malloc(m*n*sizeof(double))) == NULL)
379                        return -4;
380                for (i=0; i<n; i++) {
381                        AA[i] = AA[0]+m*i;
382                        for (j=0; j<m; j++)
383                                AA[i][j] = A_orig[i][j];
384                }
385                for (j=0; j<m; j++)
386                        bb[j] = b_orig[j];
387                A = AA;
388                b = bb;
389        } else {
390                A = A_orig;
391                b = b_orig;
392        }
393
394        iter = 0;
395        itmax = 3*n;
396
397        /* Initialise the index and x arrays */
398        p = 0;
399        for (i=0; i<n; i++) {
400                x[i] = 0;
401                index[i] = i;
402        }
403
404        /* Main loop */
405        while (p<n && p<m) {
406                /* Compute the dual (negative gradient) vector w=A^T(b-Ax)
407                   We are only interested in the components of this vector in Z,
408                   so we only calculate these.  It turns out that for these
409                   components, we have w_j=(A^T.b)_j, and that (A^T.Ax)_j=0,
410                   remembering that A is continually modified.  I really don't
411                   yet know why this is true. */
412
413                for (i=p; i<n; i++) {
414                        j = index[i];
415                        sum = 0;
416                        for (k=p; k<m; k++)
417                                sum += Amatrix(k,j) * b[k];
418                        w[j] = sum;
419                }
420
421                wmax = 0;
422                while (wmax == 0) {
423                        /* We now find the maximum positive w_j. */
424                        zmax = -1;
425                        for (i=p; i<n; i++) {
426                                j = index[i];
427                                if (w[j]>wmax) {
428                                        wmax = w[j];
429                                        zmax = i;
430                                }
431                        }
432
433                        if (wmax <= 0) /* all w_j non-positive, so done */
434                                goto terminate;
435
436                        /* We move index[zmax] from Z to P
437                           We begin the transformation and check the new diagonal element
438                           to avoid near linear dependence */
439                        index_zmax = index[zmax];
440                        asave = Amatrix(p,index_zmax);
441
442                        Householder(1, A[index_zmax], p, p+1, m, &u_p, NULL, 0);
443                        /* u has ended up in the index_zmax-th column of A */
444                        unorm = 0;
445                        for (k=0; k<p; k++)
446                                unorm += Amatrix(k,index_zmax)*Amatrix(k,index_zmax);
447                        unorm = sqrt(unorm);
448
449                        /* Is the index_zmax-th column sufficiently independent
450                           (whatever that means)? */
451                        if ((unorm != 0 && fabs(Amatrix(p,index_zmax)/unorm) < TOL) ||
452                                (unorm == 0 && Amatrix(p,index_zmax) == 0)) {
453                                /* no, it isn't */
454                                Amatrix(p,index_zmax) = asave;
455                                w[index_zmax] = 0;
456                                wmax = 0;
457                        } else {
458                                /* it is: copy b[] into z[], update z[] and solve for ztest,
459                                   the proposed new value for x[index_zmax] */
460                                double ztest;
461                                for (i=0; i<m; i++)
462                                        z[i] = b[i];
463                                C[0] = z;  /* Need to pass Householder an array of pointers */
464                                Householder(2, A[index_zmax], p, p+1, m, &u_p, C, 1);
465                                ztest = z[p] / Amatrix(p,index_zmax);
466                                if (ztest <= 0) {
467                                        /* nah, this won't do, try a different zmax */
468                                        Amatrix(p,index_zmax) = asave;
469                                        w[index_zmax] = 0;
470                                        wmax = 0;
471                                }
472                                /* We've now got an acceptable index to move from Z to P!
473                                   We'll leave this while (wmax==0) loop now, then. */
474                        }
475                }
476
477                /* And take heed of the fact that we've got our index */
478                for (i=0; i<m; i++)
479                        b[i] = z[i];
480                index[zmax] = index[p];
481                index[p] = index_zmax;
482                p++;
483
484                /* We now apply our Householder transformation to all of the
485                   columns of A which correspond to index[p], ..., index[n-1].  We
486                   could call the Householder function for each column separately,
487                   but it's probably more efficient (fewer function calls) to
488                   bundle the columns up into one new temporary array.  Note that
489                   we've incremented p! */
490                if (p<n) {
491                        for (i=p; i<n; i++) {
492                                C[i-p] = A[index[i]];  /* the index[i]-th column of A */
493                        }
494                        Householder(2, A[index_zmax], p-1, p, m, &u_p, C, n-p);
495                }
496
497                /* ... zero the index_zmax-th column of A below the
498                   diagonal, and set w[index_zmax]=0 ... */
499                for (i=p; i<m; i++)
500                        Amatrix(i,index_zmax) = 0;
501                w[index_zmax] = 0;
502
503                /* ... and solve the triangular system to solve the least squares
504                   problem |A_P.z-b| (step 6 of algorithm), putting the solution
505                   into z.  Note also that a copy of b is still stored in z when
506                   we begin.
507
508                   We have the matrix A_P which has j-th column being index[j],
509                   j=0, ..., p-1.  The j-th column is zero below the diagonal.  So
510                   we solve it by back-substitution. */
511   
512                for (i=p-1; i>=0; i--) {
513                        j = index[i];
514                        z[i] /= Amatrix(i,j);
515                        for (k=0; k<i; k++)
516                                z[k] -= Amatrix(k,j) * z[i];
517                }
518
519                /* This next part is called the "secondary loop" in the book */
520                while (1) {
521                        double alpha, tmp;
522
523                        if (++iter > itmax)  /* over the iteration count, oops */
524                                goto terminate;
525
526                        alpha = 2;  /* greater than the values we will be
527                                                   comparing, which will all be <= 1 */
528                        for (i=0; i<p; i++)
529                                if (z[i] <= 0) {
530                                        tmp = x[index[i]] / (x[index[i]]-z[i]);  /* This is <= 1 */
531                                        if (tmp<alpha) {
532                                                alpha = tmp;
533                                                q = i;  /* keep track of the minimum index */
534                                        }
535                                }
536
537                        /* Were all z_j>0 for j in P?  If so, set x:=z and break out of
538                           this inner loop, returning to the main loop. */
539                        if (alpha > 1) {
540                                /* the next four lines of code corresponds to lines 244-247 in
541                                   the Fortran 66 version */
542                                for (i=0; i<p; i++) {
543                                        x[index[i]] = z[i];
544                                }
545                                break;
546                        }
547
548                        /* Interpolate between old x and new z using 0<alpha<=1 */
549                        for (i=0; i<p; i++)
550                                x[index[i]] += alpha * (z[i] - x[index[i]]);
551
552                        /* Now we have x[index[q]]=0, so modify A and b and the index
553                           arrays to move index[q] and any other i with x[i]=0 from set
554                           P to set Z */
555     
556                        while (q >= 0) {
557                                i = index[q];
558                                x[i] = 0; /* ensure that rounding errors don't creep in */
559                                /* now use Givens rotations to fix up A and b */
560                                for (jj=q+1; jj<p; jj++) {
561                                        double c, s;
562                                        ii = index[jj];
563                                        index[jj-1] = ii;
564                                        GivensCalc(Amatrix(jj-1,ii), Amatrix(jj,ii),
565                                                           &c, &s, &Amatrix(jj-1,ii));
566                                        Amatrix(jj,ii) = 0;
567
568                                        for (l=0; l<n; l++)
569                                                if (l != ii)
570                                                        GivensApply(c, s, &Amatrix(jj-1,l), &Amatrix(jj,l));
571                                        GivensApply(c, s, &b[jj-1], &b[jj]);
572                                }
573                                /* We've taken one element out of P */
574                                p--;
575                                index[p] = i;
576
577                                /* Check that the remaining coefficients in set P are
578                                   feasible, that is, >=0.  They should be, because of
579                                   the way alpha was determined.  If they are not,
580                                   this is due to rounding error, so we set any
581                                   non-positive elements to zero and move them too
582                                   from set P to set Z */
583                                for (q=p-1; q>=0; q--)
584                                        if (x[index[q]] <= 0) break;
585                                /* now if q>=0, then x[index[q]] <= 0,
586                                   and if q<0, we are done */
587                        } /* end of while (q >= 0) loop */
588
589                        /* OK, all x[i]=0 have now had their indices moved from P
590                           to Z.  We can now solve the least squares problem
591                           |Az-b| once again, ready for the next secondary loop.
592                           So we simply copy b into z and solve the triangular
593                           system, as before.  We could reduce the code size a
594                           fraction by having this code at the beginning of the
595                           loop, but that would make the loop exit code (checking
596                           iter) appear in the middle, which would be a tad
597                           confusing. */
598
599                        for (i=0; i<m; i++)
600                                z[i] = b[i];
601                        for (i=p-1; i>=0; i--) {
602                                j = index[i];
603                                z[i] /= Amatrix(i,j);
604                                for (k=0; k<i; k++)
605                                        z[k] -= Amatrix(k,j) * z[i];
606                        }
607                } /* end of secondary loop */
608        } /* end of primary loop */   
609
610 terminate:
611        sum = 0;
612        if (p < m) {
613                for (i=p; i<m; i++)
614                        sum += b[i] * b[i];
615        } else {
616                /* this is to get the w (dual) vector correct, in case we ever
617                   decide to give it as an output to this function. */
618                for (i=0; i<n; i++)
619                        w[i] = 0;
620        }
621
622        (*rnorm) = sqrt(sum);
623
624        if (preserve)
625                free(AA[0]);
626
627        return (iter > itmax) ? -3 : 0;
628}
629
630
631// Emacs settings:
632// Local variables:
633// mode: c
634// c-basic-offset: 4
635// tab-width: 4
636// End:
Note: See TracBrowser for help on using the repository browser.