Esempio n. 1
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
int n, ncon, m, mcon, cnp, pnp, mnp, nvars, nprojs, minnvars;
int status, reftype=BA_MOTSTRUCT, itmax, verbose=0, havejac, havedynproj, havedynprojac;
int len, nopts, nextra, nreserved, covlen;
double *p0, *p, *x, *covx=NULL;
double opts[SBA_OPTSSZ]={SBA_INIT_MU, SBA_STOP_THRESH, SBA_STOP_THRESH, SBA_STOP_THRESH, 0.0};
double info[SBA_INFOSZ];
char *vmask, *str;
register double *pdbl;
mxArray **prhs=(mxArray **)&Prhs[0];
struct mexdata mdata;
const int min1=MININARGS-1;

static char *reftypename[]={"motion & structure", "motion only", "structure only"};

clock_t start_time, end_time;

  /* parse input args; start by checking their number */
  if(nrhs<MININARGS)
    matlabFmtdErrMsgTxt("sba: at least %d input arguments required (got %d).", MININARGS, nrhs);
  if(nlhs<MINOUTARGS)
    matlabFmtdErrMsgTxt("sba: at least %d output arguments required (got %d).", MINOUTARGS, nlhs);
    
  /** n **/
  /* the first argument must be a scalar */
  if(!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) || mxGetM(prhs[0])!=1 || mxGetN(prhs[0])!=1)
    mexErrMsgTxt("sba: n must be a scalar.");
  n=(int)mxGetScalar(prhs[0]);

  /** ncon **/
  /* the second argument must be a scalar */
  if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || mxGetM(prhs[1])!=1 || mxGetN(prhs[1])!=1)
    mexErrMsgTxt("sba: ncon must be a scalar.");
  ncon=(int)mxGetScalar(prhs[1]);

  /** m **/
  /* the third argument must be a scalar */
  if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || mxGetM(prhs[2])!=1 || mxGetN(prhs[2])!=1)
    mexErrMsgTxt("sba: m must be a scalar.");
  m=(int)mxGetScalar(prhs[2]);

  /** mcon **/
  /* the fourth argument must be a scalar */
  if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
    mexErrMsgTxt("sba: mcon must be a scalar.");
  mcon=(int)mxGetScalar(prhs[3]);

  /** mask **/
  /* the fifth argument must be a nxm matrix */
  if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || mxGetM(prhs[4])!=n || mxGetN(prhs[4])!=m)
    matlabFmtdErrMsgTxt("sba: mask must be a %dx%d matrix (got %dx%d).", n, m, mxGetM(prhs[4]), mxGetN(prhs[4]));
  if(mxIsSparse(prhs[4])) vmask=getVMaskSparse(prhs[4]);
  else vmask=getVMaskDense(prhs[4]);
