sba.c

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

  p=mxMalloc(nvars*sizeof(double));
  /*
  for(i=0; i<nvars; ++i)
    p[i]=p0[i];
  */
  dblcopy_(p, p0, nvars);

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

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

  /* 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 eighth argument must be a vector */
  if(!mxIsDouble(prhs[7]) || mxIsComplex(prhs[7]) || !(mxGetM(prhs[7])==1 || mxGetN(prhs[7])==1))
    mexErrMsgTxt("sba: x must be a real vector.");
  x=mxGetPr(prhs[7]);
  nprojs=_MAX_(mxGetM(prhs[7]), mxGetN(prhs[7]));

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

      ++prhs;
      --nrhs;
    }
  }

  /** mnp **/
  /* the ninth required argument must be a scalar */
  if(!mxIsDouble(prhs[8]) || mxIsComplex(prhs[8]) || mxGetM(prhs[8])!=1 || mxGetN(prhs[8])!=1)
    mexErrMsgTxt("sba: mnp must be a scalar.");
  mnp=(int)mxGetScalar(prhs[8]);
  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 tenth required argument must be a string , i.e. a char row vector */
  if(mxIsChar(prhs[9])!=1)
    mexErrMsgTxt("sba: proj argument must be a string.");
  if(mxGetM(prhs[9])!=1)
    mexErrMsgTxt("sba: proj argument must be a string (i.e. char row vector).");
  /* retrieve supplied name */
  len=mxGetN(prhs[9])+1;
  status=mxGetString(prhs[9], 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 eleventh argument is a nonempty string */
  if(mxIsChar(prhs[10])==1){
    switch(mxGetM(prhs[10])){
      case 1:
        /* store supplied name */
        len=mxGetN(prhs[10])+1;
        status=mxGetString(prhs[10], 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[10]), mxGetN(prhs[10]));
    }
  }

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

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

  /* 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=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=2;
    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, cnp, mxREAL); /* camera parameters */
    mdata.rhs[1]=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(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, 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, 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
      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
        matlabFmtdErrMsgTxt("sba: error unloading dynamic library %s!\n", mdata.projaclibname);
    }
  }
}