diff options
Diffstat (limited to 'redist/sba/sba_levmar_wrap.c')
| -rw-r--r-- | redist/sba/sba_levmar_wrap.c | 917 |
1 files changed, 917 insertions, 0 deletions
diff --git a/redist/sba/sba_levmar_wrap.c b/redist/sba/sba_levmar_wrap.c new file mode 100644 index 0000000..159a040 --- /dev/null +++ b/redist/sba/sba_levmar_wrap.c @@ -0,0 +1,917 @@ +///////////////////////////////////////////////////////////////////////////////// +//// +//// Simple drivers for sparse bundle adjustment based on the +//// Levenberg - Marquardt minimization algorithm +//// This file provides simple wrappers to the functions defined in sba_levmar.c +//// Copyright (C) 2004-2009 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 <float.h> +#include <math.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include "sba.h" + +#define FABS(x) (((x) >= 0) ? (x) : -(x)) + +struct wrap_motstr_data_ { + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata); // Q + void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata); // dQ/da, dQ/db + int cnp, pnp, mnp; /* parameter numbers */ + void *adata; +}; + +struct wrap_mot_data_ { + void (*proj)(int j, int i, double *aj, double *xij, void *adata); // Q + void (*projac)(int j, int i, double *aj, double *Aij, void *adata); // dQ/da + int cnp, mnp; /* parameter numbers */ + void *adata; +}; + +struct wrap_str_data_ { + void (*proj)(int j, int i, double *bi, double *xij, void *adata); // Q + void (*projac)(int j, int i, double *bi, double *Bij, void *adata); // dQ/db + int pnp, mnp; /* parameter numbers */ + void *adata; +}; + +/* Routines to estimate the estimated measurement vector (i.e. "func") and + * its sparse jacobian (i.e. "fjac") needed by BA expert drivers. Code below + * makes use of user-supplied functions computing "Q", "dQ/da", d"Q/db", + * i.e. predicted projection and associated jacobians for a SINGLE image measurement. + * Notice also that what follows is two pairs of "func" and corresponding "fjac" routines. + * The first is to be used in full (i.e. motion + structure) BA, the second in + * motion only BA. + */ + +/* FULL BUNDLE ADJUSTMENT */ + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in + * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements + * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted + * projection of the i-th point on the j-th camera. + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the hx_ij might be missing + * + */ +static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) { + register int i, j; + int cnp, pnp, mnp; + double *pa, *pb, *paj, *pbi, *pxij; + int n, m, nnz; + struct wrap_motstr_data_ *wdata; + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *proj_adata); + void *proj_adata; + + wdata = (struct wrap_motstr_data_ *)adata; + cnp = wdata->cnp; + pnp = wdata->pnp; + mnp = wdata->mnp; + proj = wdata->proj; + proj_adata = wdata->adata; + + n = idxij->nr; + m = idxij->nc; + pa = p; + pb = p + m * cnp; + + for (j = 0; j < m; ++j) { + /* j-th camera parameters */ + paj = pa + j * cnp; + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for (i = 0; i < nnz; ++i) { + pbi = pb + rcsubs[i] * pnp; + pxij = hx + idxij->val[rcidxs[i]] * mnp; // set pxij to point to hx_ij + + (*proj)(j, rcsubs[i], paj, pbi, pxij, proj_adata); // evaluate Q in pxij + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, 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 returned in the order (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm), + * where A_ij=dx_ij/db_j and B_ij=dx_ij/db_i (see HZ). + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the A_ij, B_ij might be missing + * + */ +static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) { + register int i, j; + int cnp, pnp, mnp; + double *pa, *pb, *paj, *pbi, *pAij, *pBij; + int n, m, nnz, Asz, Bsz, ABsz, idx; + struct wrap_motstr_data_ *wdata; + void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *projac_adata); + void *projac_adata; + + wdata = (struct wrap_motstr_data_ *)adata; + cnp = wdata->cnp; + pnp = wdata->pnp; + mnp = wdata->mnp; + projac = wdata->projac; + projac_adata = wdata->adata; + + n = idxij->nr; + m = idxij->nc; + pa = p; + pb = p + m * cnp; + Asz = mnp * cnp; + Bsz = mnp * pnp; + ABsz = Asz + Bsz; + + for (j = 0; j < m; ++j) { + /* j-th camera parameters */ + paj = pa + j * cnp; + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for (i = 0; i < nnz; ++i) { + pbi = pb + rcsubs[i] * pnp; + idx = idxij->val[rcidxs[i]]; + pAij = jac + idx * ABsz; // set pAij to point to A_ij + pBij = pAij + Asz; // set pBij to point to B_ij + + (*projac)(j, rcsubs[i], paj, pbi, pAij, pBij, projac_adata); // evaluate dQ/da, dQ/db in pAij, pBij + } + } +} + +/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, 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 + * (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm), + * where A_ij=dx_ij/da_j and B_ij=dx_ij/db_i (see HZ). + * Notice that depending on idxij, some of the A_ij, 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_motstr_Qs_fdjac( + double *p, /* I: current parameter estimate, (m*cnp+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_motstr_data_" structure */ +{ + register int i, j, ii, jj; + double *pa, *pb, *paj, *pbi; + register double *pAB; + int n, m, nnz, Asz, Bsz, ABsz; + + double tmp; + register double d, d1; + + struct wrap_motstr_data_ *fdjd; + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata); + double *hxij, *hxxij; + int cnp, pnp, mnp; + void *adata; + + /* retrieve problem-specific information passed in *dat */ + fdjd = (struct wrap_motstr_data_ *)dat; + proj = fdjd->proj; + cnp = fdjd->cnp; + pnp = fdjd->pnp; + mnp = fdjd->mnp; + adata = fdjd->adata; + + n = idxij->nr; + m = idxij->nc; + pa = p; + pb = p + m * cnp; + Asz = mnp * cnp; + Bsz = mnp * pnp; + ABsz = Asz + Bsz; + + /* allocate memory for hxij, hxxij */ + if ((hxij = malloc(2 * mnp * sizeof(double))) == NULL) { + fprintf(stderr, "memory allocation request failed in sba_motstr_Qs_fdjac()!\n"); + exit(1); + } + hxxij = hxij + mnp; + + /* compute A_ij */ + for (j = 0; j < m; ++j) { + paj = pa + j * cnp; // j-th camera parameters + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for (jj = 0; jj < cnp; ++jj) { + /* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */ + d = (double)(SBA_DELTA_SCALE)*paj[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 (i = 0; i < nnz; ++i) { + pbi = pb + rcsubs[i] * pnp; // i-th point parameters + (*proj)(j, rcsubs[i], paj, pbi, hxij, adata); // evaluate supplied function on current solution + + tmp = paj[jj]; + paj[jj] += d; + (*proj)(j, rcsubs[i], paj, pbi, hxxij, adata); + paj[jj] = tmp; /* restore */ + + pAB = jac + idxij->val[rcidxs[i]] * ABsz; // set pAB to point to A_ij + for (ii = 0; ii < mnp; ++ii) + pAB[ii * cnp + jj] = (hxxij[ii] - hxij[ii]) * d1; + } + } + } + + /* compute B_ij */ + for (i = 0; i < n; ++i) { + pbi = pb + 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) { + paj = pa + rcsubs[j] * cnp; // j-th camera parameters + (*proj)(rcsubs[j], i, paj, pbi, hxij, adata); // evaluate supplied function on current solution + + tmp = pbi[jj]; + pbi[jj] += d; + (*proj)(rcsubs[j], i, paj, pbi, hxxij, adata); + pbi[jj] = tmp; /* restore */ + + pAB = jac + idxij->val[rcidxs[j]] * ABsz + Asz; // set pAB to point to B_ij + for (ii = 0; ii < mnp; ++ii) + pAB[ii * pnp + jj] = (hxxij[ii] - hxij[ii]) * d1; + } + } + } + + free(hxij); +} + +/* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */ + +/* Given a parameter vector p made up of the parameters of m cameras, compute in + * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. + * The measurements are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, + * where hx_ij is the predicted projection of the i-th point on the j-th camera. + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the hx_ij might be missing + * + */ +static void sba_mot_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) { + register int i, j; + int cnp, mnp; + double *paj, *pxij; + // int n; + int m, nnz; + struct wrap_mot_data_ *wdata; + void (*proj)(int j, int i, double *aj, double *xij, void *proj_adata); + void *proj_adata; + + wdata = (struct wrap_mot_data_ *)adata; + cnp = wdata->cnp; + mnp = wdata->mnp; + proj = wdata->proj; + proj_adata = wdata->adata; + + // n=idxij->nr; + m = idxij->nc; + + for (j = 0; j < m; ++j) { + /* j-th camera parameters */ + paj = p + j * cnp; + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for (i = 0; i < nnz; ++i) { + pxij = hx + idxij->val[rcidxs[i]] * mnp; // set pxij to point to hx_ij + + (*proj)(j, rcsubs[i], paj, pxij, proj_adata); // evaluate Q in pxij + } + } +} + +/* Given a parameter vector p made up of the parameters of m cameras, 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 returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm), + * where A_ij=dx_ij/db_j (see HZ). + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the A_ij might be missing + * + */ +static void sba_mot_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) { + register int i, j; + int cnp, mnp; + double *paj, *pAij; + // int n; + int m, nnz, Asz, idx; + struct wrap_mot_data_ *wdata; + void (*projac)(int j, int i, double *aj, double *Aij, void *projac_adata); + void *projac_adata; + + wdata = (struct wrap_mot_data_ *)adata; + cnp = wdata->cnp; + mnp = wdata->mnp; + projac = wdata->projac; + projac_adata = wdata->adata; + + // n=idxij->nr; + m = idxij->nc; + Asz = mnp * cnp; + + for (j = 0; j < m; ++j) { + /* j-th camera parameters */ + paj = p + j * cnp; + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for (i = 0; i < nnz; ++i) { + idx = idxij->val[rcidxs[i]]; + pAij = jac + idx * Asz; // set pAij to point to A_ij + + (*projac)(j, rcsubs[i], paj, pAij, projac_adata); // evaluate dQ/da in pAij + } + } +} + +/* Given a parameter vector p made up of the parameters of m cameras, 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 + * (A_11, ..., A_1m, ..., A_n1, ..., A_nm), where A_ij=dx_ij/da_j (see HZ). + * Notice that depending on idxij, some of the A_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_mot_Qs_fdjac( + double *p, /* I: current parameter estimate, (m*cnp)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_mot_data_" structure */ +{ + register int i, j, ii, jj; + double *paj; + register double *pA; + // int n; + int m, nnz, Asz; + + double tmp; + register double d, d1; + + struct wrap_mot_data_ *fdjd; + void (*proj)(int j, int i, double *aj, double *xij, void *adata); + double *hxij, *hxxij; + int cnp, mnp; + void *adata; + + /* retrieve problem-specific information passed in *dat */ + fdjd = (struct wrap_mot_data_ *)dat; + proj = fdjd->proj; + cnp = fdjd->cnp; + mnp = fdjd->mnp; + adata = fdjd->adata; + + // n=idxij->nr; + m = idxij->nc; + Asz = mnp * cnp; + + /* allocate memory for hxij, hxxij */ + if ((hxij = malloc(2 * mnp * sizeof(double))) == NULL) { + fprintf(stderr, "memory allocation request failed in sba_mot_Qs_fdjac()!\n"); + exit(1); + } + hxxij = hxij + mnp; + + /* compute A_ij */ + for (j = 0; j < m; ++j) { + paj = p + j * cnp; // j-th camera parameters + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */ + for (jj = 0; jj < cnp; ++jj) { + /* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */ + d = (double)(SBA_DELTA_SCALE)*paj[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 (i = 0; i < nnz; ++i) { + (*proj)(j, rcsubs[i], paj, hxij, adata); // evaluate supplied function on current solution + + tmp = paj[jj]; + paj[jj] += d; + (*proj)(j, rcsubs[i], paj, hxxij, adata); + paj[jj] = tmp; /* restore */ + + pA = jac + idxij->val[rcidxs[i]] * Asz; // set pA to point to A_ij + for (ii = 0; ii < mnp; ++ii) + pA[ii * cnp + jj] = (hxxij[ii] - hxij[ii]) * d1; + } + } + } + + free(hxij); +} + +/* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY */ + +/* Given a parameter vector p made up of the 3D coordinates of n points, compute in + * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements + * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted + * projection of the i-th point on the j-th camera. + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the hx_ij might be missing + * + */ +static void sba_str_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata) { + register int i, j; + int pnp, mnp; + double *pbi, *pxij; + // int n; + int m, nnz; + struct wrap_str_data_ *wdata; + void (*proj)(int j, int i, double *bi, double *xij, void *proj_adata); + void *proj_adata; + + wdata = (struct wrap_str_data_ *)adata; + pnp = wdata->pnp; + mnp = wdata->mnp; + proj = wdata->proj; + proj_adata = wdata->adata; + + // n=idxij->nr; + m = idxij->nc; + + for (j = 0; j < m; ++j) { + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for (i = 0; i < nnz; ++i) { + pbi = p + rcsubs[i] * pnp; + pxij = hx + idxij->val[rcidxs[i]] * mnp; // set pxij to point to hx_ij + + (*proj)(j, rcsubs[i], pbi, pxij, proj_adata); // evaluate Q in pxij + } + } +} + +/* 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 returned in the order (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ). + * Caller supplies rcidxs and rcsubs which can be used as working memory. + * Notice that depending on idxij, some of the B_ij might be missing + * + */ +static void sba_str_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata) { + register int i, j; + int pnp, mnp; + double *pbi, *pBij; + // int n; + int m, nnz, Bsz, idx; + struct wrap_str_data_ *wdata; + void (*projac)(int j, int i, double *bi, double *Bij, void *projac_adata); + void *projac_adata; + + wdata = (struct wrap_str_data_ *)adata; + pnp = wdata->pnp; + mnp = wdata->mnp; + projac = wdata->projac; + projac_adata = wdata->adata; + + // n=idxij->nr; + m = idxij->nc; + Bsz = mnp * pnp; + + for (j = 0; j < m; ++j) { + + nnz = sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */ + + for (i = 0; i < nnz; ++i) { + pbi = p + rcsubs[i] * pnp; + idx = idxij->val[rcidxs[i]]; + pBij = jac + idx * Bsz; // set pBij to point to B_ij + + (*projac)(j, rcsubs[i], pbi, pBij, projac_adata); // evaluate dQ/db in pBij + } + } +} + +/* 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); +} + +/* + * Simple driver to sba_motstr_levmar_x for bundle adjustment on camera and structure parameters. + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_motstr_levmar( + const int n, /* number of points */ + const int ncon, /* number of points (starting from the 1st) whose parameters should not be modified. + * All B_ij (see below) with i<ncon are assumed to be zero + */ + const int m, /* number of images */ + const int mcon, /* number of images (starting from the 1st) whose parameters should not be modified. + * All A_ij (see below) with j<mcon are assumed to be zero + */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (a1, ..., am, b1, ..., bn). + * aj are the image j parameters, bi are the i-th point parameters, + * size m*cnp + n*pnp + */ + const int cnp, /* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */ + const int pnp, /* number of parameters for ONE point; e.g. 3 for Euclidean points */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp, /* number of parameters for EACH measurement; usually 2 */ + void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata), + /* functional relation computing a SINGLE image measurement. Assuming that + * the parameters of point i are bi and the parameters of camera j aj, + * computes a prediction of \hat{x}_{ij}. aj is cnp x 1, bi is pnp x 1 and + * xij is mnp x 1. This function is called only if point i is visible in + * image j (i.e. vmask[i, j]==1) + */ + void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata), + /* functional relation to evaluate d x_ij / d a_j and + * d x_ij / d b_i in Aij and Bij resp. + * This function is called only if point i is visible in * image j + * (i.e. vmask[i, j]==1). Also, A_ij and B_ij are mnp x cnp and mnp x pnp + * matrices resp. and they should be stored in row-major order. + * + * If NULL, the jacobians are approximated by repetitive proj calls + * and finite differences. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to proj, projac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate + return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial + * \mu, + * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - too many attempts to increase damping. Restart with increased mu + * 6 - stopped by small ||e||_2 + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ + ) { + int retval; + struct wrap_motstr_data_ wdata; + static void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata); + + wdata.proj = proj; + wdata.projac = projac; + wdata.cnp = cnp; + wdata.pnp = pnp; + wdata.mnp = mnp; + wdata.adata = adata; + + fjac = (projac) ? sba_motstr_Qs_jac : sba_motstr_Qs_fdjac; + retval = sba_motstr_levmar_x(n, ncon, m, mcon, vmask, p, cnp, pnp, x, covx, mnp, sba_motstr_Qs, fjac, &wdata, itmax, + verbose, opts, info); + + if (info) { + register int i; + int nvis; + + /* count visible image points */ + for (i = nvis = 0; i < n * m; ++i) + nvis += (vmask[i] != 0); + + /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */ + info[7] *= nvis; + info[8] *= nvis; + } + + return retval; +} + +/* + * Simple driver to sba_mot_levmar_x for bundle adjustment on camera parameters. + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_mot_levmar( + const int n, /* number of points */ + const int m, /* number of images */ + const int mcon, /* number of images (starting from the 1st) whose parameters should not be modified. + * All A_ij (see below) with j<mcon are assumed to be zero + */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (a1, ..., am). + * aj are the image j parameters, size m*cnp */ + const int cnp, /* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp, /* number of parameters for EACH measurement; usually 2 */ + void (*proj)(int j, int i, double *aj, double *xij, void *adata), + /* functional relation computing a SINGLE image measurement. Assuming that + * the parameters of camera j are aj, computes a prediction of \hat{x}_{ij} + * for point i. aj is cnp x 1 and xij is mnp x 1. + * This function is called only if point i is visible in image j (i.e. vmask[i, j]==1) + */ + void (*projac)(int j, int i, double *aj, double *Aij, void *adata), + /* functional relation to evaluate d x_ij / d a_j in Aij + * This function is called only if point i is visible in image j + * (i.e. vmask[i, j]==1). Also, A_ij are a mnp x cnp matrices + * and should be stored in row-major order. + * + * If NULL, the jacobian is approximated by repetitive proj calls + * and finite differences. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to proj, projac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate + return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon]. Respectively the scale factor for initial + * \mu, + * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - too many attempts to increase damping. Restart with increased mu + * 6 - stopped by small ||e||_2 + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ + ) { + int retval; + struct wrap_mot_data_ wdata; + void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata); + + wdata.proj = proj; + wdata.projac = projac; + wdata.cnp = cnp; + wdata.mnp = mnp; + wdata.adata = adata; + + fjac = (projac) ? sba_mot_Qs_jac : sba_mot_Qs_fdjac; + retval = + sba_mot_levmar_x(n, m, mcon, vmask, p, cnp, x, covx, mnp, sba_mot_Qs, fjac, &wdata, itmax, verbose, opts, info); + + if (info) { + register int i; + int nvis; + + /* count visible image points */ + for (i = nvis = 0; i < n * m; ++i) + nvis += (vmask[i] != 0); + + /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */ + info[7] *= nvis; + info[8] *= nvis; + } + + return retval; +} + +/* + * Simple driver to sba_str_levmar_x for bundle adjustment on structure parameters. + * + * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed + */ + +int sba_str_levmar( + const int n, /* number of points */ + const int ncon, /* number of points (starting from the 1st) whose parameters should not be modified. + * All B_ij (see below) with i<ncon are assumed to be zero + */ + const int m, /* number of images */ + char *vmask, /* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */ + double *p, /* initial parameter vector p0: (b1, ..., bn). + * bi are the i-th point parameters, size n*pnp + */ + const int pnp, /* number of parameters for ONE point; e.g. 3 for Euclidean points */ + double *x, /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where + * x_ij is the projection of the i-th point on the j-th image. + * NOTE: some of the x_ij might be missing, if point i is not visible in image j; + * see vmask[i, j], max. size n*m*mnp + */ + double *covx, /* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm), + * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no + * covariance estimates are available (identity matrices are implicitly used in this case). + * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing; + * see vmask[i, j], max. size n*m*mnp*mnp + */ + const int mnp, /* number of parameters for EACH measurement; usually 2 */ + void (*proj)(int j, int i, double *bi, double *xij, void *adata), + /* functional relation computing a SINGLE image measurement. Assuming that + * the parameters of point i are bi, computes a prediction of \hat{x}_{ij}. + * bi is pnp x 1 and xij is mnp x 1. This function is called only if point + * i is visible in image j (i.e. vmask[i, j]==1) + */ + void (*projac)(int j, int i, double *bi, double *Bij, void *adata), + /* functional relation to evaluate d x_ij / d b_i in Bij. + * This function is called only if point i is visible in image j + * (i.e. vmask[i, j]==1). Also, B_ij are mnp x pnp matrices + * and they should be stored in row-major order. + * + * If NULL, the jacobians are approximated by repetitive proj calls + * and finite differences. + */ + void *adata, /* pointer to possibly additional data, passed uninterpreted to proj, projac */ + + const int itmax, /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate + return */ + const int verbose, /* I: verbosity */ + const double opts[SBA_OPTSSZ], + /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial + * \mu, + * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2 + */ + double info[SBA_INFOSZ] + /* O: information regarding the minimization. Set to NULL if don't care + * info[0]=||e||_2 at initial p. + * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p. + * info[5]= # iterations, + * info[6]=reason for terminating: 1 - stopped by small gradient J^T e + * 2 - stopped by small dp + * 3 - stopped by itmax + * 4 - stopped by small relative reduction in ||e||_2 + * 5 - too many attempts to increase damping. Restart with increased mu + * 6 - stopped by small ||e||_2 + * 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error + * info[7]= # function evaluations + * info[8]= # jacobian evaluations + * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error + */ + ) { + int retval; + struct wrap_str_data_ wdata; + static void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata); + + wdata.proj = proj; + wdata.projac = projac; + wdata.pnp = pnp; + wdata.mnp = mnp; + wdata.adata = adata; + + fjac = (projac) ? sba_str_Qs_jac : sba_str_Qs_fdjac; + retval = + sba_str_levmar_x(n, ncon, m, vmask, p, pnp, x, covx, mnp, sba_str_Qs, fjac, &wdata, itmax, verbose, opts, info); + + if (info) { + register int i; + int nvis; + + /* count visible image points */ + for (i = nvis = 0; i < n * m; ++i) + nvis += (vmask[i] != 0); + + /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */ + info[7] *= nvis; + info[8] *= nvis; + } + + return retval; +} |