#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "SBA: %s point visibility mask\n", mxIsSparse(prhs[4])? "sparse" : "dense");
#endif /* DEBUG */

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

  /** cnp **/
  /* the seventh argument must be a scalar */
  if(!mxIsDouble(prhs[6]) || mxIsComplex(prhs[6]) || mxGetM(prhs[6])!=1 || mxGetN(prhs[6])!=1)
    mexErrMsgTxt("sba: cnp must be a scalar.");
  cnp=(int)mxGetScalar(prhs[6]);

  /** pnp **/
  /* the eighth argument must be a scalar */
  if(!mxIsDouble(prhs[7]) || mxIsComplex(prhs[7]) || mxGetM(prhs[7])!=1 || mxGetN(prhs[7])!=1)
    mexErrMsgTxt("sba: pnp must be a scalar.");
  pnp=(int)mxGetScalar(prhs[7]);

  /* check that p has the right dimension */
  if(nvars!=m*cnp + n*pnp)
    matlabFmtdErrMsgTxt("sba: p must have %d elements (got %d).", m*cnp + n*pnp, nvars);

  /** x **/
  /* the ninth argument must be a vector */
  if(!mxIsDouble(prhs[8]) || mxIsComplex(prhs[8]) || !(mxGetM(prhs[8])==1 || mxGetN(prhs[8])==1))
    mexErrMsgTxt("sba: x must be a real vector.");
  x=mxGetPr(prhs[8]);
  nprojs=_MAX_(mxGetM(prhs[8]), mxGetN(prhs[8]));

  /* covx (optional) */
  /* check if the tenth argument is a vector */
  if(mxIsDouble(prhs[9]) && !mxIsComplex(prhs[9]) && (mxGetM(prhs[9])==1 || mxGetN(prhs[9])==1)){
    covlen=_MAX_(mxGetM(prhs[9]), mxGetN(prhs[9]));
    if(covlen>1){ /* make sure that argument is not a scalar */
      covx=mxGetPr(prhs[9]);

      ++prhs;
      --nrhs;
    }
  }

  /** mnp **/
  /* the tenth required argument must be a scalar */
  if(!mxIsDouble(prhs[9]) || mxIsComplex(prhs[9]) || mxGetM(prhs[9])!=1 || mxGetN(prhs[9])!=1)
    mexErrMsgTxt("sba: mnp must be a scalar.");
  mnp=(int)mxGetScalar(prhs[9]);
  nprojs/=mnp;

  /* check that x has the correct dimension, comparing with the elements in vmask */
  for(i=len=0; i<m*n; ++i)
    if(vmask[i]) ++len;
  if(nprojs!=len)
    matlabFmtdErrMsgTxt("sba: the size of x should agree with the number of non-zeros in vmask (got %d and %d).", nprojs, len);

  /* if supplied, check that covx has the correct dimension comparing with the elements in vmask */
  if(covx && covlen!=len*mnp*mnp)
    matlabFmtdErrMsgTxt("sba: covx must be a real vector of size %d (got %d).", len*mnp*mnp, covlen);

  /** proj **/ 
  /* the eleventh required argument must be a string , i.e. a char row vector */
  if(mxIsChar(prhs[10])!=1)
    mexErrMsgTxt("sba: proj argument must be a string.");
  if(mxGetM(prhs[10])!=1)
    mexErrMsgTxt("sba: proj argument must be a string (i.e. char row vector).");
  /* retrieve supplied name */
  len=mxGetN(prhs[10])+1;
  status=mxGetString(prhs[10], mdata.projname, _MIN_(len, MAXNAMELEN));
  if(status!=0)
    mexErrMsgTxt("sba: not enough space. String is truncated.");
  /* check if we have a name@library pair */
  if((str=strchr(mdata.projname, '@'))){
    *str++='\0';
    /* copy the library name */
    strcpy(mdata.projlibname, str);
    /* attempt to load the library */
#ifdef _WIN32
    mdata.projlibhandle=LoadLibrary(mdata.projlibname);
    if(!mdata.projlibhandle)
#else
    mdata.projlibhandle=dlopen(mdata.projlibname, RTLD_LAZY);
    if(!mdata.projlibhandle)
#endif /* _WIN32 */
      matlabFmtdErrMsgTxt("sba: error loading dynamic library %s!\n", mdata.projlibname);
    havedynproj=1;
  }
  else{
    mdata.projlibhandle=NULL;
    havedynproj=0;
  }

  /** jac (optional) **/
  havejac=havedynprojac=0;
  /* check whether the twelfth argument is a nonempty string */
  if(mxIsChar(prhs[11])==1){
    switch(mxGetM(prhs[11])){
      case 1:
        /* store supplied name */
        len=mxGetN(prhs[11])+1;
        status=mxGetString(prhs[11], mdata.projacname, _MIN_(len, MAXNAMELEN));
        if(status!=0)
          mexErrMsgTxt("sba: not enough space. String is truncated.");
        havejac=1;

        /* check if we have a name@library pair */
        if((str=strchr(mdata.projacname, '@'))){
          *str++='\0';
          /* copy the library name */
          strcpy(mdata.projaclibname, str);
          if(!havedynproj || strcmp(mdata.projlibname, mdata.projaclibname)){ /* is this a different library from that for the proj. function? */
            /* yes, attempt to load it */
#         ifdef _WIN32
            mdata.projaclibhandle=LoadLibrary(mdata.projaclibname);
            if(!mdata.projaclibhandle)
#         else
            mdata.projaclibhandle=dlopen(mdata.projaclibname, RTLD_LAZY);
            if(!mdata.projaclibhandle)
#         endif /* _WIN32 */
              matlabFmtdErrMsgTxt("sba: error loading dynamic library %s!\n", mdata.projaclibname);
          }
          else /* proj. function and Jacobian come from the same library */ 
            mdata.projaclibhandle=mdata.projlibhandle;
          havedynprojac=1;
        }
        else{
          mdata.projaclibhandle=NULL;
          havedynprojac=0;
        }

        /* falling through! */
      case 0: /* empty string, ignore */
        ++prhs;
        --nrhs;
        break;
      default:
        matlabFmtdErrMsgTxt("sba: projac argument must be a string (i.e. row vector); got %dx%d.",
                                  mxGetM(prhs[11]), mxGetN(prhs[11]));
    }
  }

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "SBA: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */

  /** itmax **/
  /* the twelfth required argument must be a scalar */
  if(!mxIsDouble(prhs[11]) || mxIsComplex(prhs[11]) || mxGetM(prhs[11])!=1 || mxGetN(prhs[11])!=1)
    mexErrMsgTxt("sba: itmax must be a scalar.");
  itmax=(int)mxGetScalar(prhs[11]);

  /* all arguments below this point are optional */

  /* check if we have a scalar argument; if yes, this is taken to be the 'verbose' argument */
  if(nrhs>=MININARGS){
    if(mxIsDouble(prhs[min1]) && !mxIsComplex(prhs[min1]) && mxGetM(prhs[min1])==1 && mxGetN(prhs[min1])==1){
      verbose=(int)mxGetScalar(prhs[min1]);

      ++prhs;
      --nrhs;
    }
  }

  /* check if we have a vector argument; if yes, this is taken to be the 'opts' argument */
  if(nrhs>=MININARGS && mxIsDouble(prhs[min1]) && !mxIsComplex(prhs[min1]) && ((mxGetM(prhs[min1])==1 || mxGetN(prhs[min1])==1)
                                                                     || (mxGetM(prhs[min1])==0 && mxGetN(prhs[min1])==0))){
    pdbl=mxGetPr(prhs[min1]);
    nopts=_MAX_(mxGetM(prhs[min1]), mxGetN(prhs[min1]));
    if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
      if(nopts>SBA_OPTSSZ)
        matlabFmtdErrMsgTxt("sba: opts must have at most %d elements, got %d.", SBA_OPTSSZ, nopts);
      else if(nopts<SBA_OPTSSZ)
        matlabFmtdWarnMsgTxt("sba: 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, "SBA: empty options vector, using defaults\n");
    }
#endif /* DEBUG */

    ++prhs;
    --nrhs;
  }

  /* check if we have a string argument; if yes, this is taken to be the 'reftype' argument */
  if(nrhs>=MININARGS && mxIsChar(prhs[min1])==1 && mxGetM(prhs[min1])==1){
    char *refhowto;

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

    for(i=0; refhowto[i]; ++i)
      refhowto[i]=tolower(refhowto[i]);

    if(!strcmp(refhowto, "motstr")) reftype=BA_MOTSTRUCT;
    else if(!strcmp(refhowto, "mot")) reftype=BA_MOT;
    else if(!strcmp(refhowto, "str")) reftype=BA_STRUCT;
    else matlabFmtdErrMsgTxt("sba: unknown minimization type '%s'.", refhowto);

    mxFree(refhowto);

    ++prhs;
    --nrhs;
  }
  else
    reftype=BA_MOTSTRUCT;


  /* arguments below this point are assumed to be extra arguments passed
   * to every invocation of the projection function and its Jacobian
   */

  nextra=nrhs-MININARGS+1;
