sba.c

sba, a C/C++ package for generic sparse bundle adjustment is almost invariably used as the last step

   * computed by matlab is returned in row-major as a vector
   */
  mp=mxGetPr(lhs[0]);
  /*
  for(k=0; k<dat->mnp*dat->cnp; ++k)
    Aij[k]=mp[k];
  */
  dblcopy_(Aij, mp, dat->mnp*dat->cnp);

  /* delete the vector created by matlab */
  mxDestroyArray(lhs[0]);
}

static void proj_strMATLAB(int j, int i, double *bi, double *xij, void *adata)
{
mxArray *lhs[1];
register double *mp;
double *aj;
register int k;
struct mexdata *dat=(struct mexdata *)adata;

  /* prepare to call matlab */
  mp=mxGetPr(dat->rhs[0]);
  aj=dat->pa + j*dat->cnp;
  for(k=0; k<dat->cnp; ++k)
    mp[k]=aj[k];

  mp=mxGetPr(dat->rhs[1]);
  for(k=0; k<dat->pnp; ++k)
    mp[k]=bi[k];

  /* invoke matlab */
  mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->projname);

  /* copy back results & cleanup */
  mp=mxGetPr(lhs[0]);
  for(k=0; k<dat->mnp; ++k)
    xij[k]=mp[k];

  /* delete the vector created by matlab */
  mxDestroyArray(lhs[0]);
}

static void projac_strMATLAB(int j, int i, double *bi, double *Bij, void *adata)
{
mxArray *lhs[1];
register double *mp;
double *aj;
register int k;
struct mexdata *dat=(struct mexdata *)adata;

  /* prepare to call matlab */
  mp=mxGetPr(dat->rhs[0]);
  aj=dat->pa + j*dat->cnp;
  for(k=0; k<dat->cnp; ++k)
    mp[k]=aj[k];

  mp=mxGetPr(dat->rhs[1]);
  for(k=0; k<dat->pnp; ++k)
    mp[k]=bi[k];

  /* invoke matlab */
  mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->projacname);

  /* copy back results & cleanup. Note that the Jacobian 
   * computed by matlab is returned in row-major as a vector
   */
  mp=mxGetPr(lhs[0]);
  /*
  for(k=0; k<dat->mnp*dat->pnp; ++k)
    Bij[k]=mp[k];
  */
  dblcopy_(Bij, mp, dat->mnp*dat->pnp);

  /* delete the vector created by matlab */
  mxDestroyArray(lhs[0]);
}

