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

Revision 7781, 20.3 KB checked in by paulbarber, 8 years ago (diff)

Added GPL license to all source code.

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