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