///////////////////////////////////////////////////////////////////////////////////// ////  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