misc_core.c

a copylefted C/C++ implementation of the Levenberg-Marquardt non-linear least squares algorithm

    LM_REAL aux;

    /* compute machine epsilon */
    for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
                                          ;
    eps*=LM_CNST(2.0);
  }

  /* compute the pseudoinverse in B */
	for(i=0; i<a_sz; i++) B[i]=0.0; /* initialize to zero */
  for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
    one_over_denom=LM_CNST(1.0)/s[rank];

    for(j=0; j<m; j++)
      for(i=0; i<m; i++)
        B[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
  }

  free(buf);

	return rank;
}
#else // no LAPACK

/*
 * This function computes the inverse of A in B. A and B can coincide
 *
 * The function employs LAPACK-free LU decomposition of A to solve m linear
 * systems A*B_i=I_i, where B_i and I_i are the i-th columns of B and I.
 *
 * A and B are mxm
 *
 * The function returns 0 in case of error,
 * 1 if successfull
 *
 */
static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m)
{
void *buf=NULL;
int buf_sz=0;

register int i, j, k, l;
int *idx, maxi=-1, idx_sz, a_sz, x_sz, work_sz, tot_sz;
LM_REAL *a, *x, *work, max, sum, tmp;

  /* calculate required memory size */
  idx_sz=m;
  a_sz=m*m;
  x_sz=m;
  work_sz=m;
  tot_sz=idx_sz*sizeof(int) + (a_sz+x_sz+work_sz)*sizeof(LM_REAL);

  buf_sz=tot_sz;
  buf=(void *)malloc(tot_sz);
  if(!buf){
    fprintf(stderr, RCAT("memory allocation in ", LEVMAR_LUINVERSE) "() failed!\n");
    exit(1);
  }

  idx=(int *)buf;
  a=(LM_REAL *)(idx + idx_sz);
  x=a + a_sz;
  work=x + x_sz;

  /* avoid destroying A by copying it to a */
  for(i=0; i<a_sz; ++i) a[i]=A[i];

  /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
	for(i=0; i<m; ++i){
		max=0.0;
		for(j=0; j<m; ++j)
			if((tmp=FABS(a[i*m+j]))>max)
        max=tmp;
		  if(max==0.0){
        fprintf(stderr, RCAT("Singular matrix A in ", LEVMAR_LUINVERSE) "()!\n");
        free(buf);

        return 0;
      }
		  work[i]=LM_CNST(1.0)/max;
	}

	for(j=0; j<m; ++j){
		for(i=0; i<j; ++i){
			sum=a[i*m+j];
			for(k=0; k<i; ++k)
        sum-=a[i*m+k]*a[k*m+j];
			a[i*m+j]=sum;
		}
		max=0.0;
		for(i=j; i<m; ++i){
			sum=a[i*m+j];
			for(k=0; k<j; ++k)
        sum-=a[i*m+k]*a[k*m+j];
			a[i*m+j]=sum;
			if((tmp=work[i]*FABS(sum))>=max){
				max=tmp;
				maxi=i;
			}
		}
		if(j!=maxi){
			for(k=0; k<m; ++k){
				tmp=a[maxi*m+k];
				a[maxi*m+k]=a[j*m+k];
				a[j*m+k]=tmp;
			}
			work[maxi]=work[j];
		}
		idx[j]=maxi;
		if(a[j*m+j]==0.0)
      a[j*m+j]=LM_REAL_EPSILON;
		if(j!=m-1){
			tmp=LM_CNST(1.0)/(a[j*m+j]);
			for(i=j+1; i<m; ++i)
        a[i*m+j]*=tmp;
		}
	}

  /* The decomposition has now replaced a. Solve the m linear systems using
   * forward and back substitution
   */
  for(l=0; l<m; ++l){
    for(i=0; i<m; ++i) x[i]=0.0;
    x[l]=LM_CNST(1.0);

	  for(i=k=0; i<m; ++i){
		  j=idx[i];
		  sum=x[j];
		  x[j]=x[i];
		  if(k!=0)
			  for(j=k-1; j<i; ++j)
          sum-=a[i*m+j]*x[j];
		  else
        if(sum!=0.0)
			    k=i+1;
		  x[i]=sum;
	  }

	  for(i=m-1; i>=0; --i){
		  sum=x[i];
		  for(j=i+1; j<m; ++j)
        sum-=a[i*m+j]*x[j];
		  x[i]=sum/a[i*m+i];
	  }

    for(i=0; i<m; ++i)
      B[i*m+l]=x[i];
  }

  free(buf);

  return 1;
}
#endif /* HAVE_LAPACK */

