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 run_sfm(int num_pts, int num_cameras, int ncons,
char *vmask,
double *projections,
int est_focal_length,
int const_focal_length,
int undistort,
int explicit_camera_centers,
camera_params_t *init_camera_params,
v3_t *init_pts,
int use_constraints,
int use_point_constraints,
v3_t *pt_constraints,
double pt_constraint_weight,
int fix_points,
int optimize_for_fisheye,
double eps2,
double *Vout,
double *Sout,
double *Uout, double *Wout
/* size num_cameras ** 2 * cnp * cnp */)
{
int cnp;
double *params;
#ifdef SBA_V121
double opts[6]; // opts[5];
#else
double opts[3];
#endif
double info[10];
int i, j, idx, base;
int num_camera_params, num_pt_params, num_params;
sfm_global_t global_params;
// #ifndef SBA_V121
camera_constraints_t *constraints = NULL;
// #endif
point_constraints_t *point_constraints = NULL;
const double f_scale = 0.001;
const double k_scale = 5.0;
if (est_focal_length)
cnp = 7;
else
cnp = 6;
if (undistort)
cnp += 2;
num_camera_params = cnp * num_cameras;
num_pt_params = 3 * num_pts;
num_params = num_camera_params + num_pt_params;
params = (double *) safe_malloc(sizeof(double) * num_params, "params");
/* Fill parameters */
for (j = 0; j < num_cameras; j++) {
int c = 0;
#ifdef TEST_FOCAL
init_camera_params[j].f_scale = f_scale;
init_camera_params[j].k_scale = k_scale;
#else
init_camera_params[j].f_scale = 1.0;
init_camera_params[j].k_scale = 1.0;
#endif
/* Translation is zero */
params[cnp * j + 0] = init_camera_params[j].t[0]; // 0.0;
params[cnp * j + 1] = init_camera_params[j].t[1]; // 0.0;
params[cnp * j + 2] = init_camera_params[j].t[2]; // 0.0;
/* Rotation is zero */
params[cnp * j + 3] = 0.0;
params[cnp * j + 4] = 0.0;
params[cnp * j + 5] = 0.0;
if (est_focal_length) {
/* Focal length is initial estimate */
#ifndef TEST_FOCAL
params[cnp * j + 6] = init_camera_params[j].f;
#else
params[cnp * j + 6] =
init_camera_params[j].f * init_camera_params[j].f_scale;
#endif
c = 7;
} else {
c = 6;
}
if (undistort) {
#ifndef TEST_FOCAL
params[cnp * j + c] = init_camera_params[j].k[0];
params[cnp * j + c+1] = init_camera_params[j].k[1];
#else
double scale = init_camera_params[j].k_scale;
params[cnp * j + c] = init_camera_params[j].k[0] * scale;
params[cnp * j + c+1] = init_camera_params[j].k[1] * scale;
#endif
}
}
base = num_camera_params;
for (i = 0; i < num_pts; i++) {
params[base + 3 * i + 0] = Vx(init_pts[i]);
params[base + 3 * i + 1] = Vy(init_pts[i]);
params[base + 3 * i + 2] = Vz(init_pts[i]);
}
opts[0] = 1.0e-3;
opts[1] = 1.0e-10; // 1.0e-15;
opts[2] = eps2; // 0.0; // 1.0e-10; // 1.0e-15;
#ifdef SBA_V121
opts[3] = 1.0e-12;
// opts[4] = 4.0e-2;
opts[4] = 0.0;
opts[5] = 4.0e-2; // change this back to opts[4] for sba v1.2.1
#endif
// opts[1] = 1.0e-8;
// opts[2] = 1.0e-8;
// #ifndef SBA_V121
/* Create the constraints */
if (use_constraints) {
constraints =
(camera_constraints_t *)
malloc(num_cameras * sizeof(camera_constraints_t));
for (i = 0; i < num_cameras; i++) {
constraints[i].constrained = (char *) malloc(cnp);
constraints[i].constraints =
(double *) malloc(sizeof(double) * cnp);
constraints[i].weights = (double *) malloc(sizeof(double) * cnp);
memcpy(constraints[i].constrained,
init_camera_params[i].constrained, cnp * sizeof(char));
memcpy(constraints[i].constraints,
init_camera_params[i].constraints, cnp * sizeof(double));
memcpy(constraints[i].weights,
init_camera_params[i].weights, cnp * sizeof(double));
#ifdef TEST_FOCAL
if (est_focal_length) {
constraints[i].constraints[6] *= f_scale;
constraints[i].weights[6] *= (1.0 / (f_scale * f_scale));
}
if (undistort) {
constraints[i].constraints[7] *= k_scale;
constraints[i].weights[7] *= (1.0 / (k_scale * k_scale));
constraints[i].constraints[8] *= k_scale;
constraints[i].weights[8] *= (1.0 / (k_scale * k_scale));
}
#endif
}
}
// #endif
if (use_point_constraints) {
point_constraints =
(point_constraints_t *)
malloc(num_pts * sizeof(point_constraints_t));
for (i = 0; i < num_pts; i++) {
if (Vx(pt_constraints[i]) == 0.0 &&
Vy(pt_constraints[i]) == 0.0 &&
Vz(pt_constraints[i]) == 0.0) {
point_constraints[i].constrained = 0;
point_constraints[i].constraints[0] = 0.0;
point_constraints[i].constraints[1] = 0.0;
point_constraints[i].constraints[2] = 0.0;
point_constraints[i].weight = 0.0;
} else {
// printf("[run_sfm] Constraining point %d\n", i);
point_constraints[i].constrained = 1;
point_constraints[i].weight = pt_constraint_weight;
point_constraints[i].constraints[0] = Vx(pt_constraints[i]);
point_constraints[i].constraints[1] = Vy(pt_constraints[i]);
point_constraints[i].constraints[2] = Vz(pt_constraints[i]);
}
}
}
/* Fill global param struct */
global_params.num_cameras = num_cameras;
global_params.num_points = num_pts;
global_params.num_params_per_camera = cnp;
global_params.est_focal_length = est_focal_length;
global_params.const_focal_length = const_focal_length;
global_params.estimate_distortion = undistort;
global_params.explicit_camera_centers = explicit_camera_centers,
global_params.global_params.f = 1.0;
global_params.init_params = init_camera_params;
global_last_ws =
safe_malloc(3 * num_cameras * sizeof(double), "global_last_ws");
global_last_Rs =
safe_malloc(9 * num_cameras * sizeof(double), "global_last_ws");
global_params.points = init_pts;
for (i = 0; i < num_cameras; i++) {
global_last_ws[3 * i + 0] = 0.0;
global_last_ws[3 * i + 1] = 0.0;
global_last_ws[3 * i + 2] = 0.0;
memcpy(global_last_Rs + 9 * i,
init_camera_params[i].R, 9 * sizeof(double));
}
/* Run sparse bundle adjustment */
#define MAX_ITERS 150 // 256
#define VERBOSITY 3
#ifdef SBA_V121
if (fix_points == 0) {
if (optimize_for_fisheye == 0) {
sba_motstr_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, 3, projections, NULL, 2,
//remove NULL in prev line for sba v1.2.1
sfm_project_point3, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints,
use_point_constraints,
point_constraints, Vout, Sout, Uout, Wout);
} else {
sba_motstr_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, 3, projections, NULL, 2,
sfm_project_point2_fisheye, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints,
use_point_constraints,
point_constraints, Vout, Sout, Uout, Wout);
}
} else {
if (optimize_for_fisheye == 0) {
sba_mot_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, projections, NULL, 2,
sfm_project_point3_mot, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints);
} else {
sba_mot_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, projections, NULL, 2,
sfm_project_point2_fisheye_mot, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints);
}
}
#else
if (fix_points == 0) {
sba_motstr_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, 3, projections, 2,
sfm_project_point2, NULL, (void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints,
Vout, Sout, Uout, Wout);
} else {
sba_mot_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, projections, 2,
sfm_mot_project_point, NULL, (void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info);
}
#endif
printf("[run_sfm] Number of iterations: %d\n", (int) info[5]);
printf("info[6] = %0.3f\n", info[6]);
/* Copy out the params */
for (j = 0; j < num_cameras; j++) {
double *dt = params + cnp * j + 0;
double *w = params + cnp * j + 3;
double Rnew[9];
int c;
/* Translation */
init_camera_params[j].t[0] = dt[0];
init_camera_params[j].t[1] = dt[1];
init_camera_params[j].t[2] = dt[2];
// init_camera_params[j].t[0] += dt[0];
// init_camera_params[j].t[1] += dt[1];
// init_camera_params[j].t[2] += dt[2];
/* Rotation */
rot_update(init_camera_params[j].R, w, Rnew);
memcpy(init_camera_params[j].R, Rnew, 9 * sizeof(double));
/* Focal length */
if (est_focal_length) {
c = 7;
#ifndef TEST_FOCAL
init_camera_params[j].f = params[cnp * j + 6];
#else
init_camera_params[j].f =
params[cnp * j + 6] / init_camera_params[j].f_scale;
#endif
} else {
c = 6;
}
if (undistort) {
#ifndef TEST_FOCAL
init_camera_params[j].k[0] = params[cnp * j + c];
init_camera_params[j].k[1] = params[cnp * j + c+1];
#else
double scale = init_camera_params[j].k_scale;
init_camera_params[j].k[0] = params[cnp * j + c] / scale;
init_camera_params[j].k[1] = params[cnp * j + c+1] / scale;
#endif
}
#ifdef TEST_FOCAL
init_camera_params[j].f_scale = 1.0;
init_camera_params[j].k_scale = 1.0;
#endif
}
base = num_camera_params;
for (i = 0; i < num_pts; i++) {
Vx(init_pts[i]) = params[base + 3 * i + 0];
Vy(init_pts[i]) = params[base + 3 * i + 1];
Vz(init_pts[i]) = params[base + 3 * i + 2];
}
// #define DEBUG_SFM
#ifdef DEBUG_SFM
for (i = 0; i < num_cameras; i++) {
int num_projs = 0;
double error = 0.0;
double error_max = 0.0;
int idx_max = 0;
double px_max = 0.0, py_max = 0.0;
double K[9] = { init_camera_params[i].f, 0.0, 0.0,
0.0, init_camera_params[i].f, 0.0,
0.0, 0.0, 1.0 };
double w[3] = { 0.0, 0.0, 0.0 };
double dt[3] = { init_camera_params[i].t[0],
init_camera_params[i].t[1],
init_camera_params[i].t[2] };
// double dt[3] = { 0.0, 0.0, 0.0 };
for (j = 0; j < num_pts; j++) {
double b[3], pr[2];
double dx, dy, dist;
if (!vmask[j * num_cameras + i])
continue;
b[0] = Vx(init_pts[j]);
b[1] = Vy(init_pts[j]);
b[2] = Vz(init_pts[j]);
sfm_project(&(init_camera_params[i]), K, w, dt, b, pr,
global_params.explicit_camera_centers);
dx = pr[0] - Vx(projections[j * num_cameras + i]);
dy = pr[1] - Vy(projections[j * num_cameras + i]);
dist = dx * dx + dy * dy;
error += dist;
if (dist > error_max) {
idx_max = j;
error_max = dist;
px_max = Vx(projections[j * num_cameras + i]);
py_max = Vy(projections[j * num_cameras + i]);
}
num_projs++;
}
printf("Camera %d: error = %0.3f (%0.3f)\n", i,
error, sqrt(error / num_projs));
printf(" error_max = %0.3f (%d)\n", sqrt(error_max), idx_max);
printf(" proj = %0.3f, %0.3f\n", px_max, py_max);
}
#endif /* DEBUG_SFM */
free(params);
// #ifndef SBA_V121
if (use_constraints) {
for (i = 0; i < num_cameras; i++) {
free(constraints[i].constraints);
free(constraints[i].constrained);
free(constraints[i].weights);
}
free(constraints);
}
free(global_last_ws);
free(global_last_Rs);
// #endif
}
