levmar.c

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

  /* the second required argument must be a real row or column vector */
  if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 || mxGetN(prhs[1])==1))
    mexErrMsgTxt("levmar: p0 must be a real vector.");
  p0=mxGetPr(prhs[1]);
  /* determine if we have a row or column vector and retrieve its 
   * size, i.e. the number of parameters
   */
  if(mxGetM(prhs[1])==1){
    m=mxGetN(prhs[1]);
    mdata.isrow_p0=1;
  }
  else{
    m=mxGetM(prhs[1]);
    mdata.isrow_p0=0;
  }
  /* copy input parameter vector to avoid destroying it */
  p=mxMalloc(m*sizeof(double));
  for(i=0; i<m; ++i)
    p[i]=p0[i];
    
  /** x **/
  /* the third required argument must be a real row or column vector */
  if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || !(mxGetM(prhs[2])==1 || mxGetN(prhs[2])==1))
    mexErrMsgTxt("levmar: x must be a real vector.");
  x=mxGetPr(prhs[2]);
  n=__MAX__(mxGetM(prhs[2]), mxGetN(prhs[2]));

  /** itmax **/
  /* the fourth required argument must be a scalar */
  if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
    mexErrMsgTxt("levmar: itmax must be a scalar.");
  itmax=(int)mxGetScalar(prhs[3]);
    
  /** opts **/
  /* the fifth required argument must be a real row or column vector */
  if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || (!(mxGetM(prhs[4])==1 || mxGetN(prhs[4])==1) &&
                                                      !(mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0)))
    mexErrMsgTxt("levmar: opts must be a real vector.");
  pdbl=mxGetPr(prhs[4]);
  nopts=__MAX__(mxGetM(prhs[4]), mxGetN(prhs[4]));
  if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
    if(nopts>LM_OPTS_SZ)
      matlabFmtdErrMsgTxt("levmar: opts must have at most %d elements, got %d.", LM_OPTS_SZ, nopts);
    else if(nopts<((havejac)? LM_OPTS_SZ-1 : LM_OPTS_SZ))
      matlabFmtdWarnMsgTxt("levmar: only the %d first elements of opts specified, remaining set to defaults.", nopts);
    for(i=0; i<nopts; ++i)
      opts[i]=pdbl[i];
  }
#ifdef DEBUG
  else{
    fflush(stderr);
    fprintf(stderr, "LEVMAR: empty options vector, using defaults\n");
  }
#endif /* DEBUG */

  /** mintype (optional) **/
  /* check whether sixth argument is a string */
  if(nrhs>=6 && mxIsChar(prhs[5])==1 && mxGetM(prhs[5])==1){
    char *minhowto;

    /* examine supplied name */
    len=mxGetN(prhs[5])+1;
    minhowto=mxCalloc(len, sizeof(char));
    status=mxGetString(prhs[5], minhowto, len);
    if(status!=0)
      mexErrMsgTxt("levmar: not enough space. String is truncated.");

    for(i=0; minhowto[i]; ++i)
      minhowto[i]=tolower(minhowto[i]);
    if(!strncmp(minhowto, "unc", 3)) mintype=MIN_UNCONSTRAINED;
    else if(!strncmp(minhowto, "bc", 2)) mintype=MIN_CONSTRAINED_BC;
    else if(!strncmp(minhowto, "lec", 3)) mintype=MIN_CONSTRAINED_LEC;
    else if(!strncmp(minhowto, "blec", 4)) mintype=MIN_CONSTRAINED_BLEC;
    else matlabFmtdErrMsgTxt("levmar: unknown minimization type '%s'.", minhowto);

    mxFree(minhowto);

    ++prhs;
    --nrhs;
  }
  else
    mintype=MIN_UNCONSTRAINED;

  if(mintype==MIN_UNCONSTRAINED) goto extraargs;

  /* arguments below this point are optional and their presence depends
   * upon the minimization type determined above
   */
  /** lb, ub **/
  if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BC || mintype==MIN_CONSTRAINED_BLEC)){
    /* check if the next two arguments are real row or column vectors */
    if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
      if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
        if((i=__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5])))!=m)
          matlabFmtdErrMsgTxt("levmar: lb must have %d elements, got %d.", m, i);
        if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=m)
          matlabFmtdErrMsgTxt("levmar: ub must have %d elements, got %d.", m, i);

        lb=mxGetPr(prhs[5]);
        ub=mxGetPr(prhs[6]);

        prhs+=2;
        nrhs-=2;
      }
    }
  }

  /** A, b **/
  if(nrhs>=7 && (mintype==MIN_CONSTRAINED_LEC || mintype==MIN_CONSTRAINED_BLEC)){
    /* check if the next two arguments are a real matrix and a real row or column vector */
    if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
      if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
        if((i=mxGetN(prhs[5]))!=m)
          matlabFmtdErrMsgTxt("levmar: A must have %d columns, got %d.", m, i);
        if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Arows=mxGetM(prhs[5])))
          matlabFmtdErrMsgTxt("levmar: b must have %d elements, got %d.", Arows, i);

        At=prhs[5];
        b=mxGetPr(prhs[6]);
        A=getTranspose(At);

        prhs+=2;
        nrhs-=2;
      }
    }
  }

  /* wghts */
  /* check if we have a weights vector */
  if(nrhs>=6 && mintype==MIN_CONSTRAINED_BLEC){ /* only check if we have seen both box & linear constraints */
    if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
      if(__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5]))==m){
        wghts=mxGetPr(prhs[5]);

        ++prhs;
        --nrhs;
      }
    }
  }
  /* arguments below this point are assumed to be extra arguments passed
   * to every invocation of the fitting function and its Jacobian
   */

