sba_chkjac.c

sparse bundle ajustment的源码

/* Routines for directly checking the jacobians supplied to the simple drivers.
 * They shouldn't be necessary since these jacobians can be verified indirectly
 * through the expert sba_XXX_chkjac_x() routines.
 */

/*****************************************************************************************/
// Sample code for using sba_motstr_chkjac():

  for(i=0; i<n; ++i)
    for(j=mcon; j<m; ++j){
      if(!vmask[i*m+j]) continue; // point i does not appear in image j

    sba_motstr_chkjac(proj, projac, p+j*cnp, p+m*cnp+i*pnp, j, i, cnp, pnp, mnp, adata, adata);
  }


/*****************************************************************************************/


/* union used for passing pointers to the user-supplied functions for the motstr/mot/str simple drivers */
union proj_projac{
  struct{
    void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata);
    void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata);
  } motstr;

  struct{
    void (*proj)(int j, int i, double *aj, double *xij, void *adata);
    void (*projac)(int j, int i, double *aj, double *Aij, void *adata);
  } mot;

  struct{
    void (*proj)(int j, int i, double *bi, double *xij, void *adata);
    void (*projac)(int j, int i, double *bi, double *Bij, void *adata);
  } str;
};


/* 
 * Check the jacobian of a projection function in cnp+pnp variables
 * evaluated at a point p, for consistency with the function itself.
 * Simple version of the above, NOT to be called directly
 *
 * Based on fortran77 subroutine CHKDER by
 * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
 * Argonne National Laboratory. MINPACK project. March 1980.
 *
 *
 * proj points to a function from R^{cnp+pnp} --> R^{mnp}: Given a p=(aj, bi) in R^{cnp+pnp}
 *      it yields hx in R^{mnp}
 * projac points to a function implementing the jacobian of func, whose consistency with proj
 *     is to be tested. Given a p in R^{cnp+pnp}, jacf computes into the matrix jac=[Aij | Bij]
 *     jacobian of proj at p. Note that row i of jac corresponds to the gradient of the i-th
 *     component of proj, evaluated at p.
 * aj, bi are input arrays of lengths cnp, pnp containing the parameters for the point of
 *     evaluation, i.e. j-th camera and i-th point
 * jj, ii specify the point (ii) whose projection jacobian in image (jj) is being checked
 * cnp, pnp, mnp are as usual. Note that if cnp=0 or
 *     pnp=0 a jacobian corresponding resp. to motion or camera parameters
 *     only is assumed.
 * func_adata, jac_adata point to possible additional data and are passed
 *     uninterpreted to func, jacf respectively.
 * err is an array of length mnp. On output, err contains measures
 *     of correctness of the respective gradients. if there is
 *     no severe loss of significance, then if err[i] is 1.0 the
 *     i-th gradient is correct, while if err[i] is 0.0 the i-th
 *     gradient is incorrect. For values of err between 0.0 and 1.0,
 *     the categorization is less certain. In general, a value of
 *     err[i] greater than 0.5 indicates that the i-th gradient is
 *     probably correct, while a value of err[i] less than 0.5
 *     indicates that the i-th gradient is probably incorrect.
 *
 * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
 * following excerpt comes from CHKDER's documentation:
 *
 *     "The function does not perform reliably if cancellation or
 *     rounding errors cause a severe loss of significance in the
 *     evaluation of a function. therefore, none of the components
 *     of p should be unusually small (in particular, zero) or any
 *     other value which may cause loss of significance."
 */