#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "SBA: %d extra args\n", nextra);
#endif /* DEBUG */
  /* handle any extra args and allocate memory for
   * passing them to matlab/C
   */
  if(!havedynproj || !havedynprojac){ /* at least one of the projection and Jacobian functions are in matlab */
    nreserved=4; /* j, i, aj, bi */
    mdata.nrhs=nextra+nreserved;
    mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
    for(i=0; i<nextra; ++i)
      mdata.rhs[i+nreserved]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */

    mdata.rhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL); /* camera index */
    mdata.rhs[1]=mxCreateDoubleMatrix(1, 1, mxREAL); /* point index */
    mdata.rhs[2]=mxCreateDoubleMatrix(1, cnp, mxREAL); /* camera parameters */
    mdata.rhs[3]=mxCreateDoubleMatrix(1, pnp, mxREAL); /* point  parameters */

    mdata.dynadata=NULL;
  }
  else
    mdata.rhs=NULL;

  mdata.dynadata=NULL;
  if(havedynproj || havedynprojac){ /* at least one of the projection and Jacobian functions are from a dynlib */
    if(nextra>0){
      mdata.dynadata=(double **)mxMalloc(nextra*sizeof(double *));
      for(i=0; i<nextra; ++i)
        mdata.dynadata[i]=mxGetPr(prhs[nrhs-nextra+i]);
    }
  }
