readparams.c

sparse bundle ajustment的源码

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

      continue;
    }

    if(feof(fp)) break;

    ungetc(ch, fp);

    n=readNDoubles(fp, params, pnp); /* read in point parameters */
    if(n==EOF) break;
    if(n!=pnp){
      fprintf(stderr, "readPointParamsAndProjections(): error reading input file, line %d:\n"
                      "expecting %d parameters for 3D point, read %d\n", lineno, pnp, n);
      exit(1);
    }
    params+=pnp;

    n=readNInts(fp, &nframes, 1);  /* read in number of image projections */
    if(n!=1){
      fprintf(stderr, "readPointParamsAndProjections(): error reading input file, line %d:\n"
                      "expecting number of frames for 3D point\n", lineno);
      exit(1);
    }

    for(i=0; i<nframes; ++i){
      n=readNInts(fp, &frameno, 1); /* read in frame number... */
      if(frameno>=ncams){
        fprintf(stderr, "readPointParamsAndProjections(): line %d contains an image projection for frame %d "
                        "but only %d cameras have been specified!\n", lineno+1, frameno, ncams);
        exit(1);
      }

      n+=readNDoubles(fp, projs, mnp); /* ...and image projection */
      projs+=mnp;
      if(n!=mnp+1){
        fprintf(stderr, "readPointParamsAndProjections(): error reading image projections from line %d [n=%d].\n"
                        "Perhaps line contains fewer than %d projections?\n", lineno+1, n, nframes);
        exit(1);
      }

      if(covprojs!=NULL){
        if(havecov==TRICOV){
          ntord=tricovsz;
        }
        else{
          ntord=covsz;
        }
        n=readNDoubles(fp, covprojs, ntord); /* read in covariance values */
        if(n!=ntord){
          fprintf(stderr, "readPointParamsAndProjections(): error reading image projection covariances from line %d [n=%d].\n"
                          "Perhaps line contains fewer than %d projections?\n", lineno+1, n, nframes);
          exit(1);
        }
        if(havecov==TRICOV){
          /* complete the full matrix from the triangular part that was read.
           * First, prepare upper part: element (ii, mnp-1) is at position mnp-1 + ii*(2*mnp-ii-1)/2.
           * Row ii has mnp-ii elements that must be shifted by ii*(ii+1)/2
           * positions to the right to make space for the lower triangular part
           */
          for(ii=mnp; --ii; ){
            k=mnp-1 + ((ii*((mnp<<1)-ii-1))>>1); //mnp-1 + ii*(2*mnp-ii-1)/2
            nshift=(ii*(ii+1))>>1; //ii*(ii+1)/2;
            for(jj=0; jj<mnp-ii; ++jj){
              covprojs[k-jj+nshift]=covprojs[k-jj];
              //covprojs[k-jj]=0.0; // this clears the lower part
            }
          }
          /* copy to lower part */
          for(ii=mnp; ii--; )
            for(jj=ii; jj--; )
              covprojs[ii*mnp+jj]=covprojs[jj*mnp+ii];
        }
        covprojs+=covsz;
      }

      vmask[ptno*ncams+frameno]=1;
    }

    fscanf(fp, "\n"); // consume trailing newline

    lineno++;
    ptno++;
  }
}


/* combines the above routines to read the initial estimates of the motion + structure parameters from text files.
 * Also, it loads the projections of 3D points across images and optionally their covariances.
 * The routine dynamically allocates the required amount of memory (last 4 arguments).
 * If no covariances are supplied, *covimgpts is set to NULL
 */
void readInitialSBAEstimate(char *camsfname, char *ptsfname, int cnp, int pnp, int mnp,
                            void (*infilter)(double *pin, int nin, double *pout, int nout), int filecnp,
                            int *ncams, int *n3Dpts, int *n2Dprojs,
                            double **motstruct, double **imgpts, double **covimgpts, char **vmask)
{
FILE *fpc, *fpp;
int havecov;

  if((fpc=fopen(camsfname, "r"))==NULL){
    fprintf(stderr, "cannot open file %s, exiting\n", camsfname);
    exit(1);
  }

  if((fpp=fopen(ptsfname, "r"))==NULL){
    fprintf(stderr, "cannot open file %s, exiting\n", ptsfname);
    exit(1);
  }

  *ncams=findNcameras(fpc);
  readNpointsAndNprojections(fpp, n3Dpts, pnp, n2Dprojs, mnp, &havecov);

  *motstruct=(double *)malloc((*ncams*cnp + *n3Dpts*pnp)*sizeof(double));
  if(*motstruct==NULL){
    fprintf(stderr, "memory allocation for 'motstruct' failed in readInitialSBAEstimate()\n");
    exit(1);
  }
  *imgpts=(double *)malloc(*n2Dprojs*mnp*sizeof(double));
  if(*imgpts==NULL){
    fprintf(stderr, "memory allocation for 'imgpts' failed in readInitialSBAEstimate()\n");
    exit(1);
  }
  if(havecov){
    *covimgpts=(double *)malloc(*n2Dprojs*mnp*mnp*sizeof(double));
    if(*covimgpts==NULL){
      fprintf(stderr, "memory allocation for 'covimgpts' failed in readInitialSBAEstimate()\n");
      exit(1);
    }
  }
  else
    *covimgpts=NULL;
  *vmask=(char *)malloc(*n3Dpts * *ncams * sizeof(char));
  if(*vmask==NULL){
    fprintf(stderr, "memory allocation for 'vmask' failed in readInitialSBAEstimate()\n");
    exit(1);
  }
  memset(*vmask, 0, *n3Dpts * *ncams * sizeof(char)); /* clear vmask */


  /* prepare for re-reading files */
  rewind(fpc);
  rewind(fpp);

  readCameraParams(fpc, cnp, infilter, filecnp, *motstruct);
  readPointParamsAndProjections(fpp, *motstruct+*ncams*cnp, pnp, *imgpts, *covimgpts, havecov, mnp, *vmask, *ncams);

  fclose(fpc);
  fclose(fpp);
}

