sba_chkjac.c

sparse bundle ajustment的源码

/////////////////////////////////////////////////////////////////////////////////
//// 
////  Verification routines for the jacobians employed in the expert & simple drivers
////  for sparse bundle adjustment based on the Levenberg - Marquardt minimization algorithm
////  Copyright (C) 2005-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 <math.h>
#include <float.h>

#include "compiler.h"
#include "sba.h"

#define emalloc(sz)       emalloc_(__FILE__, __LINE__, sz)

#define FABS(x)           (((x)>=0)? (x) : -(x))


/* auxiliary memory allocation routine with error checking */
inline static void *emalloc_(char *file, int line, size_t sz)
{
void *ptr;

  ptr=(void *)malloc(sz);
  if(ptr==NULL){
    fprintf(stderr, "SBA: memory allocation request for %u bytes failed in file %s, line %d, exiting", sz, file, line);
    exit(1);
  }

  return ptr;
}

/* 
 * Check the jacobian of a projection function in nvars variables
 * evaluated at a point p, for consistency with the function itself.
 * Expert version
 *
 * Based on fortran77 subroutine CHKDER by
 * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
 * Argonne National Laboratory. MINPACK project. March 1980.
 *
 *
 * func points to a function from R^{nvars} --> R^{nobs}: Given a p in R^{nvars}
 *      it yields hx in R^{nobs}
 * jacf points to a function implementing the jacobian of func, whose consistency with
 *     func is to be tested. Given a p in R^{nvars}, jacf computes into the nvis*(Asz+Bsz)
 *     matrix jac the jacobian of func at p. Note the jacobian is sparse, consisting of
 *     all A_ij, B_ij and that row i of jac corresponds to the gradient of the i-th
 *     component of func, evaluated at p.
 * p is an input array of length nvars containing the point of evaluation.
 * idxij, rcidxs, rcsubs, mcon, 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 nobs. 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."
 */

void sba_motstr_chkjac_x(
    void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
    void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
    double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, 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, *pp, *fvecp, *buf, *err;

int nvars, nobs, m, n, Asz, Bsz, ABsz, nnz;
register int i, j, ii, jj;
double eps, epsf, temp, epsmch, epslog;
register double *ptr1, *ptr2, *pab;
double *pa, *pb;
int fvec_sz, pp_sz, fvecp_sz, numerr=0;

  nobs=idxij->nnz*mnp;
  n=idxij->nr; m=idxij->nc;
  nvars=m*cnp + n*pnp;
  epsmch=DBL_EPSILON;
  eps=sqrt(epsmch);

  Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
  fjac=(double *)emalloc(idxij->nnz*ABsz*sizeof(double));

  fvec_sz=fvecp_sz=nobs;
  pp_sz=nvars;
  buf=(double *)emalloc((fvec_sz + pp_sz + fvecp_sz)*sizeof(double));
  fvec=buf;
  pp=fvec+fvec_sz;
  fvecp=pp+pp_sz;

  err=(double *)emalloc(nobs*sizeof(double));

  /* compute fvec=func(p) */
  (*func)(p, idxij, rcidxs, rcsubs, fvec, func_adata);

  /* compute the jacobian at p */
  (*jacf)(p, idxij, rcidxs, rcsubs, fjac, jac_adata);

  /* compute pp */
  for(j=0; j<nvars; ++j){
    temp=eps*FABS(p[j]);
    if(temp==zero) temp=eps;
    pp[j]=p[j]+temp;
  }

  /* compute fvecp=func(pp) */
  (*func)(pp, idxij, rcidxs, rcsubs, fvecp, func_adata);

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

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

  pa=p;
  pb=p + m*cnp;
  for(i=0; i<n; ++i){
    nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero A_ij, B_ij, j=0...m-1, actual column numbers in rcsubs */
    for(j=0; j<nnz; ++j){
      if(rcsubs[j]<mcon) continue; // A_ij, B_ij are zero
 
      ptr2=err + idxij->val[rcidxs[j]]*mnp; // set ptr2 to point into err

      if(cnp){
        ptr1=fjac + idxij->val[rcidxs[j]]*ABsz; // set ptr1 to point to A_ij
        pab=pa + rcsubs[j]*cnp;
        for(jj=0; jj<cnp; ++jj){
          temp=FABS(pab[jj]);
          if(temp==zero) temp=one;

          for(ii=0; ii<mnp; ++ii)
            ptr2[ii]+=temp*ptr1[ii*cnp+jj];
        }
      }

      if(pnp){
        ptr1=fjac + idxij->val[rcidxs[j]]*ABsz + Asz; // set ptr1 to point to B_ij
        pab=pb + i*pnp;
        for(jj=0; jj<pnp; ++jj){
          temp=FABS(pab[jj]);
          if(temp==zero) temp=one;

          for(ii=0; ii<mnp; ++ii)
            ptr2[ii]+=temp*ptr1[ii*pnp+jj];
        }
      }
    }
  }

  for(i=0; i<nobs; ++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;
  }

  free(fjac);
  free(buf);

  for(i=0; i<n; ++i){
    nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero err_ij, j=0...m-1 */
    for(j=0; j<nnz; ++j){
      if(rcsubs[j]<mcon) continue; // corresponding gradients are taken to be zero

      ptr1=err + idxij->val[rcidxs[j]]*mnp; // set ptr1 to point into err
      for(ii=0; ii<mnp; ++ii)
        if(ptr1[ii]<=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",
                  idxij->val[rcidxs[j]]*mnp+ii, ii, i, rcsubs[j], (ptr1[ii]==0.0)? "wrong" : "probably wrong", ptr1[ii]);
          ++numerr;
        }
    }
  }
  if(numerr) fprintf(stderr, "SBA: found %d suspicious gradients out of %d\n\n", numerr, nobs);

  free(err);

  return;
}

void sba_mot_chkjac_x(
    void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
    void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
    double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int mnp, void *func_adata, void *jac_adata)
{
  sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, mcon, cnp, 0, mnp, func_adata, jac_adata);
}

void sba_str_chkjac_x(
    void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
    void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
    double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int pnp, int mnp, void *func_adata, void *jac_adata)
{
  sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, 0, 0, pnp, mnp, func_adata, jac_adata);
}

#if 0