readparams.c

sparse bundle ajustment的源码

/* Loading of camera, 3D point & image projection parameters from disk files */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "readparams.h"


#define MAXSTRLEN  2048 /* 2K */

/* get rid of the rest of a line upto \n or EOF */
#define SKIP_LINE(f){                                                       \
char buf[MAXSTRLEN];                                                        \
  while(!feof(f))                                                           \
    if(!fgets(buf, MAXSTRLEN-1, f) || buf[strlen(buf)-1]=='\n') break;      \
}

#define NOCOV     0
#define FULLCOV   1
#define TRICOV    2


/* reads (from "fp") "nvals" doubles into "vals".
 * Returns number of doubles actually read, EOF on end of file, EOF-1 on error
 */
static int readNDoubles(FILE *fp, double *vals, int nvals)
{
register int i;
int n, j;

  for(i=n=0; i<nvals; ++i){
    j=fscanf(fp, "%lf", vals+i);
    if(j==EOF) return EOF;

    if(j!=1 || ferror(fp)) return EOF-1;

    n+=j;
  }

  return n;
}

/* reads (from "fp") "nvals" doubles without storing them.
 * Returns EOF on end of file, EOF-1 on error
 */
static int skipNDoubles(FILE *fp, int nvals)
{
register int i;
int j;

  for(i=0; i<nvals; ++i){
    j=fscanf(fp, "%*f");
    if(j==EOF) return EOF;

    if(ferror(fp)) return EOF-1;
  }

  return nvals;
}

/* reads (from "fp") "nvals" ints into "vals".
 * Returns number of ints actually read, EOF on end of file, EOF-1 on error
 */
static int readNInts(FILE *fp, int *vals, int nvals)
{
register int i;
int n, j;

  for(i=n=0; i<nvals; ++i){
    j=fscanf(fp, "%d", vals+i);
    if(j==EOF) return EOF;
    
    if(j!=1 || ferror(fp)) return EOF-1;

    n+=j;
  }

  return n;
}

/* returns the number of cameras defined in a camera parameters file.
 * Each line of the file corresponds to the parameters of a single camera
 */
static int findNcameras(FILE *fp)
{
int lineno, ncams, ch;

  lineno=ncams=0;
  while(!feof(fp)){
    if((ch=fgetc(fp))=='#'){ /* skip comments */
      SKIP_LINE(fp);
      ++lineno;
      continue;
    }

    if(feof(fp)) break;

    ungetc(ch, fp);

    SKIP_LINE(fp);
    ++lineno;
    if(ferror(fp)){
      fprintf(stderr, "findNcameras(): error reading input file, line %d\n", lineno);
      exit(1);
    }
    ++ncams;
  }

  return ncams;
}


/* returns the number of doubles found in the first line of the supplied text file. rewinds file.
 */
static int countNDoubles(FILE *fp)
{
int lineno, ch, np, i;
char buf[MAXSTRLEN], *s;
double dummy;

  lineno=0;
  while(!feof(fp)){
    if((ch=fgetc(fp))=='#'){ /* skip comments */
      SKIP_LINE(fp);
      ++lineno;
      continue;
    }

    if(feof(fp)) return 0;
    
    ungetc(ch, fp);
    ++lineno;
    if(!fgets(buf, MAXSTRLEN-1, fp)){ /* read the line found... */
      fprintf(stderr, "countNDoubles(): error reading input file, line %d\n", lineno);
      exit(1);
    }
    /* ...and count the number of doubles it has */
    for(np=i=0, s=buf; 1 ; ++np, s+=i){
      ch=sscanf(s, "%lf%n", &dummy, &i);
      if(ch==0 || ch==EOF) break;
    }

    rewind(fp);
    return np;
  }

  return 0; // should not reach this point
}

/* reads into "params" the camera parameters defined in a camera parameters file.
 * "params" is assumed to point to sufficiently large memory.
 * Each line contains the parameters of a single camera, "cnp" parameters per camera
 *
 * infilter points to a function that converts a motion parameters vector stored
 * in a file to the format expected by eucsbademo. For instance, it can convert
 * a four element quaternion to the vector part of its unit norm equivalent.
 */