/* check the supplied matlab projection function and its Jacobian. Returns 1 on error, 0 otherwise */
static int checkFuncAndJacobianMATLAB(double *aj, double *bi, int chkproj, int chkjac, int mintype, struct mexdata *dat)
{
mxArray *lhs[2]={NULL, NULL};
register int k;
int nlhs, ret=0;
double *mp;

  mexSetTrapFlag(1); /* handle errors in the MEX-file */

  mp=mxGetPr(dat->rhs[0]);
  for(k=0; k<dat->cnp; ++k)
    mp[k]=aj[k];

  mp=mxGetPr(dat->rhs[1]);
  for(k=0; k<dat->pnp; ++k)
    mp[k]=bi[k];

  if(chkproj){
    /* attempt to call the supplied proj */
    k=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->projname);
    if(k){
      fprintf(stderr, "sba: error calling '%s'.\n", dat->projname);
      ret=1;
    }
    else if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || !(mxGetM(lhs[0])==1 || mxGetN(lhs[0])==1) ||
        _MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp){
      fprintf(stderr, "sba: '%s' should produce a real vector with %d elements (got %d).\n",
                      dat->projname, dat->mnp, _MAX_(mxGetM(lhs[0]), mxGetN(lhs[0])));
      ret=1;
    }
    /* delete the vector created by matlab */
    mxDestroyArray(lhs[0]);
  }

  if(chkjac){
    lhs[0]=lhs[1]=NULL;
    nlhs=(mintype==BA_MOTSTRUCT)? 2 : 1;
    /* attempt to call the supplied jac */
    k=mexCallMATLAB(nlhs, lhs, dat->nrhs, dat->rhs, dat->projacname);
    if(k){
      fprintf(stderr, "sba: error calling '%s'.\n", dat->projacname);
      ret=1;
    }
    else if(mintype==BA_MOTSTRUCT || mintype==BA_MOT){
      if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) ||
                    _MIN_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=1 || 
                    _MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp*dat->cnp){
        fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its first output arg (got %dx%d).\n",
                        dat->projacname, dat->mnp*dat->cnp, mxGetM(lhs[0]), mxGetN(lhs[0]));
        ret=1;
      }
    }
    else{ /* BA_STRUCT */
      if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) ||
                    _MIN_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=1 ||
                    _MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp*dat->pnp){
        fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its first output arg (got %dx%d).\n",
                        dat->projacname, dat->mnp*dat->pnp, mxGetM(lhs[0]), mxGetN(lhs[0]));
        ret=1;
      }
    }

    if(lhs[0] && mxIsSparse(lhs[0])){
      fprintf(stderr, "sba: '%s' should produce a real dense vector as its first output arg, not a sparse one.\n");
      ret=1;
    }

    if(nlhs==2){ /* BA_MOTSTRUCT */
      if(!lhs[1] || !mxIsDouble(lhs[1]) || mxIsComplex(lhs[1]) ||
                    _MIN_(mxGetM(lhs[1]), mxGetN(lhs[1]))!=1 ||
                    _MAX_(mxGetM(lhs[1]), mxGetN(lhs[1]))!=dat->mnp*dat->pnp){
        fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its second output arg (got %dx%d).\n",
                      dat->projacname, dat->mnp*dat->pnp, mxGetM(lhs[1]), mxGetN(lhs[1]));
        ret=1;
      }
      else if(lhs[1] && mxIsSparse(lhs[1])){
        fprintf(stderr, "sba: '%s' should produce a real dense vector as its second output arg, not a sparse one.\n");
        ret=1;
      }
    }

    /* delete the vectors created by matlab */
    for(k=0; k<nlhs; ++k)
      mxDestroyArray(lhs[k]);
  }

  mexSetTrapFlag(0); /* on error terminate the MEX-file and return control to the MATLAB prompt */

  return ret;
}


/** next 6 functions handle user projection & Jacobian functions coming from dynlibs **/
static void proj_motstrDL(int j, int i, double *aj, double *bi, double *xij, void *adata)
{
struct mexdata *dat=(struct mexdata *)adata;
typedef void (*projMS)(double *aj, double *bi, double *xij, double **adata);
projMS proj;

  /* get pointer to function... */
  proj=(projMS)LOADFUNCTION(dat->projlibhandle, dat->projname);
  if(!FUNCTIONISVALID(proj))
    matlabFmtdErrMsgTxt("sba: error loading projection function '%s' from dynamic library %s\n", dat->projname, dat->projlibname);

  /* ...and call it */
  (*proj)(aj, bi, xij, dat->dynadata);
}

static void projac_motstrDL(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata)
{
struct mexdata *dat=(struct mexdata *)adata;
typedef void (*projacMS)(double *aj, double *bi, double *Aij, double *Bij, double **adata);
projacMS projac;

  /* get pointer to function... */
  projac=(projacMS)LOADFUNCTION(dat->projaclibhandle, dat->projacname);
  if(!FUNCTIONISVALID(projac))
    matlabFmtdErrMsgTxt("sba: error loading Jacobian function '%s' from dynamic library %s\n", dat->projacname, dat->projaclibname);

  /* ...and call it */
  (*projac)(aj, bi, Aij, Bij, dat->dynadata);
}


static void proj_motDL(int j, int i, double *aj, double *xij, void *adata)
{
struct mexdata *dat=(struct mexdata *)adata;
typedef void (*projM)(double *aj, double *bi, double *xij, double **adata);
projM proj;
double *bi;

  /* get pointer to function... */
  proj=(projM)LOADFUNCTION(dat->projlibhandle, dat->projname);
  if(!FUNCTIONISVALID(proj))
    matlabFmtdErrMsgTxt("sba: error loading projection function '%s' from dynamic library %s\n", dat->projname, dat->projlibname);

  /* ...and call it */
  bi=dat->pb + i*dat->pnp;
  (*proj)(aj, bi, xij, dat->dynadata);
}

