sba_levmar_wrap.c

sparse bundle ajustment的源码

 *
 * Returns the number of iterations (>=0) if successfull, SBA_ERROR if failed
 */

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

  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 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, 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;
}