int sba_motstr_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, 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 use_constraints, camera_constraints_t *constraints,
int use_point_constraints,
point_constraints_t *point_constraints,
double *Vout, double *Sout, double *Uout, double *Wout)
{
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, m, mcon, vmask, p, cnp, pnp, x, covx, mnp, sba_motstr_Qs, fjac, &wdata, itmax, verbose, opts, info, use_constraints, constraints, use_point_constraints, point_constraints, Vout, Sout, Uout, Wout);
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;
}
void BundleRTS::run(int nPtsCon, int nCamsCon, int maxIter) {
refineInput(_mapPts);
numPtsCon = nPtsCon;
numCamsCon = nCamsCon;
compressMeasurements();
const int cnp = 11; //5:intrinsic parameters 6:extrinsic parameters
const int pnp = 3;
const int mnp = 2;
m_globs.cnp = cnp;
m_globs.pnp = pnp;
m_globs.mnp = mnp;
if (m_globs.rot0params) {
delete[] m_globs.rot0params;
}
m_globs.rot0params = new double[FULLQUATSZ * numCams];
//set initial camera parameters
for (int i = 0; i < numCams; ++i) {
mat2quat(Rs + 9 * i, m_globs.rot0params + 4 * i);
}
m_globs.intrcalib = 0;
m_globs.nccalib = 5;
m_globs.camparams = 0;
m_globs.ptparams = 0;
/* call sparse LM routine */
double opts[SBA_OPTSSZ];
opts[0] = SBA_INIT_MU * 1E-4;
opts[1] = SBA_STOP_THRESH;
opts[2] = SBA_STOP_THRESH;
opts[3] = 0; //0.05 * numMeas; // uncomment to force termination if the average reprojection error drops below 0.05
opts[4] = 1E-16; // uncomment to force termination if the relative reduction in the RMS reprojection error drops below 1E-05
if (m_paramVec)
delete[] m_paramVec;
m_paramVec = new double[numCams * cnp + numPts * pnp];
double * pParamVec = m_paramVec;
double* pKs = Ks.data;
double* pTs = Ts.data;
for (int i = 0; i < numCams; ++i) {
pParamVec[0] = pKs[0];
pParamVec[1] = pKs[2];
pParamVec[2] = pKs[5];
pParamVec[3] = pKs[4] / pKs[0];
pParamVec[4] = pKs[1];
pParamVec[5] = 0;
pParamVec[6] = 0;
pParamVec[7] = 0;
pParamVec[8] = pTs[0];
pParamVec[9] = pTs[1];
pParamVec[10] = pTs[2];
pParamVec += cnp;
pKs += 9;
pTs += 3;
}
double* pParamPoints = m_paramVec + numCams * cnp;
memcpy(pParamPoints, Ms.data, numPts * 3 * sizeof(double));
double sbaInfo[SBA_INFOSZ];
if (sba_motstr_levmar_x(numPts, numPtsCon, numCams, numCamsCon, vmask,
m_paramVec, cnp, pnp, ms.data, 0, mnp, img_projsKRTS_x,
img_projsKRTS_jac_x, (void *) (&m_globs), maxIter, 0, opts,
sbaInfo) == SBA_ERROR) {
//for debug
//save the bundle data for debug
repErr("bundle adjustment failed!\n");
}
//test
logInfo(
"initial error:%lf, final error:%lf #iterations:%lf stop reason:%lf\n",
sqrt(sbaInfo[0] / numMeas), sqrt(sbaInfo[1] / numMeas), sbaInfo[5],
sbaInfo[6]);
}