static void projac_motDL(int j, int i, double *aj, double *Aij, void *adata)
{
struct mexdata *dat=(struct mexdata *)adata;
typedef void (*projacM)(double *aj, double *bi, double *Aij, double **adata);
projacM projac;
double *bi;

  /* get pointer to function... */
  projac=(projacM)LOADFUNCTION(dat->projaclibhandle, dat->projacname);
  if(!FUNCTIONISVALID(projac))
    matlabFmtdErrMsgTxt("sba: error loading Jacobian function '%s' from dynamic library %s\n", dat->projacname, dat->projaclibname);

  /* ...and call it */
  bi=dat->pb + i*dat->pnp;
  (*projac)(aj, bi, Aij, dat->dynadata);
}

static void proj_strDL(int j, int i, double *bi, double *xij, void *adata)
{
struct mexdata *dat=(struct mexdata *)adata;
typedef void (*projS)(double *aj, double *bi, double *xij, double **adata);
projS proj;
double *aj;

  /* get pointer to function... */
  proj=(projS)LOADFUNCTION(dat->projlibhandle, dat->projname);
  if(!FUNCTIONISVALID(proj))
    matlabFmtdErrMsgTxt("sba: error loading projection function '%s' from dynamic library %s\n", dat->projname, dat->projlibname);

  /* ...and call it */
  aj=dat->pa + j*dat->cnp;
  (*proj)(aj, bi, xij, dat->dynadata);
}

static void projac_strDL(int j, int i, double *bi, double *Bij, void *adata)
{
struct mexdata *dat=(struct mexdata *)adata;
typedef void (*projacS)(double *aj, double *bi, double *Bij, double **adata);
projacS projac;
double *aj;

  /* get pointer to function... */
  projac=(projacS)LOADFUNCTION(dat->projaclibhandle, dat->projacname);
  if(!FUNCTIONISVALID(projac))
    matlabFmtdErrMsgTxt("sba: error loading Jacobian function '%s' from dynamic library %s\n", dat->projacname, dat->projaclibname);

  /* ...and call it */
  aj=dat->pa + j*dat->cnp;
  (*projac)(aj, bi, Bij, dat->dynadata);
}

/*
 
[ret, p, info]=sba(n, m, mcon, vmask, p, cnp, pnp, x, covx, mnp, proj, projac, itmax, verbose, opts, reftype, ...);

    Most arguments are straightforward to explain, please refer to the description of their homonymous ones
    in the C version. Below, arguments that have a special meaning in matlab are discussed in more detail.

  * vmask is a nxm matrix specifying the points visible in each camera. It can be either a dense or a sparse
    matrix; in both cases, nonzero elements indicate that a certain point is visible from the corresponding
    camera.

  * reftype specifies the type of refinement to be carried out and can be one of:
      'motstr' % refinement of motion & structure, default
      'mot'    % refinement of motion only
      'str'    % refinement of structure only (mcon is redundant in this case)
    if reftype is omitted, 'motstr is assumed'.

  * proj is a matlab function that computes the projection on camera j with parameters aj
    of point i with parameters bi; it should accept at least two input args: xij=proj(aj, bi);
    Any additional arguments passed to sba, are passed unaltered to each invocation of proj.
    This technique has been used in the demonstrated example to pass the intrinsic calibration
    parameters.

  * projac is a matlab function computing the Jacobian of proj, as follows:
    if reftype equals 'motstr', then [Aij, Bij]=projac(aj, bi);
    if reftype equals 'mot', then Aij=projac(aj, bi);
    if reftype equals 'str', then Bij=projac(aj, bi);
    Aij denotes the Jacobian of xij with respect to aj, Bij the Jacobian of xij with respect to bi.
    Any additional arguments passed to sba, are passed unaltered to each invocation of projac.

*/

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
int n, 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]);

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

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

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

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