QVision: Qt's Image, Video and Computer Vision Library

00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 #include <stdio.h>
00022 #include <stdlib.h>
00023 #include <math.h>
00024 #include <float.h>
00025 
00026 #include "compiler.h"
00027 #include "sba.h"
00028 
00029 #define emalloc(sz)       emalloc_(__FILE__, __LINE__, sz)
00030 
00031 #define FABS(x)           (((x)>=0)? (x) : -(x))
00032 
00033 
00034 /* auxiliary memory allocation routine with error checking */
00035 inline static void *emalloc_(char *file, int line, size_t sz)
00036 {
00037 void *ptr;
00038 
00039   ptr=(void *)malloc(sz);
00040   if(ptr==NULL){
00041     fprintf(stderr, "SBA: memory allocation request for %u bytes failed in file %s, line %d, exiting", sz, file, line);
00042     exit(1);
00043   }
00044 
00045   return ptr;
00046 }
00047 
00048 /* 
00049  * Check the jacobian of a projection function in nvars variables
00050  * evaluated at a point p, for consistency with the function itself.
00051  * Expert version
00052  *
00053  * Based on fortran77 subroutine CHKDER by
00054  * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
00055  * Argonne National Laboratory. MINPACK project. March 1980.
00056  *
00057  *
00058  * func points to a function from R^{nvars} --> R^{nobs}: Given a p in R^{nvars}
00059  *      it yields hx in R^{nobs}
00060  * jacf points to a function implementing the jacobian of func, whose consistency with
00061  *     func is to be tested. Given a p in R^{nvars}, jacf computes into the nvis*(Asz+Bsz)
00062  *     matrix jac the jacobian of func at p. Note the jacobian is sparse, consisting of
00063  *     all A_ij, B_ij and that row i of jac corresponds to the gradient of the i-th
00064  *     component of func, evaluated at p.
00065  * p is an input array of length nvars containing the point of evaluation.
00066  * idxij, rcidxs, rcsubs, ncon, mcon, cnp, pnp, mnp are as usual. Note that if cnp=0 or
00067  *     pnp=0 a jacobian corresponding resp. to motion or camera parameters
00068  *     only is assumed.
00069  * func_adata, jac_adata point to possible additional data and are passed
00070  *     uninterpreted to func, jacf respectively.
00071  * err is an array of length nobs. On output, err contains measures
00072  *     of correctness of the respective gradients. if there is
00073  *     no severe loss of significance, then if err[i] is 1.0 the
00074  *     i-th gradient is correct, while if err[i] is 0.0 the i-th
00075  *     gradient is incorrect. For values of err between 0.0 and 1.0,
00076  *     the categorization is less certain. In general, a value of
00077  *     err[i] greater than 0.5 indicates that the i-th gradient is
00078  *     probably correct, while a value of err[i] less than 0.5
00079  *     indicates that the i-th gradient is probably incorrect.
00080  *
00081  * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
00082  * following excerpt comes from CHKDER's documentation:
00083  *
00084  *     "The function does not perform reliably if cancellation or
00085  *     rounding errors cause a severe loss of significance in the
00086  *     evaluation of a function. therefore, none of the components
00087  *     of p should be unusually small (in particular, zero) or any
00088  *     other value which may cause loss of significance."
00089  */
00090 
00091 void sba_motstr_chkjac_x(
00092     void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
00093     void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
00094     double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int mcon, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
00095 {
00096 const double factor=100.0, one=1.0, zero=0.0;
00097 double *fvec, *fjac, *pp, *fvecp, *buf, *err;
00098 
00099 int nvars, nobs, m, n, Asz, Bsz, ABsz, nnz;
00100 register int i, j, ii, jj;
00101 double eps, epsf, temp, epsmch, epslog;
00102 register double *ptr1, *ptr2, *pab;
00103 double *pa, *pb;
00104 int fvec_sz, pp_sz, fvecp_sz, numerr=0;
00105 
00106   nobs=idxij->nnz*mnp;
00107   n=idxij->nr; m=idxij->nc;
00108   nvars=m*cnp + n*pnp;
00109   epsmch=DBL_EPSILON;
00110   eps=sqrt(epsmch);
00111 
00112   Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
00113   fjac=(double *)emalloc(idxij->nnz*ABsz*sizeof(double));
00114 
00115   fvec_sz=fvecp_sz=nobs;
00116   pp_sz=nvars;
00117   buf=(double *)emalloc((fvec_sz + pp_sz + fvecp_sz)*sizeof(double));
00118   fvec=buf;
00119   pp=fvec+fvec_sz;
00120   fvecp=pp+pp_sz;
00121 
00122   err=(double *)emalloc(nobs*sizeof(double));
00123 
00124   /* compute fvec=func(p) */
00125   (*func)(p, idxij, rcidxs, rcsubs, fvec, func_adata);
00126 
00127   /* compute the jacobian at p */
00128   (*jacf)(p, idxij, rcidxs, rcsubs, fjac, jac_adata);
00129 
00130   /* compute pp */
00131   for(j=0; j<nvars; ++j){
00132     temp=eps*FABS(p[j]);
00133     if(temp==zero) temp=eps;
00134     pp[j]=p[j]+temp;
00135   }
00136 
00137   /* compute fvecp=func(pp) */
00138   (*func)(pp, idxij, rcidxs, rcsubs, fvecp, func_adata);
00139 
00140   epsf=factor*epsmch;
00141   epslog=log10(eps);
00142 
00143   for(i=0; i<nobs; ++i)
00144     err[i]=zero;
00145 
00146   pa=p;
00147   pb=p + m*cnp;
00148   for(i=0; i<n; ++i){
00149     nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero A_ij, B_ij, j=0...m-1, actual column numbers in rcsubs */
00150     for(j=0; j<nnz; ++j){
00151       ptr2=err + idxij->val[rcidxs[j]]*mnp; // set ptr2 to point into err
00152 
00153       if(cnp && rcsubs[j]>=mcon){ // A_ij is nonzero
00154         ptr1=fjac + idxij->val[rcidxs[j]]*ABsz; // set ptr1 to point to A_ij
00155         pab=pa + rcsubs[j]*cnp;
00156         for(jj=0; jj<cnp; ++jj){
00157           temp=FABS(pab[jj]);
00158           if(temp==zero) temp=one;
00159 
00160           for(ii=0; ii<mnp; ++ii)
00161             ptr2[ii]+=temp*ptr1[ii*cnp+jj];
00162         }
00163       }
00164 
00165       if(pnp && i>=ncon){ // B_ij is nonzero
00166         ptr1=fjac + idxij->val[rcidxs[j]]*ABsz + Asz; // set ptr1 to point to B_ij
00167         pab=pb + i*pnp;
00168         for(jj=0; jj<pnp; ++jj){
00169           temp=FABS(pab[jj]);
00170           if(temp==zero) temp=one;
00171 
00172           for(ii=0; ii<mnp; ++ii)
00173             ptr2[ii]+=temp*ptr1[ii*pnp+jj];
00174         }
00175       }
00176     }
00177   }
00178 
00179   for(i=0; i<nobs; ++i){
00180     temp=one;
00181     if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
00182         temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
00183     err[i]=one;
00184     if(temp>epsmch && temp<eps)
00185         err[i]=(log10(temp) - epslog)/epslog;
00186     if(temp>=eps) err[i]=zero;
00187   }
00188 
00189   free(fjac);
00190   free(buf);
00191 
00192   for(i=0; i<n; ++i){
00193     nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero err_ij, j=0...m-1 */
00194     for(j=0; j<nnz; ++j){
00195       if(i<ncon && rcsubs[j]<mcon) continue; // corresponding gradients are taken to be zero
00196 
00197       ptr1=err + idxij->val[rcidxs[j]]*mnp; // set ptr1 to point into err
00198       for(ii=0; ii<mnp; ++ii)
00199         if(ptr1[ii]<=0.5){
00200           fprintf(stderr, "SBA: gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
00201                   idxij->val[rcidxs[j]]*mnp+ii, ii, i, rcsubs[j], (ptr1[ii]==0.0)? "wrong" : "probably wrong", ptr1[ii]);
00202           ++numerr;
00203         }
00204     }
00205   }
00206   if(numerr) fprintf(stderr, "SBA: found %d suspicious gradients out of %d\n\n", numerr, nobs);
00207 
00208   free(err);
00209 
00210   return;
00211 }
00212 
00213 void sba_mot_chkjac_x(
00214     void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
00215     void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
00216     double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int mcon, int cnp, int mnp, void *func_adata, void *jac_adata)
00217 {
00218   sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, 0, mcon, cnp, 0, mnp, func_adata, jac_adata);
00219 }
00220 
00221 void sba_str_chkjac_x(
00222     void (*func)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata),
00223     void (*jacf)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata),
00224     double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, int ncon, int pnp, int mnp, void *func_adata, void *jac_adata)
00225 {
00226   sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, ncon, 0, 0, pnp, mnp, func_adata, jac_adata);
00227 }
00228 
00229 #if 0
00230 /* Routines for directly checking the jacobians supplied to the simple drivers.
00231  * They shouldn't be necessary since these jacobians can be verified indirectly
00232  * through the expert sba_XXX_chkjac_x() routines.
00233  */
00234 
00235 /*****************************************************************************************/
00236 // Sample code for using sba_motstr_chkjac():
00237 
00238   for(i=ncon; i<n; ++i)
00239     for(j=mcon; j<m; ++j){
00240       if(!vmask[i*m+j]) continue; // point i does not appear in image j
00241 
00242     sba_motstr_chkjac(proj, projac, p+j*cnp, p+m*cnp+i*pnp, j, i, cnp, pnp, mnp, adata, adata);
00243   }
00244 
00245 
00246 /*****************************************************************************************/
00247 
00248 
00249 /* union used for passing pointers to the user-supplied functions for the motstr/mot/str simple drivers */
00250 union proj_projac{
00251   struct{
00252     void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata);
00253     void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata);
00254   } motstr;
00255 
00256   struct{
00257     void (*proj)(int j, int i, double *aj, double *xij, void *adata);
00258     void (*projac)(int j, int i, double *aj, double *Aij, void *adata);
00259   } mot;
00260 
00261   struct{
00262     void (*proj)(int j, int i, double *bi, double *xij, void *adata);
00263     void (*projac)(int j, int i, double *bi, double *Bij, void *adata);
00264   } str;
00265 };
00266 
00267 
00268 /* 
00269  * Check the jacobian of a projection function in cnp+pnp variables
00270  * evaluated at a point p, for consistency with the function itself.
00271  * Simple version of the above, NOT to be called directly
00272  *
00273  * Based on fortran77 subroutine CHKDER by
00274  * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
00275  * Argonne National Laboratory. MINPACK project. March 1980.
00276  *
00277  *
00278  * proj points to a function from R^{cnp+pnp} --> R^{mnp}: Given a p=(aj, bi) in R^{cnp+pnp}
00279  *      it yields hx in R^{mnp}
00280  * projac points to a function implementing the jacobian of func, whose consistency with proj
00281  *     is to be tested. Given a p in R^{cnp+pnp}, jacf computes into the matrix jac=[Aij | Bij]
00282  *     jacobian of proj at p. Note that row i of jac corresponds to the gradient of the i-th
00283  *     component of proj, evaluated at p.
00284  * aj, bi are input arrays of lengths cnp, pnp containing the parameters for the point of
00285  *     evaluation, i.e. j-th camera and i-th point
00286  * jj, ii specify the point (ii) whose projection jacobian in image (jj) is being checked
00287  * cnp, pnp, mnp are as usual. Note that if cnp=0 or
00288  *     pnp=0 a jacobian corresponding resp. to motion or camera parameters
00289  *     only is assumed.
00290  * func_adata, jac_adata point to possible additional data and are passed
00291  *     uninterpreted to func, jacf respectively.
00292  * err is an array of length mnp. On output, err contains measures
00293  *     of correctness of the respective gradients. if there is
00294  *     no severe loss of significance, then if err[i] is 1.0 the
00295  *     i-th gradient is correct, while if err[i] is 0.0 the i-th
00296  *     gradient is incorrect. For values of err between 0.0 and 1.0,
00297  *     the categorization is less certain. In general, a value of
00298  *     err[i] greater than 0.5 indicates that the i-th gradient is
00299  *     probably correct, while a value of err[i] less than 0.5
00300  *     indicates that the i-th gradient is probably incorrect.
00301  *
00302  * CAUTION: THIS FUNCTION IS NOT 100% FOOLPROOF. The
00303  * following excerpt comes from CHKDER's documentation:
00304  *
00305  *     "The function does not perform reliably if cancellation or
00306  *     rounding errors cause a severe loss of significance in the
00307  *     evaluation of a function. therefore, none of the components
00308  *     of p should be unusually small (in particular, zero) or any
00309  *     other value which may cause loss of significance."
00310  */
00311 
00312 static void sba_chkjac(
00313     union proj_projac *funcs, double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
00314 {
00315 const double factor=100.0, one=1.0, zero=0.0;
00316 double *fvec, *fjac, *Aij, *Bij, *ajp, *bip, *fvecp, *buf, *err;
00317 
00318 int Asz, Bsz;
00319 register int i, j;
00320 double eps, epsf, temp, epsmch, epslog;
00321 int fvec_sz, ajp_sz, bip_sz, fvecp_sz, err_sz, numerr=0;
00322 
00323   epsmch=DBL_EPSILON;
00324   eps=sqrt(epsmch);
00325 
00326   Asz=mnp*cnp; Bsz=mnp*pnp;
00327   fjac=(double *)emalloc((Asz+Bsz)*sizeof(double));
00328   Aij=fjac;
00329   Bij=Aij+Asz;
00330 
00331   fvec_sz=fvecp_sz=mnp;
00332   ajp_sz=cnp; bip_sz=pnp;
00333   err_sz=mnp;
00334   buf=(double *)emalloc((fvec_sz + ajp_sz + bip_sz + fvecp_sz + err_sz)*sizeof(double));
00335   fvec=buf;
00336   ajp=fvec+fvec_sz;
00337   bip=ajp+ajp_sz;
00338   fvecp=bip+bip_sz;
00339   err=fvecp+fvecp_sz;
00340 
00341   /* compute fvec=proj(p), p=(aj, bi) & the jacobian at p */
00342   if(cnp && pnp){
00343     (*(funcs->motstr.proj))(jj, ii, aj, bi, fvec, func_adata);
00344     (*(funcs->motstr.projac))(jj, ii, aj, bi, Aij, Bij, jac_adata);
00345   }
00346   else if(cnp){
00347     (*(funcs->mot.proj))(jj, ii, aj, fvec, func_adata);
00348     (*(funcs->mot.projac))(jj, ii, aj, Aij, jac_adata);
00349   }
00350   else{
00351     (*(funcs->str.proj))(jj, ii, bi, fvec, func_adata);
00352     (*(funcs->str.projac))(jj, ii, bi, Bij, jac_adata);
00353   }
00354 
00355   /* compute pp, pp=(ajp, bip) */
00356   for(j=0; j<cnp; ++j){
00357     temp=eps*FABS(aj[j]);
00358     if(temp==zero) temp=eps;
00359     ajp[j]=aj[j]+temp;
00360   }
00361   for(j=0; j<pnp; ++j){
00362     temp=eps*FABS(bi[j]);
00363     if(temp==zero) temp=eps;
00364     bip[j]=bi[j]+temp;
00365   }
00366 
00367   /* compute fvecp=proj(pp) */
00368   if(cnp && pnp)
00369     (*(funcs->motstr.proj))(jj, ii, ajp, bip, fvecp, func_adata);
00370   else if(cnp)
00371     (*(funcs->mot.proj))(jj, ii, ajp, fvecp, func_adata);
00372   else
00373     (*(funcs->str.proj))(jj, ii, bip, fvecp, func_adata);
00374 
00375   epsf=factor*epsmch;
00376   epslog=log10(eps);
00377 
00378   for(i=0; i<mnp; ++i)
00379     err[i]=zero;
00380 
00381   for(j=0; j<cnp; ++j){
00382     temp=FABS(aj[j]);
00383     if(temp==zero) temp=one;
00384 
00385     for(i=0; i<mnp; ++i)
00386       err[i]+=temp*Aij[i*cnp+j];
00387   }
00388   for(j=0; j<pnp; ++j){
00389     temp=FABS(bi[j]);
00390     if(temp==zero) temp=one;
00391 
00392     for(i=0; i<mnp; ++i)
00393       err[i]+=temp*Bij[i*pnp+j];
00394   }
00395 
00396   for(i=0; i<mnp; ++i){
00397     temp=one;
00398     if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
00399         temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
00400     err[i]=one;
00401     if(temp>epsmch && temp<eps)
00402         err[i]=(log10(temp) - epslog)/epslog;
00403     if(temp>=eps) err[i]=zero;
00404   }
00405 
00406   for(i=0; i<mnp; ++i)
00407     if(err[i]<=0.5){
00408       fprintf(stderr, "SBA: gradient %d (corresponding to element %d of the projection of point %d on camera %d) is %s (err=%g)\n",
00409                 i, i, ii, jj, (err[i]==0.0)? "wrong" : "probably wrong", err[i]);
00410       ++numerr;
00411   }
00412   if(numerr) fprintf(stderr, "SBA: found %d suspicious gradients out of %d\n\n", numerr, mnp);
00413 
00414   free(fjac);
00415   free(buf);
00416 
00417   return;
00418 }
00419 
00420 void sba_motstr_chkjac(
00421     void (*proj)(int jj, int ii, double *aj, double *bi, double *xij, void *adata),
00422     void (*projac)(int jj, int ii, double *aj, double *bi, double *Aij, double *Bij, void *adata),
00423     double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
00424 {
00425 union proj_projac funcs;
00426 
00427   funcs.motstr.proj=proj;
00428   funcs.motstr.projac=projac;
00429 
00430   sba_chkjac(&funcs, aj, bi, jj, ii, cnp, pnp, mnp, func_adata, jac_adata);
00431 }
00432 
00433 void sba_mot_chkjac(
00434     void (*proj)(int jj, int ii, double *aj, double *xij, void *adata),
00435     void (*projac)(int jj, int ii, double *aj, double *Aij, void *adata),
00436     double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
00437 {
00438 union proj_projac funcs;
00439 
00440   funcs.mot.proj=proj;
00441   funcs.mot.projac=projac;
00442 
00443   sba_chkjac(&funcs, aj, NULL, jj, ii, cnp, 0, mnp, func_adata, jac_adata);
00444 }
00445 
00446 void sba_str_chkjac(
00447     void (*proj)(int jj, int ii, double *bi, double *xij, void *adata),
00448     void (*projac)(int jj, int ii, double *bi, double *Bij, void *adata),
00449     double *aj, double *bi, int jj, int ii, int cnp, int pnp, int mnp, void *func_adata, void *jac_adata)
00450 {
00451 union proj_projac funcs;
00452 
00453   funcs.str.proj=proj;
00454   funcs.str.projac=projac;
00455 
00456   sba_chkjac(&funcs, NULL, bi, jj, ii, 0, pnp, mnp, func_adata, jac_adata);
00457 }
00458 #endif /* 0 */
src/qvsfm/laSBA/sba-1.6/sba_chkjac.c
