sba.c

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

/* ////////////////////////////////////////////////////////////////////////////////
// 
//  Matlab MEX file for sba's simple drivers
//  Copyright (C) 2007-2008 Manolis Lourakis (lourakis at ics forth gr)
//  Institute of Computer Science, Foundation for Research & Technology - Hellas
//  Heraklion, Crete, Greece.
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//////////////////////////////////////////////////////////////////////////////// */

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>
#include <ctype.h>

#include <time.h>

#include <sba.h>

#include "mex.h"

/**
#define DEBUG
**/

#define _MAX_(A, B)     ((A)>=(B)? (A) : (B))
#define _MIN_(A, B)     ((A)<=(B)? (A) : (B))

#define MININARGS      12
#define MINOUTARGS     2

#define BA_MOTSTRUCT            0
#define BA_MOT                  1
#define BA_STRUCT               2


#define CLOCKS_PER_MSEC (CLOCKS_PER_SEC/1000.0)

#define MAXNAMELEN              256


#ifdef WIN32
#include <windows.h>

#define LOADFUNCTION GetProcAddress
#define FUNCTIONISVALID(ptr) ((ptr)!=NULL)

typedef HINSTANCE LibHandle;

#else /* !WIN32, assume Un*x */
#include <dlfcn.h>

#define LOADFUNCTION dlsym
#define FUNCTIONISVALID(ptr) (dlerror()==NULL)

typedef void *LibHandle;

#endif /* WIN32 */

struct mexdata {
  /* matlab or dynlib names of the fitting function & its Jacobian */
  char projname[MAXNAMELEN], projacname[MAXNAMELEN];

  /* binary flags specifying if input p0 is a row or column vector */
  int isrow_p0;

  /* rhs args to be passed to matlab. note that rhs[0], rhs[1] are reserved
   * for passing the camera & structure parameter vectors, respectively.
   * If present, problem-specific data are passed in rhs[2], rhs[3], ... 
   */
  mxArray **rhs;
  int nrhs; /* >= 2 */

  /* dynamic libraries stuff */
  LibHandle projlibhandle, projaclibhandle; /* handles */
  double **dynadata; /* optional additional data to be passed to the dynlib functions */
  char projlibname[MAXNAMELEN], projaclibname[MAXNAMELEN]; /* filenames */


  /* problem dimension parameters */
  int cnp, pnp, mnp;

  /* pointers to camera & structure parameters */
  double *pa, *pb;
};


/* display printf-style error messages in matlab */
static void matlabFmtdErrMsgTxt(char *fmt, ...)
{
char  buf[256];
va_list args;

	va_start(args, fmt);
	vsprintf(buf, fmt, args);
	va_end(args);

  mexErrMsgTxt(buf);
}

/* display printf-style warning messages in matlab */
static void matlabFmtdWarnMsgTxt(char *fmt, ...)
{
char  buf[256];
va_list args;

	va_start(args, fmt);
	vsprintf(buf, fmt, args);
	va_end(args);

  mexWarnMsgTxt(buf);
}

#if 0
/* matlab matrices are in column-major, this routine converts them to row major for sba */
static double *getTransposeDbl(mxArray *Am)
{
int m, n;
double *At, *A;
register int i, j;

  m=mxGetM(Am);
  n=mxGetN(Am);
  A=mxGetPr(Am);
  At=mxMalloc(m*n*sizeof(double));

  for(i=0; i<m; ++i)
    for(j=0; j<n; ++j)
      At[i*n+j]=A[i+j*m];
  
  return At;
}
#endif

/* get visibility mask from a dense matlab array */
static char *getVMaskDense(mxArray *Am)
{
int m, n;
double *A;
char *At;
register int i, j;

  m=mxGetM(Am);
  n=mxGetN(Am);
  A=mxGetPr(Am);
  At=mxMalloc(m*n*sizeof(char));

  for(i=0; i<m; ++i)
    for(j=0; j<n; ++j)
      At[i*n+j]=(A[i+j*m])? 1 : 0;
  
  return At;
}