static void sba_chkjac(
    union proj_projac *funcs, double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
const double factor=100.0, one=1.0, zero=0.0;
double *fvec, *fjac, *Aij, *Bij, *ajp, *bip, *fvecp, *buf, *err;

int Asz, Bsz;
register int i, j;
double eps, epsf, temp, epsmch, epslog;
int fvec_sz, ajp_sz, bip_sz, fvecp_sz, err_sz, numerr=0;

  epsmch=DBL_EPSILON;
  eps=sqrt(epsmch);

  Asz=mnp*cnp; Bsz=mnp*pnp;
  fjac=(double *)emalloc((Asz+Bsz)*sizeof(double));
  Aij=fjac;
  Bij=Aij+Asz;

  fvec_sz=fvecp_sz=mnp;
  ajp_sz=cnp; bip_sz=pnp;
  err_sz=mnp;
  buf=(double *)emalloc((fvec_sz + ajp_sz + bip_sz + fvecp_sz + err_sz)*sizeof(double));
  fvec=buf;
  ajp=fvec+fvec_sz;
  bip=ajp+ajp_sz;
  fvecp=bip+bip_sz;
  err=fvecp+fvecp_sz;

  /* compute fvec=proj(p), p=(aj, bi) & the jacobian at p */
  if(cnp && pnp){
    (*(funcs->motstr.proj))(jj, ii, aj, bi, fvec, func_adata);
    (*(funcs->motstr.projac))(jj, ii, aj, bi, Aij, Bij, jac_adata);
  }
  else if(cnp){
    (*(funcs->mot.proj))(jj, ii, aj, fvec, func_adata);
    (*(funcs->mot.projac))(jj, ii, aj, Aij, jac_adata);
  }
  else{
    (*(funcs->str.proj))(jj, ii, bi, fvec, func_adata);
    (*(funcs->str.projac))(jj, ii, bi, Bij, jac_adata);
  }

  /* compute pp, pp=(ajp, bip) */
  for(j=0; j<cnp; ++j){
    temp=eps*FABS(aj[j]);
    if(temp==zero) temp=eps;
    ajp[j]=aj[j]+temp;
  }
  for(j=0; j<pnp; ++j){
    temp=eps*FABS(bi[j]);
    if(temp==zero) temp=eps;
    bip[j]=bi[j]+temp;
  }

  /* compute fvecp=proj(pp) */
  if(cnp && pnp)
    (*(funcs->motstr.proj))(jj, ii, ajp, bip, fvecp, func_adata);
  else if(cnp)
    (*(funcs->mot.proj))(jj, ii, ajp, fvecp, func_adata);
  else
    (*(funcs->str.proj))(jj, ii, bip, fvecp, func_adata);

  epsf=factor*epsmch;
  epslog=log10(eps);

  for(i=0; i<mnp; ++i)
    err[i]=zero;

  for(j=0; j<cnp; ++j){
    temp=FABS(aj[j]);
    if(temp==zero) temp=one;

    for(i=0; i<mnp; ++i)
      err[i]+=temp*Aij[i*cnp+j];
  }
  for(j=0; j<pnp; ++j){
    temp=FABS(bi[j]);
    if(temp==zero) temp=one;

    for(i=0; i<mnp; ++i)
      err[i]+=temp*Bij[i*pnp+j];
  }

  for(i=0; i<mnp; ++i){
    temp=one;
    if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
        temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
    err[i]=one;
    if(temp>epsmch && temp<eps)
        err[i]=(log10(temp) - epslog)/epslog;
    if(temp>=eps) err[i]=zero;
  }

  for(i=0; i<mnp; ++i)
    if(err[i]<=0.5){
      fprintf(stderr, "SBA: gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
                i, i, ii, jj, (err[i]==0.0)? "wrong" : "probably wrong", err[i]);
      ++numerr;
  }
  if(numerr) fprintf(stderr, "SBA: found %d suspicious gradients out of %d\n\n", numerr, mnp);

  free(fjac);
  free(buf);

  return;
}

void sba_motstr_chkjac(
    void (*proj)(int jj, int ii, double *aj, double *bi, double *xij, void *adata),
    void (*projac)(int jj, int ii, double *aj, double *bi, double *Aij, double *Bij, void *adata),
    double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
union proj_projac funcs;

  funcs.motstr.proj=proj;
  funcs.motstr.projac=projac;

  sba_chkjac(&funcs, aj, bi, jj, ii, cnp, pnp, mnp, func_adata, jac_adata);
}

void sba_mot_chkjac(
    void (*proj)(int jj, int ii, double *aj, double *xij, void *adata),
    void (*projac)(int jj, int ii, double *aj, double *Aij, void *adata),
    double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
union proj_projac funcs;

  funcs.mot.proj=proj;
  funcs.mot.projac=projac;

  sba_chkjac(&funcs, aj, NULL, jj, ii, cnp, 0, mnp, func_adata, jac_adata);
}

void sba_str_chkjac(
    void (*proj)(int jj, int ii, double *bi, double *xij, void *adata),
    void (*projac)(int jj, int ii, double *bi, double *Bij, void *adata),
    double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
{
union proj_projac funcs;

  funcs.str.proj=proj;
  funcs.str.projac=projac;

  sba_chkjac(&funcs, NULL, bi, jj, ii, 0, pnp, mnp, func_adata, jac_adata);
}
#endif /* 0 */