static void readCameraParams(FILE *fp, int cnp,
                             void (*infilter)(double *pin, int nin, double *pout, int nout), int filecnp,
                             double *params)
{
int lineno, n, ch;
double *tofilter;

  if(filecnp>0 && infilter){
    if((tofilter=(double *)malloc(filecnp*sizeof(double)))==NULL){;
      fprintf(stderr, "memory allocation failed in readCameraParams()\n");
      exit(1);
    }
  }
  else{ // camera params will be used as read
    infilter=NULL;
    tofilter=NULL;
    filecnp=cnp;
  }
  /* make sure that the parameters file contains the expected number of parameters per line */
  if((n=countNDoubles(fp))!=filecnp){
    fprintf(stderr, "readCameraParams(): expected %d camera parameters, first line contains %d!\n", filecnp, n);
    exit(1);
  }

  lineno=0;
  while(!feof(fp)){
    if((ch=fgetc(fp))=='#'){ /* skip comments */
      SKIP_LINE(fp);
      ++lineno;
      continue;
    }

    if(feof(fp)) break;

    ungetc(ch, fp);
    ++lineno;
    if(infilter){
      n=readNDoubles(fp, tofilter, filecnp);
      (*infilter)(tofilter, filecnp, params, cnp);
    }
    else
      n=readNDoubles(fp, params, cnp);
    if(n==EOF) break;
    if(n!=filecnp){
      fprintf(stderr, "readCameraParams(): line %d contains %d parameters, expected %d!\n", lineno, n, filecnp);
      exit(1);
    }
    if(ferror(fp)){
      fprintf(stderr, "findNcameras(): error reading input file, line %d\n", lineno);
      exit(1);
    }

    params+=cnp;
  }
  free(tofilter);
}


/* determines the number of 3D points contained in a points parameter file as well as the
 * total number of their 2D image projections across all images. Also decides if point 
 * covariances are being supplied. Each 3D point is assumed to be described by "pnp"
 * parameters and its parameters & image projections are stored as a single line.
 * The file format is
 * X_0...X_{pnp-1}  nframes  frame0 x0 y0 [cov0]  frame1 x1 y1 [cov1] ...
 * The portion of the line starting at "frame0" is ignored for all but the first line
 */
static void readNpointsAndNprojections(FILE *fp, int *n3Dpts, int pnp, int *nprojs, int mnp, int *havecov)
{
int nfirst, lineno, npts, nframes, ch, n;

  /* #parameters for the first line */
  nfirst=countNDoubles(fp);
  *havecov=NOCOV;
  
  *n3Dpts=*nprojs=lineno=npts=0;
  while(!feof(fp)){
    if((ch=fgetc(fp))=='#'){ /* skip comments */
      SKIP_LINE(fp);
      ++lineno;
      continue;
    }

    if(feof(fp)) break;

    ungetc(ch, fp);
    ++lineno;
    skipNDoubles(fp, pnp);
    n=readNInts(fp, &nframes, 1);
    if(n!=1){
      fprintf(stderr, "readNpointsAndNprojections(): error reading input file, line %d: "
                      "expecting number of frames for 3D point\n", lineno);
      exit(1);
    }
    if(npts==0){ /* check the parameters in the first line to determine if we have covariances */
      nfirst-=(pnp+1); /* ignore point parameters and number of frames */
      if(nfirst==nframes*(mnp+1 + mnp*mnp)){ /* full mnpxmnp covariance */
        *havecov=FULLCOV;
      }else if(nfirst==nframes*(mnp+1 + mnp*(mnp+1)/2)){ /* triangular part of mnpxmnp covariance */
        *havecov=TRICOV;
      }else{
        *havecov=NOCOV;
      }
    }
    SKIP_LINE(fp);
    *nprojs+=nframes;
    ++npts;
  }

  *n3Dpts=npts;
}


/* reads a points parameter file.
 * "params", "projs" & "vmask" are assumed preallocated, pointing to
 * memory blocks large enough to hold the parameters of 3D points, 
 * their projections in all images and the point visibility mask, respectively.
 * Also, if "covprojs" is non-NULL, it is assumed preallocated and pointing to
 * a memory block suitable to hold the covariances of image projections.
 * Each 3D point is assumed to be defined by pnp parameters and each of its projections
 * by mnp parameters. Optionally, the mnp*mnp covariance matrix in row-major order
 * follows each projection. All parameters are stored in a single line.
 *
 * File format is X_{0}...X_{pnp-1}  nframes  frame0 x0 y0 [covx0^2 covx0y0 covx0y0 covy0^2] frame1 x1 y1 [covx1^2 covx1y1 covx1y1 covy1^2] ...
 * with the parameters in angle brackets being optional. To save space, only the upper
 * triangular part of the covariance can be specified, i.e. [covx0^2 covx0y0 covy0^2], etc
 */
static void readPointParamsAndProjections(FILE *fp, double *params, int pnp, double *projs, double *covprojs,
                                          int havecov, int mnp, char *vmask, int ncams)
{
int nframes, ch, lineno, ptno, frameno, n;
int ntord, covsz=mnp*mnp, tricovsz=mnp*(mnp+1)/2, nshift;
register int i, ii, jj, k;