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_levmar_wrap.c | 917 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 917 insertions(+) create mode 100644 redist/sba/sba_levmar_wrap.c (limited to 'redist/sba/sba_levmar_wrap.c') 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 +#include +#include +#include +#include + +#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=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=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