extraargs:
  /* handle any extra args and allocate memory for
   * passing the current parameter estimate to matlab
   */
  nextra=nrhs-5;
  mdata.nrhs=nextra+1;
  mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
  for(i=0; i<nextra; ++i)
    mdata.rhs[i+1]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */
#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: %d extra args\n", nextra);
#endif /* DEBUG */

  if(mdata.isrow_p0){ /* row vector */
    mdata.rhs[0]=mxCreateDoubleMatrix(1, m, mxREAL);
    /*
    mxSetM(mdata.rhs[0], 1);
    mxSetN(mdata.rhs[0], m);
    */
  }
  else{ /* column vector */
    mdata.rhs[0]=mxCreateDoubleMatrix(m, 1, mxREAL);
    /*
    mxSetM(mdata.rhs[0], m);
    mxSetN(mdata.rhs[0], 1);
    */
  }

  /* ensure that the supplied function & Jacobian are as expected */
  if(checkFuncAndJacobian(p, m, n, havejac, &mdata)){
    status=LM_ERROR;
    goto cleanup;
  }

  if(nlhs>3) /* covariance output required */
    covar=mxMalloc(m*m*sizeof(double));

  /* invoke levmar */
  if(!lb && !ub){
    if(!A && !b){ /* no constraints */
      if(havejac)
        status=dlevmar_der(func, jacfunc, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_dif(func,          p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_der()/dlevmar_dif()\n");
#endif /* DEBUG */
    }
    else{ /* linear constraints */
#ifdef HAVE_LAPACK
      if(havejac)
        status=dlevmar_lec_der(func, jacfunc, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_lec_dif(func,          p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
      mexErrMsgTxt("levmar: no linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_lec_der()/dlevmar_lec_dif()\n");
#endif /* DEBUG */
    }
  }
  else{
    if(!A && !b){ /* box constraints */
      if(havejac)
        status=dlevmar_bc_der(func, jacfunc, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_bc_dif(func,          p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_bc_der()/dlevmar_bc_dif()\n");
#endif /* DEBUG */
    }
    else{ /* box & linear constraints */
#ifdef HAVE_LAPACK
      if(havejac)
        status=dlevmar_blec_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_blec_dif(func,          p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
      mexErrMsgTxt("levmar: no box & linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_blec_der()/dlevmar_blec_dif()\n");
#endif /* DEBUG */
    }
  }
#ifdef DEBUG
  fflush(stderr);
  printf("LEVMAR: minimization returned %d in %g iter, reason %g\n\tSolution: ", status, info[5], info[6]);
  for(i=0; i<m; ++i)
    printf("%.7g ", p[i]);
  printf("\n\n\tMinimization info:\n\t");
  for(i=0; i<LM_INFO_SZ; ++i)
    printf("%g ", info[i]);
  printf("\n");
#endif /* DEBUG */

  /* copy back return results */
  /** ret **/
  plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);
  ret=mxGetPr(plhs[0]);
  ret[0]=(double)status;

  /** popt **/
  plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, m, mxREAL) : mxCreateDoubleMatrix(m, 1, mxREAL);
  pdbl=mxGetPr(plhs[1]);
  for(i=0; i<m; ++i)
    pdbl[i]=p[i];

  /** info **/
  if(nlhs>2){
    plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL);
    pdbl=mxGetPr(plhs[2]);
    for(i=0; i<LM_INFO_SZ; ++i)
      pdbl[i]=info[i];
  }

  /** covar **/
  if(nlhs>3){
    plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL);
    pdbl=mxGetPr(plhs[3]);
    for(i=0; i<m*m; ++i) /* covariance matrices are symmetric, thus no need to transpose! */
      pdbl[i]=covar[i];
  }

cleanup:
  /* cleanup */
  mxDestroyArray(mdata.rhs[0]);
  if(A) mxFree(A);

  mxFree(mdata.fname);
  if(havejac) mxFree(mdata.jacname);
  mxFree(p);
  mxFree(mdata.rhs);
  if(covar) mxFree(covar);

  if(status==LM_ERROR)
    mexWarnMsgTxt("levmar: optimization returned with an error!");
}