From c9cf2446c5eb18a29119c7dfe90c5b62f6b374c5 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 23:14:29 -0600 Subject: Add sba poser --- redist/sba/sba_chkjac.c | 468 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 468 insertions(+) create mode 100644 redist/sba/sba_chkjac.c (limited to 'redist/sba/sba_chkjac.c') diff --git a/redist/sba/sba_chkjac.c b/redist/sba/sba_chkjac.c new file mode 100644 index 0000000..7cb53ec --- /dev/null +++ b/redist/sba/sba_chkjac.c @@ -0,0 +1,468 @@ +///////////////////////////////////////////////////////////////////////////////// +//// +//// 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 +#include +#include +#include + +#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 %zu 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, ncon, 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 ncon, 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) { + ptr2 = err + idxij->val[rcidxs[j]] * mnp; // set ptr2 to point into err + + if (cnp && rcsubs[j] >= mcon) { // A_ij is nonzero + 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 && i >= ncon) { // B_ij is nonzero + 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 (i < ncon && 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, 0, 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 ncon, int pnp, int mnp, void *func_adata, void *jac_adata) { + sba_motstr_chkjac_x(func, jacf, p, idxij, rcidxs, rcsubs, ncon, 0, 0, pnp, mnp, func_adata, jac_adata); +} + +#if 0 +/* 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=ncon; i 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; jmotstr.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=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]=zero; + } + + for(i=0; i