src/tools/levenmar.c

   1 /*
   2  * This file is part of the GROMACS molecular simulation package.
   3  *
   4  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
   5  * Copyright (c) 2001-2004, The GROMACS development team,
   6  * check out http://www.gromacs.org for more information.
   7  * Copyright (c) 2012, by the GROMACS development team, led by
   8  * David van der Spoel, Berk Hess, Erik Lindahl, and including many
   9  * others, as listed in the AUTHORS file in the top-level source
  10  * directory and at http://www.gromacs.org.
  11  *
  12  * GROMACS is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU Lesser General Public License
  14  * as published by the Free Software Foundation; either version 2.1
  15  * of the License, or (at your option) any later version.
  16  *
  17  * GROMACS is distributed in the hope that it will be useful,
  18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  20  * Lesser General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU Lesser General Public
  23  * License along with GROMACS; if not, see
  24  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
  25  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
  26  *
  27  * If you want to redistribute modifications to GROMACS, please
  28  * consider that scientific software is very special. Version
  29  * control is crucial - bugs must be traceable. We will be happy to
  30  * consider code for inclusion in the official distribution, but
  31  * derived work must not be called official GROMACS. Details are found
  32  * in the README & COPYING files - if they are missing, get the
  33  * official version at http://www.gromacs.org.
  34  *
  35  * To help us fund GROMACS development, we humbly ask that you cite
  36  * the research papers on the package. Check out http://www.gromacs.org.
  37  */
  38 #ifdef HAVE_CONFIG_H
  39 #include <config.h>
  40 #endif
  41
  42 #include <math.h>
  43 #include <stdio.h>
  44 #include <stdlib.h>
  45
  46 #include "types/simple.h"
  47
  48 static void nrerror(const char error_text[], gmx_bool bExit)
  49 {
  50   fprintf(stderr,"Numerical Recipes run-time error...\n");
  51   fprintf(stderr,"%s\n",error_text);
  52   if (bExit) {
  53     fprintf(stderr,"...now exiting to system...\n");
  54     exit(1);
  55   }
  56 }
  57
  58 /* dont use the keyword vector - it will clash with the
  59  * altivec extensions used for powerpc processors.
  60  */
  61
  62 static real *rvector(int nl,int nh)
  63 {
  64   real *v;
  65
  66   v=(real *)malloc((unsigned) (nh-nl+1)*sizeof(real));
  67   if (!v) nrerror("allocation failure in rvector()", TRUE);
  68   return v-nl;
  69 }
  70
  71 static int *ivector(int nl, int nh)
  72 {
  73   int *v;
  74
  75   v=(int *)malloc((unsigned) (nh-nl+1)*sizeof(int));
  76   if (!v) nrerror("allocation failure in ivector()", TRUE);
  77   return v-nl;
  78 }
  79
  80
  81 static real **matrix1(int nrl, int nrh, int ncl, int nch)
  82 {
  83         int i;
  84         real **m;
  85
  86         m=(real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*));
  87         if (!m) nrerror("allocation failure 1 in matrix1()", TRUE);
  88         m -= nrl;
  89
  90         for(i=nrl;i<=nrh;i++) {
  91                 m[i]=(real *) malloc((unsigned) (nch-ncl+1)*sizeof(real));
  92                 if (!m[i]) nrerror("allocation failure 2 in matrix1()", TRUE);
  93                 m[i] -= ncl;
  94         }
  95         return m;
  96 }
  97
  98 static double **dmatrix(int nrl, int nrh, int ncl, int nch)
  99 {
 100         int i;
 101         double **m;
 102
 103         m=(double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
 104         if (!m) nrerror("allocation failure 1 in dmatrix()", TRUE);
 105         m -= nrl;
 106
 107         for(i=nrl;i<=nrh;i++) {
 108                 m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
 109                 if (!m[i]) nrerror("allocation failure 2 in dmatrix()", TRUE);
 110                 m[i] -= ncl;
 111         }
 112         return m;
 113 }
 114
 115 static int **imatrix1(int nrl, int nrh, int ncl, int nch)
 116 {
 117         int i,**m;
 118
 119         m=(int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
 120         if (!m) nrerror("allocation failure 1 in imatrix1()", TRUE);
 121         m -= nrl;
 122
 123         for(i=nrl;i<=nrh;i++) {
 124                 m[i]=(int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
 125                 if (!m[i]) nrerror("allocation failure 2 in imatrix1()", TRUE);
 126                 m[i] -= ncl;
 127         }
 128         return m;
 129 }
 130
 131
 132
 133 static real **submatrix(real **a, int oldrl, int oldrh, int oldcl,
 134                  int newrl, int newcl)
 135 {
 136         int i,j;
 137         real **m;
 138
 139         m=(real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*));
 140         if (!m) nrerror("allocation failure in submatrix()", TRUE);
 141         m -= newrl;
 142
 143         for(i=oldrl,j=newrl;i<=oldrh;i++,j++) m[j]=a[i]+oldcl-newcl;
 144
 145         return m;
 146 }
 147
 148
 149
 150 static void free_vector(real *v, int nl)
 151 {
 152         free((char*) (v+nl));
 153 }
 154
 155 static void free_ivector(int *v, int nl)
 156 {
 157         free((char*) (v+nl));
 158 }
 159
 160 static void free_dvector(int *v, int nl)
 161 {
 162         free((char*) (v+nl));
 163 }
 164
 165
 166
 167 static void free_matrix(real **m, int nrl, int nrh, int ncl)
 168 {
 169         int i;
 170
 171         for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
 172         free((char*) (m+nrl));
 173 }
 174
 175 static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch)
 176 {
 177         int i,j,nrow,ncol;
 178         real **m;
 179
 180         nrow=nrh-nrl+1;
 181         ncol=nch-ncl+1;
 182         m = (real **) malloc((unsigned) (nrow)*sizeof(real*));
 183         if (!m) nrerror("allocation failure in convert_matrix()", TRUE);
 184         m -= nrl;
 185         for(i=0,j=nrl;i<=nrow-1;i++,j++) m[j]=a+ncol*i-ncl;
 186         return m;
 187 }
 188
 189
 190
 191 static void free_convert_matrix(real **b, int nrl)
 192 {
 193         free((char*) (b+nrl));
 194 }
 195
 196 #define SWAP(a,b) {real temp=(a);(a)=(b);(b)=temp;}
 197
 198 static void dump_mat(int n,real **a)
 199 {
 200   int i,j;
 201
 202   for(i=1; (i<=n); i++) {
 203     for(j=1; (j<=n); j++)
 204       fprintf(stderr,"  %10.3f",a[i][j]);
 205     fprintf(stderr,"\n");
 206   }
 207 }
 208
 209 gmx_bool gaussj(real **a, int n, real **b, int m)
 210 {
 211   int *indxc,*indxr,*ipiv;
 212   int i,icol=0,irow=0,j,k,l,ll;
 213   real big,dum,pivinv;
 214
 215   indxc=ivector(1,n);
 216   indxr=ivector(1,n);
 217   ipiv=ivector(1,n);
 218   for (j=1;j<=n;j++) ipiv[j]=0;
 219   for (i=1;i<=n;i++) {
 220     big=0.0;
 221     for (j=1;j<=n;j++)
 222       if (ipiv[j] != 1)
 223         for (k=1;k<=n;k++) {
 224           if (ipiv[k] == 0) {
 225             if (fabs(a[j][k]) >= big) {
 226               big=fabs(a[j][k]);
 227               irow=j;
 228               icol=k;
 229             }
 230           } else if (ipiv[k] > 1) {
 231             nrerror("GAUSSJ: Singular Matrix-1", FALSE);
 232             return FALSE;
 233           }
 234         }
 235     ++(ipiv[icol]);
 236     if (irow != icol) {
 237       for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
 238         for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
 239     }
 240     indxr[i]=irow;
 241     indxc[i]=icol;
 242     if (a[icol][icol] == 0.0) {
 243       fprintf(stderr,"irow = %d, icol = %d\n",irow,icol);
 244       dump_mat(n,a);
 245       nrerror("GAUSSJ: Singular Matrix-2", FALSE);
 246       return FALSE;
 247     }
 248     pivinv=1.0/a[icol][icol];
 249     a[icol][icol]=1.0;
 250     for (l=1;l<=n;l++) a[icol][l] *= pivinv;
 251     for (l=1;l<=m;l++) b[icol][l] *= pivinv;
 252     for (ll=1;ll<=n;ll++)
 253       if (ll != icol) {
 254         dum=a[ll][icol];
 255         a[ll][icol]=0.0;
 256         for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
 257         for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
 258       }
 259   }
 260   for (l=n;l>=1;l--) {
 261     if (indxr[l] != indxc[l])
 262       for (k=1;k<=n;k++)
 263         SWAP(a[k][indxr[l]],a[k][indxc[l]]);
 264   }
 265   free_ivector(ipiv,1);
 266   free_ivector(indxr,1);
 267   free_ivector(indxc,1);
 268
 269   return TRUE;
 270 }
 271
 272 #undef SWAP
 273
 274
 275 static void covsrt(real **covar, int ma, int lista[], int mfit)
 276 {
 277   int i,j;
 278   real swap;
 279
 280   for (j=1;j<ma;j++)
 281     for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
 282   for (i=1;i<mfit;i++)
 283     for (j=i+1;j<=mfit;j++) {
 284       if (lista[j] > lista[i])
 285         covar[lista[j]][lista[i]]=covar[i][j];
 286       else
 287         covar[lista[i]][lista[j]]=covar[i][j];
 288     }
 289   swap=covar[1][1];
 290   for (j=1;j<=ma;j++) {
 291     covar[1][j]=covar[j][j];
 292     covar[j][j]=0.0;
 293   }
 294   covar[lista[1]][lista[1]]=swap;
 295   for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
 296   for (j=2;j<=ma;j++)
 297     for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
 298 }
 299
 300 #define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
 301
 302 static void covsrt_new(real **covar,int ma, int ia[], int mfit)
 303      /* Expand in storage the covariance matrix covar, so as to take
 304       * into account parameters that are being held fixed. (For the
 305       * latter, return zero covariances.)
 306       */
 307 {
 308   int i,j,k;
 309   real swap;
 310   for (i=mfit+1;i<=ma;i++)
 311     for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
 312   k=mfit;
 313   for (j=ma;j>=1;j--) {
 314     if (ia[j]) {
 315       for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
 316       for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
 317       k--;
 318     }
 319   }
 320 }
 321 #undef SWAP
 322
 323 static void mrqcof(real x[], real y[], real sig[], int ndata, real a[],
 324                    int ma, int lista[], int mfit,
 325                    real **alpha, real beta[], real *chisq,
 326                    void (*funcs)(real,real *,real *,real *))
 327 {
 328   int k,j,i;
 329   real ymod,wt,sig2i,dy,*dyda;
 330
 331   dyda=rvector(1,ma);
 332   for (j=1;j<=mfit;j++) {
 333     for (k=1;k<=j;k++) alpha[j][k]=0.0;
 334     beta[j]=0.0;
 335   }
 336   *chisq=0.0;
 337   for (i=1;i<=ndata;i++) {
 338     (*funcs)(x[i],a,&ymod,dyda);
 339     sig2i=1.0/(sig[i]*sig[i]);
 340     dy=y[i]-ymod;
 341     for (j=1;j<=mfit;j++) {
 342       wt=dyda[lista[j]]*sig2i;
 343       for (k=1;k<=j;k++)
 344         alpha[j][k] += wt*dyda[lista[k]];
 345       beta[j] += dy*wt;
 346     }
 347     (*chisq) += dy*dy*sig2i;
 348   }
 349   for (j=2;j<=mfit;j++)
 350     for (k=1;k<=j-1;k++) alpha[k][j]=alpha[j][k];
 351   free_vector(dyda,1);
 352 }
 353
 354
 355 gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
 356             int ma, int lista[], int mfit,
 357             real **covar, real **alpha, real *chisq,
 358             void (*funcs)(real,real *,real *,real *),
 359             real *alamda)
 360 {
 361   int k,kk,j,ihit;
 362   static real *da,*atry,**oneda,*beta,ochisq;
 363
 364   if (*alamda < 0.0) {
 365     oneda=matrix1(1,mfit,1,1);
 366     atry=rvector(1,ma);
 367     da=rvector(1,ma);
 368     beta=rvector(1,ma);
 369     kk=mfit+1;
 370     for (j=1;j<=ma;j++) {
 371       ihit=0;
 372       for (k=1;k<=mfit;k++)
 373         if (lista[k] == j) ihit++;
 374       if (ihit == 0)
 375         lista[kk++]=j;
 376       else if (ihit > 1) {
 377         nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
 378         return FALSE;
 379       }
 380     }
 381     if (kk != ma+1) {
 382       nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
 383       return FALSE;
 384     }
 385     *alamda=0.001;
 386     mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
 387     ochisq=(*chisq);
 388   }
 389   for (j=1;j<=mfit;j++) {
 390     for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
 391     covar[j][j]=alpha[j][j]*(1.0+(*alamda));
 392     oneda[j][1]=beta[j];
 393   }
 394   if (!gaussj(covar,mfit,oneda,1))
 395     return FALSE;
 396   for (j=1;j<=mfit;j++)
 397     da[j]=oneda[j][1];
 398   if (*alamda == 0.0) {
 399     covsrt(covar,ma,lista,mfit);
 400     free_vector(beta,1);
 401     free_vector(da,1);
 402     free_vector(atry,1);
 403     free_matrix(oneda,1,mfit,1);
 404     return TRUE;
 405   }
 406   for (j=1;j<=ma;j++) atry[j]=a[j];
 407   for (j=1;j<=mfit;j++)
 408     atry[lista[j]] = a[lista[j]]+da[j];
 409   mrqcof(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
 410   if (*chisq < ochisq) {
 411     *alamda *= 0.1;
 412     ochisq=(*chisq);
 413     for (j=1;j<=mfit;j++) {
 414       for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
 415       beta[j]=da[j];
 416       a[lista[j]]=atry[lista[j]];
 417     }
 418   } else {
 419     *alamda *= 10.0;
 420     *chisq=ochisq;
 421   }
 422   return TRUE;
 423 }
 424
 425
 426 gmx_bool mrqmin_new(real x[],real y[],real sig[],int ndata,real a[],
 427                 int ia[],int ma,real **covar,real **alpha,real *chisq,
 428                 void (*funcs)(real, real [], real *, real []),
 429                 real *alamda)
 430      /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
 431       * of a fit between a set of data points x[1..ndata], y[1..ndata]
 432       * with individual standard deviations sig[1..ndata], and a nonlinear
 433       * function dependent on ma coefficients a[1..ma]. The input array
 434       * ia[1..ma] indicates by nonzero entries those components of a that
 435       * should be fitted for, and by zero entries those components that
 436       * should be held fixed at their input values. The program returns
 437       * current best-fit values for the parameters a[1..ma], and
 438       * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
 439       * are used as working space during most iterations. Supply a routine
 440       * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
 441       * and its derivatives dyda[1..ma] with respect to the fitting
 442       * parameters a at x. On the first call provide an initial guess for
 443       * the parameters a, and set alamda < 0 for initialization (which then
 444       * sets alamda=.001). If a step succeeds chisq becomes smaller and
 445       * alamda de-creases by a factor of 10. If a step fails alamda grows by
 446       * a factor of 10. You must call this routine repeatedly until
 447       * convergence is achieved. Then, make one final call with alamda=0,
 448       * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
 449       * the curvature matrix.
 450       * (Parameters held fixed will return zero covariances.)
 451       */
 452 {
 453   void covsrt(real **covar, int ma, int ia[], int mfit);
 454   gmx_bool gaussj(real **a, int n, real **b,int m);
 455   void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
 456               int ia[], int ma, real **alpha, real beta[], real *chisq,
 457               void (*funcs)(real, real [], real *, real []));
 458   int j,k,l;
 459   static int mfit;
 460   static real ochisq,*atry,*beta,*da,**oneda;
 461
 462   if (*alamda < 0.0) {                    /* Initialization. */
 463     atry=rvector(1,ma);
 464     beta=rvector(1,ma);
 465     da=rvector(1,ma);
 466     for (mfit=0,j=1;j<=ma;j++)
 467       if (ia[j]) mfit++;
 468     oneda=matrix1(1,mfit,1,1);
 469     *alamda=0.001;
 470     mrqcof_new(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
 471     ochisq=(*chisq);
 472     for (j=1;j<=ma;j++)
 473       atry[j]=a[j];
 474   }
 475   for (j=1;j<=mfit;j++) { /* Alter linearized fitting matrix, by augmenting. */
 476     for (k=1;k<=mfit;k++)
 477       covar[j][k]=alpha[j][k]; /* diagonal elements. */
 478     covar[j][j]=alpha[j][j]*(1.0+(*alamda));
 479     oneda[j][1]=beta[j];
 480   }
 481   if (!gaussj(covar,mfit,oneda,1))      /* Matrix solution. */
 482     return FALSE;
 483   for (j=1;j<=mfit;j++)
 484     da[j]=oneda[j][1];
 485   if (*alamda == 0.0) { /* Once converged, evaluate covariance matrix. */
 486     covsrt_new(covar,ma,ia,mfit);
 487     free_matrix(oneda,1,mfit,1);
 488     free_vector(da,1);
 489     free_vector(beta,1);
 490     free_vector(atry,1);
 491     return TRUE;
 492   }
 493   for (j=0,l=1;l<=ma;l++) /* Did the trial succeed? */
 494     if (ia[l]) atry[l]=a[l]+da[++j];
 495   mrqcof_new(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
 496   if (*chisq < ochisq) {
 497     /* Success, accept the new solution. */
 498     *alamda *= 0.1;
 499     ochisq=(*chisq);
 500     for (j=1;j<=mfit;j++) {
 501         for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
 502         beta[j]=da[j];
 503     }
 504     for (l=1;l<=ma;l++) a[l]=atry[l];
 505   } else {               /* Failure, increase alamda and return. */
 506     *alamda *= 10.0;
 507     *chisq=ochisq;
 508   }
 509   return TRUE;
 510 }
 511
 512 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
 513             int ia[], int ma, real **alpha, real beta[], real *chisq,
 514             void (*funcs)(real, real [], real *, real[]))
 515      /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
 516       * vector beta as in (15.5.8), and calculate Chi^2.
 517       */
 518 {
 519   int i,j,k,l,m,mfit=0;
 520   real ymod,wt,sig2i,dy,*dyda;
 521
 522   dyda=rvector(1,ma);
 523   for (j=1;j<=ma;j++)
 524     if (ia[j]) mfit++;
 525   for (j=1;j<=mfit;j++) { /* Initialize (symmetric) alpha), beta. */
 526     for (k=1;k<=j;k++) alpha[j][k]=0.0;
 527     beta[j]=0.0;
 528   }
 529   *chisq=0.0;
 530   for (i=1;i<=ndata;i++) { /* Summation loop over all data. */
 531     (*funcs)(x[i],a,&ymod,dyda);
 532     sig2i=1.0/(sig[i]*sig[i]);
 533     dy=y[i]-ymod;
 534     for (j=0,l=1;l<=ma;l++) {
 535       if (ia[l]) {
 536         wt=dyda[l]*sig2i;
 537         for (j++,k=0,m=1;m<=l;m++)
 538           if (ia[m]) alpha[j][++k] += wt*dyda[m];
 539         beta[j] += dy*wt;
 540       }
 541     }
 542     *chisq += dy*dy*sig2i;  /* And find Chi^2. */
 543   }
 544   for (j=2;j<=mfit;j++)     /* Fill in the symmetric side. */
 545     for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
 546   free_vector(dyda,1);
 547 }