/*
 * This function computes in C the covariance matrix corresponding to a least
 * squares fit. JtJ is the approximate Hessian at the solution (i.e. J^T*J, where
 * J is the Jacobian at the solution), sumsq is the sum of squared residuals
 * (i.e. goodnes of fit) at the solution, m is the number of parameters (variables)
 * and n the number of observations. JtJ can coincide with C.
 * 
 * if JtJ is of full rank, C is computed as sumsq/(n-m)*(JtJ)^-1
 * otherwise and if LAPACK is available, C=sumsq/(n-r)*(JtJ)^+
 * where r is JtJ's rank and ^+ denotes the pseudoinverse
 * The diagonal of C is made up from the estimates of the variances
 * of the estimated regression coefficients.
 * See the documentation of routine E04YCF from the NAG fortran lib
 *
 * The function returns the rank of JtJ if successful, 0 on error
 *
 * A and C are mxm
 *
 */
int LEVMAR_COVAR(LM_REAL *JtJ, LM_REAL *C, LM_REAL sumsq, int m, int n)
{
register int i;
int rnk;
LM_REAL fact;

#ifdef HAVE_LAPACK
   rnk=LEVMAR_PSEUDOINVERSE(JtJ, C, m);
   if(!rnk) return 0;
#else
#ifdef _MSC_VER
#pragma message("LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times")
#else
#warning LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times
#endif // _MSC_VER

   rnk=LEVMAR_LUINVERSE(JtJ, C, m);
   if(!rnk) return 0;

   rnk=m; /* assume full rank */
#endif /* HAVE_LAPACK */

   fact=sumsq/(LM_REAL)(n-rnk);
   for(i=0; i<m*m; ++i)
     C[i]*=fact;

   return rnk;
}

/*  standard deviation of the best-fit parameter i.
 *  covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
 *
 *  The standard deviation is computed as \sigma_{i} = \sqrt{C_{ii}} 
 */
LM_REAL LEVMAR_STDDEV(LM_REAL *covar, int m, int i)
{
   return (LM_REAL)sqrt(covar[i*m+i]);
}

/* Pearson's correlation coefficient of the best-fit parameters i and j.
 * covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
 *
 * The coefficient is computed as \rho_{ij} = C_{ij} / sqrt(C_{ii} C_{jj})
 */
LM_REAL LEVMAR_CORCOEF(LM_REAL *covar, int m, int i, int j)
{
   return (LM_REAL)(covar[i*m+j]/sqrt(covar[i*m+i]*covar[j*m+j]));
}

/* check box constraints for consistency */
int LEVMAR_BOX_CHECK(LM_REAL *lb, LM_REAL *ub, int m)
{
register int i;

  if(!lb || !ub) return 1;

  for(i=0; i<m; ++i)
    if(lb[i]>ub[i]) return 0;

  return 1;
}

#ifdef HAVE_LAPACK

/* compute the Cholesky decompostion of C in W, s.t. C=W^t W and W is upper triangular */
int LEVMAR_CHOLESKY(LM_REAL *C, LM_REAL *W, int m)
{
register int i, j;
int info;

  /* copy weights array C to W (in column-major order!) so that LAPACK won't destroy it */
  for(i=0; i<m; i++)
    for(j=0; j<m; j++)
      W[i+j*m]=C[i*m+j];

  /* cholesky decomposition */
  POTF2("U", (int *)&m, W, (int *)&m, (int *)&info);
  /* error treatment */
  if(info!=0){
		if(info<0){
      fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2 in %s\n", -info, LCAT(LEVMAR_DER, "()"));
		  exit(1);
		}
		else{
			fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s()\n", info,
						RCAT("and the cholesky factorization could not be completed in ", LEVMAR_CHOLESKY));
			return LM_ERROR;
		}
  }

  /* the decomposition is in the upper part of W (in column-major order!).
   * copying it to the lower part and zeroing the upper transposes
   * W in row-major order
   */
  for(i=0; i<m; i++)
    for(j=0; j<i; j++){
      W[i+j*m]=W[j+i*m];
      W[j+i*m]=0.0;
    }

  return 0;
}
#endif /* HAVE_LAPACK */