/* get visibility mask from a sparse matlab array */
/* matlab stores sparse arrays in the Compressed Column Storage (CCS) format */
char *getVMaskSparse(mxArray *Am)
{
int m, n, *rowidx, *colptr;
/* int nnz=mxGetNzmax(Am); */
double *val;
register int i, j;
char *At;

  m=mxGetM(Am);
  n=mxGetN(Am);
  val=mxGetPr(Am);
  rowidx=mxGetIr(Am);
  colptr=mxGetJc(Am);

  At=mxMalloc(m*n*sizeof(char));
  for(i=0; i<m*n; ++i)
    At[i]=0;

  for(j=0; j<n; ++j)
    for(i=colptr[j]; i<colptr[j+1]; ++i){
      /* element rowidx[i], j equals val[i]. matlab's sparse matrix indices are zero based */
      if(val[i]!=0.0){
        /* printf("(%d %d): %g\n", rowidx[i], j, val[i]); */
        At[rowidx[i]*n+j]=1;
      }
    }

  return At;
}

/* memory copy (i.e., x=y) using loop unrolling and blocking.
 * see http://www.abarnett.demon.co.uk/tutorial.html
 */
#define UNROLLBLOCKSIZE  8 
static void dblcopy_(double *x, double *y, int n)
{ 
register int i=0; 
int blockn;

  /* n may not be divisible by UNROLLBLOCKSIZE, 
  * go as near as we can first, then tidy up.
  */ 
  blockn=(n/UNROLLBLOCKSIZE)*UNROLLBLOCKSIZE; 

  /* unroll the loop in blocks of `UNROLLBLOCKSIZE' */ 
  while(i<blockn) { 
    x[i]  =y[i];
    x[i+1]=y[i+1];
    x[i+2]=y[i+2];
    x[i+3]=y[i+3];
    x[i+4]=y[i+4];
    x[i+5]=y[i+5];
    x[i+6]=y[i+6];
    x[i+7]=y[i+7];

    /* update the counter */ 
    i+=UNROLLBLOCKSIZE;
  } 

 /* 
  * There may be some left to do.
  * This could be done as a simple for() loop, 
  * but a switch is faster (and more interesting) 
  */ 

  if(i<n){ 
    /* Jump into the case at the place that will allow
     * us to finish off the appropriate number of items. 
     */ 

    switch(n-i){ 
      case 7 : x[i]=y[i]; ++i;
      case 6 : x[i]=y[i]; ++i;
      case 5 : x[i]=y[i]; ++i;
      case 4 : x[i]=y[i]; ++i;
      case 3 : x[i]=y[i]; ++i;
      case 2 : x[i]=y[i]; ++i;
      case 1 : x[i]=y[i]; ++i;
    }
  }

  return;
}

/** next 7 functions handle user projection & Jacobian functions coded in Matlab **/
static void proj_motstrMATLAB(int j, int i, double *aj, double *bi, double *xij, void *adata)
{
mxArray *lhs[1];
register double *mp;
register int k;
struct mexdata *dat=(struct mexdata *)adata;

  /* prepare to call matlab */
  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];

  /* 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_motstrMATLAB(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata)
{
mxArray *lhs[2];
register double *mp;
register int k;
struct mexdata *dat=(struct mexdata *)adata;

  /* prepare to call matlab */
  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];

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

  /* copy back results & cleanup. Note that the Jacobians
   * computed by matlab are returned in row-major as vectors
   */
  mp=mxGetPr(lhs[0]);
  /*
  for(k=0; k<dat->mnp*dat->cnp; ++k)
    Aij[k]=mp[k];
  */
  dblcopy_(Aij, mp, dat->mnp*dat->cnp);

  mp=mxGetPr(lhs[1]);
  /*
  for(k=0; k<dat->mnp*dat->pnp; ++k)
    Bij[k]=mp[k];
  */
  dblcopy_(Bij, mp, dat->mnp*dat->pnp);

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


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

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

  mp=mxGetPr(dat->rhs[1]);
  bi=dat->pb + i*dat->pnp;
  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_motMATLAB(int j, int i, double *aj, double *Aij, void *adata)
{
mxArray *lhs[1];
register double *mp;
double *bi;
register int k;
struct mexdata *dat=(struct mexdata *)adata;

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

  mp=mxGetPr(dat->rhs[1]);
  bi=dat->pb + i*dat->pnp;
  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