/* reads the 3x3 intrinsic calibration matrix contained in a file */
void readCalibParams(char *fname, double ical[9])
{
  FILE *fp;
  int i, ch=EOF;

  if((fp=fopen(fname, "r"))==NULL){
    fprintf(stderr, "cannot open file %s, exiting\n", fname);
    exit(1);
  }

  while(!feof(fp) && (ch=fgetc(fp))=='#') /* skip comments */
    SKIP_LINE(fp);

  if(feof(fp)){
    ical[0]=ical[1]=ical[2]=ical[3]=ical[4]=ical[5]=ical[6]=ical[7]=ical[8]=0.0;
    return;
  }

  ungetc(ch, fp);

  for(i=0; i<3; i++){
    fscanf(fp, "%lf%lf%lf\n", ical, ical+1, ical+2);
    ical+=3;
  }

  fclose(fp);
}

/* routines for printing the motion and structure parameters, plus the projections
 * of 3D points across images. Mainly for debugging purposes.
 *
 * outfilter points to a function that converts a motion parameters vector from 
 * the interrnal representation used by eucsbademo to a format suitable for display.
 * For instance, it can expand a quaternion to 4 elements from its vector part.
 */

/* motion parameters only */
void printSBAMotionData(double *motstruct, int ncams, int cnp,
                        void (*outfilter)(double *pin, int nin, double *pout, int nout), int outcnp)
{
register int i;

  printf("Motion parameters:\n");
  if(!outfilter || outcnp<=0){ // no output filtering
    for(i=0; i<ncams*cnp; ++i){
      printf("%lf ", motstruct[i]);
      if((i+1)%cnp==0) putchar('\n');
    }
  }
  else{
    register int j;
    double *filtered;

    if((filtered=(double *)malloc(outcnp*sizeof(double)))==NULL){;
      fprintf(stderr, "memory allocation failed in printSBAMotionData()\n");
      exit(1);
    }
    for(i=0; i<ncams*cnp; i+=cnp){
      (*outfilter)(motstruct+i, cnp, filtered, outcnp);
      for(j=0; j<outcnp; ++j)
        printf("%lf ", filtered[j]);
      putchar('\n');
    }
    free(filtered);
  }
}

/* structure parameters only */
void printSBAStructureData(double *motstruct, int ncams, int n3Dpts, int cnp, int pnp)
{
register int i;

  motstruct+=ncams*cnp;
  printf("\n\nStructure parameters:\n");
  for(i=0; i<n3Dpts*pnp; ++i){
    printf("%lf ", motstruct[i]);
    if((i+1)%pnp==0) putchar('\n');
  }
}

/* prints the estimates of the motion + structure parameters. It also prints the projections
 * of 3D points across images.
 */
void printSBAData(double *motstruct, int cnp, int pnp, int mnp, 
                  void (*outfilter)(double *pin, int nin, double *pout, int nout), int outcnp,
                  int ncams, int n3Dpts, double *imgpts, int n2Dprojs, char *vmask)
{
register int i, j, k, l;
int nframes;

  printSBAMotionData(motstruct, ncams, cnp, outfilter, outcnp);
  motstruct+=ncams*cnp;

  printf("\n\nStructure parameters and image projections:\n");
  for(i=k=0; i<n3Dpts; ++i){
    for(j=0; j<pnp; ++j)
      printf("%lf ", motstruct[i*pnp+j]);
    
    for(j=nframes=0; j<ncams; ++j)
      if(vmask[i*ncams+j]) ++nframes;
    printf("%d", nframes);
    for(j=0; j<ncams; ++j)
      if(vmask[i*ncams+j]){
        printf(" %d", j);
        for(l=0; l<mnp; ++l, ++k)
          printf(" %lf", imgpts[k]);
      }
    putchar('\n');
  }

#if 0
  /* alternative output format */
  printf("\n\nStructure parameters:\n");
  for(i=0; i<n3Dpts*pnp; ++i){
    printf("%lf ", motstruct[i]);
    if((i+1)%pnp==0) putchar('\n');
  }

  printf("\n\nImage projections:\n");
  for(i=0; i<n2Dprojs*mnp; ++i){
    printf("%lf ", imgpts[i]);
  }

  printf("\n\nVisibility mask\n");
  for(i=0; i<ncams*n3Dpts; ++i)
    printf("%d%s", (int)vmask[i], ((i+1)%ncams)? " " : "\n");
  putchar('\n');
#endif

  fflush(stdout);
}