main()
{
int mat[7][6]= {
{ 10, 0, 0, 0, -2, 0},
{ 3, 9, 0, 0, 0, 3},
{ 0, 7, 8, 7, 0, 0},
{ 3, 0, 8, 7, 5, 0},
{ 0, 8, 0, 9, 9, 13},
{ 0, 4, 0, 0, 2, -1},
{ 3, 7, 0, 9, 2, 0}
};
struct sba_crsm sm;
int idx1, idx2, k, pyramidLevel;
int vidxs[7], /* max(6, 7) */
jidxs[6], iidxs[7];
sba_crsm_build(&sm, mat[0], 7, 6);
sba_crsm_print(&sm, stdout);
for(idx1=0; idx1<7; ++idx1) {
for(idx2=0; idx2<6; ++idx2)
printf("%3d ", ((k=sba_crsm_elmidx(&sm, idx1, idx2))!=-1)? sm.val[k] : 0);
printf("\n");
}
for(idx1=0; idx1<7; ++idx1) {
k=sba_crsm_row_elmidxs(&sm, idx1, vidxs, jidxs);
printf("row %d\n", idx1);
for(pyramidLevel=0; pyramidLevel<k; ++pyramidLevel) {
idx2=jidxs[pyramidLevel];
printf("%d %d ", idx2, sm.val[vidxs[pyramidLevel]]);
}
printf("\n");
}
for(idx2=0; idx2<6; ++idx2) {
k=sba_crsm_col_elmidxs(&sm, idx2, vidxs, iidxs);
printf("col %d\n", idx2);
for(pyramidLevel=0; pyramidLevel<k; ++pyramidLevel) {
idx1=iidxs[pyramidLevel];
printf("%d %d ", idx1, sm.val[vidxs[pyramidLevel]]);
}
printf("\n");
}
sba_crsm_free(&sm);
}
/* Given a parameter vector p made up of the 3D coordinates of n points, compute in
* jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
* The jacobian is approximated with the aid of finite differences and is returned in the order
* (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ).
* Notice that depending on idxij, some of the B_ij might be missing
*
* Problem-specific information is assumed to be stored in a structure pointed to by "dat".
*
* NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,
* the jacobian should be computed analytically
*/
static void sba_str_Qs_fdjac(
double *p, /* I: current parameter estimate, (n*pnp)x1 */
struct sba_crsm *idxij, /* I: sparse matrix containing the location of x_ij in hx */
int *rcidxs, /* work array for the indexes of nonzero elements of a single sparse matrix row/column */
int *rcsubs, /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */
double *jac, /* O: array for storing the approximated jacobian */
void *dat) /* I: points to a "wrap_str_data_" structure */
{
register int i, j, ii, jj;
double *pbi;
register double *pB;
//int m;
int n, nnz, Bsz;
double tmp;
register double d, d1;
struct wrap_str_data_ *fdjd;
void (*proj)(int j, int i, double *bi, double *xij, void *adata);
double *hxij, *hxxij;
int pnp, mnp;
void *adata;
/* retrieve problem-specific information passed in *dat */
fdjd=(struct wrap_str_data_ *)dat;
proj=fdjd->proj;
pnp=fdjd->pnp; mnp=fdjd->mnp;
adata=fdjd->adata;
n=idxij->nr;
//m=idxij->nc;
Bsz=mnp*pnp;
/* allocate memory for hxij, hxxij */
if((hxij=malloc(2*mnp*sizeof(double)))==NULL){
fprintf(stderr, "memory allocation request failed in sba_str_Qs_fdjac()!\n");
exit(1);
}
hxxij=hxij+mnp;
/* compute B_ij */
for(i=0; i<n; ++i){
pbi=p+i*pnp; // i-th point parameters
nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */
for(jj=0; jj<pnp; ++jj){
/* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */
d=(double)(SBA_DELTA_SCALE)*pbi[jj]; // force evaluation
d=FABS(d);
if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;
d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */
for(j=0; j<nnz; ++j){
(*proj)(rcsubs[j], i, pbi, hxij, adata); // evaluate supplied function on current solution
tmp=pbi[jj];
pbi[jj]+=d;
(*proj)(rcsubs[j], i, pbi, hxxij, adata);
pbi[jj]=tmp; /* restore */
pB=jac + idxij->val[rcidxs[j]]*Bsz; // set pB to point to B_ij
for(ii=0; ii<mnp; ++ii)
pB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1;
}
}
}
free(hxij);
}
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;
}