/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.
 * e can coincide with either x or y; x can be NULL, in which case it is assumed
 * to be equal to the zero vector.
 * Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline
 * stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html
 */

LM_REAL LEVMAR_L2NRMXMY(LM_REAL *e, LM_REAL *x, LM_REAL *y, int n)
{
const int blocksize=8, bpwr=3; /* 8=2^3 */
register int i;
int j1, j2, j3, j4, j5, j6, j7;
int blockn;
register LM_REAL sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0;

  /* n may not be divisible by blocksize, 
   * go as near as we can first, then tidy up.
   */ 
  blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */

  if(x){
    /* unroll the loop in blocks of `blocksize' */
    for(i=0; i<blockn; i+=blocksize){
              e[i ]=x[i ]-y[i ]; sum0+=e[i ]*e[i ];
      j1=i+1; e[j1]=x[j1]-y[j1]; sum1+=e[j1]*e[j1];
      j2=i+2; e[j2]=x[j2]-y[j2]; sum2+=e[j2]*e[j2];
      j3=i+3; e[j3]=x[j3]-y[j3]; sum3+=e[j3]*e[j3];
      j4=i+4; e[j4]=x[j4]-y[j4]; sum0+=e[j4]*e[j4];
      j5=i+5; e[j5]=x[j5]-y[j5]; sum1+=e[j5]*e[j5];
      j6=i+6; e[j6]=x[j6]-y[j6]; sum2+=e[j6]*e[j6];
      j7=i+7; e[j7]=x[j7]-y[j7]; sum3+=e[j7]*e[j7];
    }

   /*
    * There may be some left to do.
    * This could be done as a simple for() loop, 
    * but a switch is faster (and more interesting) 
    */ 

    if(i<n){ 
      /* Jump into the case at the place that will allow
       * us to finish off the appropriate number of items. 
       */ 

      switch(n - i){ 
        case 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
        case 6 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
        case 5 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
        case 4 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
        case 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
        case 2 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
        case 1 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
      }
    }
  }
  else{ /* x==0 */
    /* unroll the loop in blocks of `blocksize' */
    for(i=0; i<blockn; i+=blocksize){
              e[i ]=-y[i ]; sum0+=e[i ]*e[i ];
      j1=i+1; e[j1]=-y[j1]; sum1+=e[j1]*e[j1];
      j2=i+2; e[j2]=-y[j2]; sum2+=e[j2]*e[j2];
      j3=i+3; e[j3]=-y[j3]; sum3+=e[j3]*e[j3];
      j4=i+4; e[j4]=-y[j4]; sum0+=e[j4]*e[j4];
      j5=i+5; e[j5]=-y[j5]; sum1+=e[j5]*e[j5];
      j6=i+6; e[j6]=-y[j6]; sum2+=e[j6]*e[j6];
      j7=i+7; e[j7]=-y[j7]; sum3+=e[j7]*e[j7];
    }

   /*
    * There may be some left to do.
    * This could be done as a simple for() loop, 
    * but a switch is faster (and more interesting) 
    */ 

    if(i<n){ 
      /* Jump into the case at the place that will allow
       * us to finish off the appropriate number of items. 
       */ 

      switch(n - i){ 
        case 7 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
        case 6 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
        case 5 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
        case 4 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
        case 3 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
        case 2 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
        case 1 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
      }
    }
  }

  return sum0+sum1+sum2+sum3;
}

/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LEVMAR_PSEUDOINVERSE
#undef LEVMAR_LUINVERSE
#undef LEVMAR_BOX_CHECK
#undef LEVMAR_CHOLESKY
#undef LEVMAR_COVAR
#undef LEVMAR_STDDEV
#undef LEVMAR_CORCOEF
#undef LEVMAR_CHKJAC
#undef LEVMAR_FDIF_FORW_JAC_APPROX
#undef LEVMAR_FDIF_CENT_JAC_APPROX
#undef LEVMAR_TRANS_MAT_MAT_MULT
#undef LEVMAR_L2NRMXMY