#ifdef DEBUG
  fprintf(stderr, "Projection function: %s, Jacobian: %s, %s\n", havedynproj? "dynamic" : "matlab",
                                                                 havejac? "present" : "not present", havedynprojac? "dynamic" : "matlab");
#endif

  mdata.cnp=cnp; mdata.pnp=pnp;
  mdata.mnp=mnp;

  /* ensure that the supplied matlab function & Jacobian are as expected */
  if((!havedynproj || !havedynprojac) && /* at least one in matlab */
      checkFuncAndJacobianMATLAB(0, 0, p, p+m*cnp, !havedynproj, havejac && !havedynprojac, reftype, &mdata)){ /* check using first camera & first point */
    status=SBA_ERROR;
    goto cleanup;
  }

  /* invoke sba */
  start_time=clock();
  switch(reftype){
    case BA_MOTSTRUCT:
      minnvars=nvars;
      mdata.pa=mdata.pb=NULL; /* not needed */
      status=sba_motstr_levmar(n, ncon, m, mcon, vmask, p, cnp, pnp, x, covx, mnp,
                              (havedynproj)? proj_motstrDL : proj_motstrMATLAB, (havejac)? (havedynprojac? projac_motstrDL : projac_motstrMATLAB) : NULL,
                              (void *)&mdata, itmax, verbose>1, opts, info);
      break;
    case BA_MOT:
      minnvars=m*cnp;
      mdata.pa=NULL; /* not needed */
      mdata.pb=p+m*cnp;
      /* note: only the first part of p is used in the call below (i.e. first m*cnp elements) */
      status=sba_mot_levmar(n, m, mcon, vmask, p, cnp, x, covx, mnp,
                            (havedynproj)? proj_motDL : proj_motMATLAB, (havejac)? (havedynprojac? projac_motDL : projac_motMATLAB) : NULL,
                            (void *)&mdata, itmax, verbose>1, opts, info);
      break;
    case BA_STRUCT:
      minnvars=n*pnp;
      mdata.pa=p;
      mdata.pb=NULL; /* not needed */
      status=sba_str_levmar(n, ncon, m, vmask, p+m*cnp, pnp, x, covx, mnp,
                            (havedynproj)? proj_strDL : proj_strMATLAB, (havejac)? (havedynprojac? projac_strDL : projac_strMATLAB) : NULL,
                            (void *)&mdata, itmax, verbose>1, opts, info);
      break;
    default: /* should not reach this point */
      matlabFmtdErrMsgTxt("sba: unknown refinement type %d requested.", reftype);
  }
  end_time=clock();

  if(verbose){
    fflush(stdout);
    mexPrintf("\nSBA using %d 3D pts, %d frames and %d image projections, %d variables\n",
              n, m, nprojs, minnvars);
    mexPrintf("\nRefining %s, %s Jacobian\n\n", reftypename[reftype], havejac? "analytic" : "approximate");
    mexPrintf("SBA returned %d in %g iter, reason %g, error %g [initial %g], %d/%d func/fjac evals, %d lin. systems\n",
              status, info[5], info[6], info[1]/nprojs, info[0]/nprojs, (int)info[7], (int)info[8], (int)info[9]);
    mexPrintf("Elapsed time: %.2lf seconds, %.2lf msecs\n", ((double) (end_time - start_time)) / CLOCKS_PER_SEC,
              ((double) (end_time - start_time)) / CLOCKS_PER_MSEC);
    fflush(stdout);
  }

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

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

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

