/* Euclidean bundle adjustment demo using the sba package */#include <stdio.h>#include <stdlib.h>#include <string.h>#include <math.h>#include <time.h>#include <sba.h>#include "eucsbademo.h"#include "readparams.h"#define CLOCKS_PER_MSEC (CLOCKS_PER_SEC/1000.0)#define MAXITER         100/* pointers to additional data, used for computed image projections and their jacobians */struct globs_{	double *intrcalib; /* the 5 intrinsic calibration parameters in the order [fu, u0, v0, ar, skew],                      * where ar is the aspect ratio fv/fu.                      * Used only when calibration is fixed for all cameras;                      * otherwise, it is null and the intrinsic parameters are                      * included in the set of motion parameters for each camera                      */  int nccalib; /* number of calibration parameters that must be kept constant.                * 0: all paremeters are free                 * 1: skew is fixed to its initial value, all other parameters vary (i.e. fu, u0, v0, ar)                 * 2: skew and aspect ratio are fixed to their initial values, all other parameters vary (i.e. fu, u0, v0)                * 3: meaningless                * 4: skew, aspect ratio and principal point are fixed to their initial values, only the focal length varies (i.e. fu)                * >=5: meaningless                * Used only when calibration varies among cameras                */  int cnp, pnp, mnp; /* dimensions */	double *ptparams; /* needed only when bundle adjusting for camera parameters only */	double *camparams; /* needed only when bundle adjusting for structure parameters only */} globs;/* Routines to estimate the estimated measurement vector (i.e. "func") and * its sparse jacobian (i.e. "fjac") needed in BA. Code below makes use of the * routines calcImgProj() and calcImgProjJacXXX() which * compute the predicted projection & jacobian of a SINGLE 3D point (see imgproj.c). * In the terminology of TR-340, these routines compute Q and its jacobians A=dQ/da, B=dQ/db. * 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. *//****************************************************************************************//* MEASUREMENT VECTOR AND JACOBIAN COMPUTATION FOR VARYING CAMERA POSE AND 3D STRUCTURE *//****************************************************************************************//*** MEASUREMENT VECTOR AND JACOBIAN COMPUTATION FOR THE SIMPLE DRIVERS ***//* FULL BUNDLE ADJUSTMENT, I.E. SIMULTANEOUS ESTIMATION OF CAMERA AND STRUCTURE PARAMETERS *//* Given the parameter vectors aj and bi of camera j and point i, computes in xij the * predicted projection of point i on image j */static void img_projRTS(int j, int i, double *aj, double *bi, double *xij, void *adata){  double *Kparms;  struct globs_ *gl;  gl=(struct globs_ *)adata;  Kparms=gl->intrcalib;  calcImgProj(Kparms, aj, aj+3, bi, xij); // 3 is the quaternion's vector part length}/* Given the parameter vectors aj and bi of camera j and point i, computes in Aij, Bij the * jacobian of the predicted projection of point i on image j */static void img_projRTS_jac(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata){  double *Kparms;  struct globs_ *gl;    gl=(struct globs_ *)adata;  Kparms=gl->intrcalib;  calcImgProjJacRTS(Kparms, aj, aj+3, bi, (double (*)[6])Aij, (double (*)[3])Bij); // 3 is the quaternion's vector part length}/* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY *//* Given the parameter vector aj of camera j, computes in xij the * predicted projection of point i on image j */static void img_projRT(int j, int i, double *aj, double *xij, void *adata){  int pnp;  double *Kparms, *ptparams;  struct globs_ *gl;  gl=(struct globs_ *)adata;  pnp=gl->pnp;  Kparms=gl->intrcalib;  ptparams=gl->ptparams;  calcImgProj(Kparms, aj, aj+3, ptparams+i*pnp, xij); // 3 is the quaternion's vector part length}/* Given the parameter vector aj of camera j, computes in Aij * the jacobian of the predicted projection of point i on image j */static void img_projRT_jac(int j, int i, double *aj, double *Aij, void *adata){  int pnp;  double *Kparms, *ptparams;  struct globs_ *gl;    gl=(struct globs_ *)adata;  pnp=gl->pnp;  Kparms=gl->intrcalib;  ptparams=gl->ptparams;  calcImgProjJacRT(Kparms, aj, aj+3, ptparams+i*pnp, (double (*)[6])Aij); // 3 is the quaternion's vector part length}/* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY *//* Given the parameter vector bi of point i, computes in xij the * predicted projection of point i on image j */static void img_projS(int j, int i, double *bi, double *xij, void *adata){  int cnp;  double *Kparms, *camparams, *aj;  struct globs_ *gl;  gl=(struct globs_ *)adata;  cnp=gl->cnp;  Kparms=gl->intrcalib;  camparams=gl->camparams;  aj=camparams+j*cnp;  calcImgProj(Kparms, aj, aj+3, bi, xij); // 3 is the quaternion's vector part length}/* Given the parameter vector bi of point i, computes in Bij * the jacobian of the predicted projection of point i on image j */static void img_projS_jac(int j, int i, double *bi, double *Bij, void *adata){  int cnp;  double *Kparms, *camparams, *aj;  struct globs_ *gl;    gl=(struct globs_ *)adata;  cnp=gl->cnp;  Kparms=gl->intrcalib;  camparams=gl->camparams;  aj=camparams+j*cnp;  calcImgProjJacS(Kparms, aj, aj+3, bi, (double (*)[3])Bij); // 3 is the quaternion's vector part length}/*** MEASUREMENT VECTOR AND JACOBIAN COMPUTATION FOR THE EXPERT DRIVERS ***//* FULL BUNDLE ADJUSTMENT, I.E. SIMULTANEOUS ESTIMATION OF CAMERA AND STRUCTURE PARAMETERS *//* 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. * Notice that depending on idxij, some of the hx_ij might be missing * */static void img_projsRTS_x(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, *pqr, *pt, *ppt, *pmeas, *Kparms;  //int n;  int m, nnz;  struct globs_ *gl;  gl=(struct globs_ *)adata;  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;  Kparms=gl->intrcalib;  //n=idxij->nr;  m=idxij->nc;  pa=p; pb=p+m*cnp;  for(j=0; j<m; ++j){    /* j-th camera parameters */    pqr=pa+j*cnp;    pt=pqr+3; // quaternion vector part has 3 elements    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */    for(i=0; i<nnz; ++i){      ppt=pb + rcsubs[i]*pnp;      pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij      calcImgProj(Kparms, pqr, pt, ppt, pmeas); // evaluate Q in pmeas    }  }}/* 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, ..., A_1m, ..., A_n1, ..., A_nm, B_11, ..., B_1m, ..., B_n1, ..., B_nm), * where A_ij=dx_ij/db_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 * */static void img_projsRTS_jac_x(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, *pqr, *pt, *ppt, *pA, *pB, *Kparms;  //int n;  int m, nnz, Asz, Bsz, ABsz, idx;  struct globs_ *gl;    gl=(struct globs_ *)adata;  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;  Kparms=gl->intrcalib;  //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 */    pqr=pa+j*cnp;    pt=pqr+3; // quaternion vector part has 3 elements    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */    for(i=0; i<nnz; ++i){      ppt=pb + rcsubs[i]*pnp;      idx=idxij->val[rcidxs[i]];      pA=jac + idx*ABsz; // set pA to point to A_ij      pB=pA  + Asz; // set pB to point to B_ij      calcImgProjJacRTS(Kparms, pqr, pt, ppt, (double (*)[6])pA, (double (*)[3])pB); // evaluate dQ/da, dQ/db in pA, pB    }  }}/* 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. * Notice that depending on idxij, some of the hx_ij might be missing * */static void img_projsRT_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata){  register int i, j;  int cnp, pnp, mnp;  double *pqr, *pt, *ppt, *pmeas, *Kparms, *ptparams;  //int n;  int m, nnz;  struct globs_ *gl;  gl=(struct globs_ *)adata;  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;  Kparms=gl->intrcalib;  ptparams=gl->ptparams;  //n=idxij->nr;  m=idxij->nc;  for(j=0; j<m; ++j){    /* j-th camera parameters */    pqr=p+j*cnp;    pt=pqr+3; // quaternion vector part has 3 elements    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */    for(i=0; i<nnz; ++i){	    ppt=ptparams + rcsubs[i]*pnp;      pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij      calcImgProj(Kparms, pqr, pt, ppt, pmeas); // evaluate Q in pmeas    }  }}/* 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). * Notice that depending on idxij, some of the A_ij might be missing * */static void img_projsRT_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata){  register int i, j;  int cnp, pnp, mnp;  double *pqr, *pt, *ppt, *pA, *Kparms, *ptparams;  //int n;  int m, nnz, Asz, idx;  struct globs_ *gl;    gl=(struct globs_ *)adata;  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;  Kparms=gl->intrcalib;  ptparams=gl->ptparams;  //n=idxij->nr;  m=idxij->nc;  Asz=mnp*cnp;  for(j=0; j<m; ++j){    /* j-th camera parameters */    pqr=p+j*cnp;    pt=pqr+3; // quaternion vector part has 3 elements    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */    for(i=0; i<nnz; ++i){      ppt=ptparams + rcsubs[i]*pnp;      idx=idxij->val[rcidxs[i]];      pA=jac + idx*Asz; // set pA to point to A_ij      calcImgProjJacRT(Kparms, pqr, pt, ppt, (double (*)[6])pA); // evaluate dQ/da in pA    }  }}/* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY *//* 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. * Notice that depending on idxij, some of the hx_ij might be missing * */static void img_projsS_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata){  register int i, j;  int cnp, pnp, mnp;  double *pqr, *pt, *ppt, *pmeas, *Kparms, *camparams;  //int n;  int m, nnz;  struct globs_ *gl;  gl=(struct globs_ *)adata;  cnp=gl->cnp; pnp=gl->pnp; mnp=gl->mnp;  Kparms=gl->intrcalib;  camparams=gl->camparams;  //n=idxij->nr;  m=idxij->nc;  for(j=0; j<m; ++j){    /* j-th camera parameters */    pqr=camparams+j*cnp;    pt=pqr+3; // quaternion vector part has 3 elements    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */    for(i=0; i<nnz; ++i){      ppt=p + rcsubs[i]*pnp;      pmeas=hx + idxij->val[rcidxs[i]]*mnp; // set pmeas to point to hx_ij      calcImgProj(Kparms, pqr, pt, ppt, pmeas); // evaluate Q in pmeas    }  }}/* 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). * Notice that depending on idxij, some of the B_ij might be missing *