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]); }