cleanup:
  /* cleanup */
  mxFree(vmask);
  mxFree(p);

  if(mdata.rhs){
    for(i=0; i<nreserved; ++i)
      mxDestroyArray(mdata.rhs[i]);
    mxFree(mdata.rhs);
  }

  if(mdata.dynadata) mxFree(mdata.dynadata);

  /* unload libraries */
  if(havedynproj){
#ifdef _WIN32
    i=FreeLibrary(mdata.projlibhandle);
    if(i==0)
#else
    i=dlclose(mdata.projlibhandle);
    if(i!=0)
#endif /* _WIN32 */
      matlabFmtdErrMsgTxt("sba: error unloading dynamic library %s!\n", mdata.projlibname);
  }
  if(havedynprojac){
    if(mdata.projaclibhandle!=mdata.projlibhandle){
#ifdef _WIN32
      i=FreeLibrary(mdata.projaclibhandle);
      if(i==0)
#else
      i=dlclose(mdata.projaclibhandle);
      if(i!=0)
#endif /* _WIN32 */
        matlabFmtdErrMsgTxt("sba: error unloading dynamic library %s!\n", mdata.projaclibname);
    }
  }
}
Esempio n. 2
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
register double *pdbl;
mxArray **prhs=(mxArray **)&Prhs[0], *At, *Ct;
struct mexdata mdata;
int len, status;
double *p, *p0, *ret, *x;
int m, n, havejac, Arows, Crows, itmax, nopts, mintype, nextra;
double opts[LM_OPTS_SZ]={LM_INIT_MU, LM_STOP_THRESH, LM_STOP_THRESH, LM_STOP_THRESH, LM_DIFF_DELTA};
double info[LM_INFO_SZ];
double *lb=NULL, *ub=NULL, *A=NULL, *b=NULL, *wghts=NULL, *C=NULL, *d=NULL, *covar=NULL;

  /* parse input args; start by checking their number */
  if((nrhs<5))
    matlabFmtdErrMsgTxt("levmar: at least 5 input arguments required (got %d).", nrhs);
  if(nlhs>4)
    matlabFmtdErrMsgTxt("levmar: too many output arguments (max. 4, got %d).", nlhs);
  else if(nlhs<2)
    matlabFmtdErrMsgTxt("levmar: too few output arguments (min. 2, got %d).", nlhs);
    
  /* note that in order to accommodate optional args, prhs & nrhs are adjusted accordingly below */

  /** func **/
  /* first argument must be a string , i.e. a char row vector */
  if(mxIsChar(prhs[0])!=1)
    mexErrMsgTxt("levmar: first argument must be a string.");
  if(mxGetM(prhs[0])!=1)
    mexErrMsgTxt("levmar: first argument must be a string (i.e. char row vector).");
  /* store supplied name */
  len=mxGetN(prhs[0])+1;
  mdata.fname=mxCalloc(len, sizeof(char));
  status=mxGetString(prhs[0], mdata.fname, len);
  if(status!=0)
    mexErrMsgTxt("levmar: not enough space. String is truncated.");

  /** jac (optional) **/
  /* check whether second argument is a string */
  if(mxIsChar(prhs[1])==1){
    if(mxGetM(prhs[1])!=1)
      mexErrMsgTxt("levmar: second argument must be a string (i.e. row vector).");
    /* store supplied name */
    len=mxGetN(prhs[1])+1;
    mdata.jacname=mxCalloc(len, sizeof(char));
    status=mxGetString(prhs[1], mdata.jacname, len);
    if(status!=0)
      mexErrMsgTxt("levmar: not enough space. String is truncated.");
    havejac=1;

    ++prhs;
    --nrhs;
  }
  else{
    mdata.jacname=NULL;
    havejac=0;
  }

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */

