lmblec_core.c

A sparse variant of the Levenberg-Marquardt algorithm implemented by levmar has been applied to bund

                      *                                 2 - stopped by small Dp
                      *                                 3 - stopped by itmax
                      *                                 4 - singular matrix. Restart from current p with increased mu 
                      *                                 5 - no further error reduction is possible. Restart with increased mu
                      *                                 6 - stopped by small ||e||_2
                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
                      * info[7]= # function evaluations
                      * info[8]= # Jacobian evaluations
                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
                      */
  LM_REAL *work,     /* working memory at least LM_BLEC_DER_WORKSZ() reals large, allocated if NULL */
  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
                      * Set to NULL if not needed
                      */
{
  struct LMBLEC_DATA data;
  int ret;
  LM_REAL locinfo[LM_INFO_SZ];
  register int i;

  if(!jacf){
    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEC_DER)
      RCAT("().\nIf no such function is available, use ", LEVMAR_BLEC_DIF) RCAT("() rather than ", LEVMAR_BLEC_DER) "()\n");
    return LM_ERROR;
  }

  if(!lb && !ub){
    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DER, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
          LEVMAR_LEC_DER) "() in this case!\n");
    return LM_ERROR;
  }

  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
    return LM_ERROR;
  }

  /* measurement vector needs to be extended by m */
  if(x){ /* nonzero x */
    data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
    if(!data.x){
      fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
      return LM_ERROR;
    }

    for(i=0; i<n; ++i)
      data.x[i]=x[i];
    for(i=n; i<n+m; ++i)
      data.x[i]=0.0;
  }
  else
    data.x=NULL;

  data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
  if(!data.w){
    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
    if(data.x) free(data.x);
    return LM_ERROR;
  }
  data.bctype=(int *)(data.w+m);

  /* note: at this point, one of lb, ub are not NULL */
  for(i=0; i<m; ++i){
    data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
    if(!lb) data.bctype[i]=__BC_HIGH__;
    else if(!ub) data.bctype[i]=__BC_LOW__;
    else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
    else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
    else data.bctype[i]=__BC_HIGH__;
  }

  data.lb=lb;
  data.ub=ub;
  data.func=func;
  data.jacf=jacf;
  data.adata=adata;

  if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */
  ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);

  if(data.x) free(data.x);
  free(data.w);

  return ret;
}

/* Similar to the LEVMAR_BLEC_DER() function above, except that the Jacobian is approximated
 * with the aid of finite differences (forward or central, see the comment for the opts argument)
 */
int LEVMAR_BLEC_DIF(
  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
  int n,              /* I: measurement vector dimension */
  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
  LM_REAL *A,         /* I: constraints matrix, kxm */
  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
  int k,              /* I: number of constraints (i.e. A's #rows) */
  LM_REAL *wghts,     /* mx1 weights for penalty terms, defaults used if NULL */
  int itmax,          /* I: maximum number of iterations */
  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
                       * (but slower!) compared to the forward differences employed by default. 
                       */
  LM_REAL info[LM_INFO_SZ],
					           /* O: information regarding the minimization. Set to NULL if don't care
                      * info[0]= ||e||_2 at initial p.
                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
                      * info[5]= # iterations,
                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
                      *                                 2 - stopped by small Dp
                      *                                 3 - stopped by itmax
                      *                                 4 - singular matrix. Restart from current p with increased mu 
                      *                                 5 - no further error reduction is possible. Restart with increased mu
                      *                                 6 - stopped by small ||e||_2
                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
                      * info[7]= # function evaluations
                      * info[8]= # Jacobian evaluations
                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
                      */
  LM_REAL *work,     /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated if NULL */
  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
                      * Set to NULL if not needed
                      */
{
  struct LMBLEC_DATA data;
  int ret;
  register int i;
  LM_REAL locinfo[LM_INFO_SZ];

  if(!lb && !ub){
    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DIF, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
          LEVMAR_LEC_DIF) "() in this case!\n");
    return LM_ERROR;
  }

  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
    return LM_ERROR;
  }

  /* measurement vector needs to be extended by m */
  if(x){ /* nonzero x */
    data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
    if(!data.x){
      fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
      return LM_ERROR;
    }

    for(i=0; i<n; ++i)
      data.x[i]=x[i];
    for(i=n; i<n+m; ++i)
      data.x[i]=0.0;
  }
  else
    data.x=NULL;

  data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
  if(!data.w){
    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
    if(data.x) free(data.x);
    return LM_ERROR;
  }
  data.bctype=(int *)(data.w+m);

  /* note: at this point, one of lb, ub are not NULL */
  for(i=0; i<m; ++i){
    data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
    if(!lb) data.bctype[i]=__BC_HIGH__;
    else if(!ub) data.bctype[i]=__BC_LOW__;
    else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
    else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
    else data.bctype[i]=__BC_HIGH__;
  }

  data.lb=lb;
  data.ub=ub;
  data.func=func;
  data.jacf=NULL;
  data.adata=adata;

  if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */
  ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);

  if(data.x) free(data.x);
  free(data.w);

  return ret;
}

/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LEVMAR_BOX_CHECK
#undef LMBLEC_DATA
#undef LMBLEC_FUNC
#undef LMBLEC_JACF
#undef LEVMAR_COVAR
#undef LEVMAR_LEC_DER
#undef LEVMAR_LEC_DIF
#undef LEVMAR_BLEC_DER
#undef LEVMAR_BLEC_DIF