/* CHECK 
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 && mxGetN(prhs[1])==1))
*/

  /** p0 **/
  /* 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 if(!strncmp(minhowto, "bleic", 5)) mintype=MIN_CONSTRAINED_BLEIC;
    else if(!strncmp(minhowto, "blic", 4)) mintype=MIN_CONSTRAINED_BLIC;
    else if(!strncmp(minhowto, "leic", 4)) mintype=MIN_CONSTRAINED_LEIC;
    else if(!strncmp(minhowto, "lic", 3)) mintype=MIN_CONSTRAINED_BLIC;
    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 || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_BLEIC)){
    /* 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 || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_BLEIC)){
    /* 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;
      }
    }
  }

  /** C, d **/
  if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BLEIC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_LIC)){
    /* 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: C must have %d columns, got %d.", m, i);
        if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Crows=mxGetM(prhs[5])))
          matlabFmtdErrMsgTxt("levmar: d must have %d elements, got %d.", Crows, i);

        Ct=prhs[5];
        d=mxGetPr(prhs[6]);
        C=getTranspose(Ct);

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

  /* 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 */
  switch(mintype){
    case MIN_UNCONSTRAINED: /* 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 */
    break;
    case MIN_CONSTRAINED_BC: /* 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 */
    break;
    case MIN_CONSTRAINED_LEC:  /* linear equation 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 */
    break;
    case MIN_CONSTRAINED_BLEC: /* box & linear equation 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 */
    break;
    case MIN_CONSTRAINED_BLEIC: /* box, linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
      if(havejac)
        status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
      mexErrMsgTxt("levmar: no box, linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_bleic_der()/dlevmar_bleic_dif()\n");
#endif /* DEBUG */
    break;
    case MIN_CONSTRAINED_BLIC: /* box, linear inequalities constraints */
#ifdef HAVE_LAPACK
      if(havejac)
        status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
      mexErrMsgTxt("levmar: no box & linear inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_blic_der()/dlevmar_blic_dif()\n");
#endif /* DEBUG */
    break;
    case MIN_CONSTRAINED_LEIC: /* linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
      if(havejac)
        status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
      mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_leic_der()/dlevmar_leic_dif()\n");
#endif /* DEBUG */
    break;
    case MIN_CONSTRAINED_LIC: /* linear inequalities constraints */
#ifdef HAVE_LAPACK
      if(havejac)
        status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
      else
        status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
      mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */

#ifdef DEBUG
  fflush(stderr);
  fprintf(stderr, "LEVMAR: calling dlevmar_lic_der()/dlevmar_lic_dif()\n");
#endif /* DEBUG */
    break;
    default:
      mexErrMsgTxt("levmar: unexpected internal error.");
  }

#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);
  if(C) mxFree(C);

  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!");
}