[37] | 1 | /* Loading of camera, 3D point & image projection parameters from disk files */ |
---|
| 2 | |
---|
| 3 | |
---|
| 4 | #include <stdio.h> |
---|
| 5 | #include <stdlib.h> |
---|
| 6 | #include <string.h> |
---|
| 7 | |
---|
| 8 | #define MAXSTRLEN 4096 /* 4K */ |
---|
| 9 | |
---|
| 10 | static char buf[MAXSTRLEN]; |
---|
| 11 | /* get rid of the rest of a line upto \n or EOF */ |
---|
| 12 | #define SKIP_LINE(f){ \ |
---|
| 13 | while(!feof(f)) \ |
---|
| 14 | if(!fgets(buf, MAXSTRLEN-1, f) || buf[strlen(buf)-1]=='\n') break; \ |
---|
| 15 | } |
---|
| 16 | |
---|
| 17 | #include "eucsbademo.h" |
---|
| 18 | |
---|
| 19 | /* returns the number of cameras defined in a camera parameters file. |
---|
| 20 | * Each line of the file corresponds to the parameters of a single camera |
---|
| 21 | */ |
---|
| 22 | static int readNcameras(FILE *fp) |
---|
| 23 | { |
---|
| 24 | int lineno, ncams, ch; |
---|
| 25 | |
---|
| 26 | lineno=ncams=0; |
---|
| 27 | while(!feof(fp)){ |
---|
| 28 | if((ch=fgetc(fp))=='#'){ /* skip comments */ |
---|
| 29 | SKIP_LINE(fp); |
---|
| 30 | ++lineno; |
---|
| 31 | continue; |
---|
| 32 | } |
---|
| 33 | |
---|
| 34 | if(feof(fp)) break; |
---|
| 35 | |
---|
| 36 | SKIP_LINE(fp); |
---|
| 37 | ++lineno; |
---|
| 38 | if(ferror(fp)){ |
---|
| 39 | fprintf(stderr, "readNcameras(): error reading input file, line %d\n", lineno); |
---|
| 40 | exit(1); |
---|
| 41 | } |
---|
| 42 | ++ncams; |
---|
| 43 | } |
---|
| 44 | |
---|
| 45 | return ncams; |
---|
| 46 | } |
---|
| 47 | |
---|
| 48 | /* reads into "params" the camera parameters defined in a camera parameters file. |
---|
| 49 | * "params" is assumed to point to sufficiently large memory. |
---|
| 50 | * Each line contains the parameters of a single camera, 7 parameters per camera are |
---|
| 51 | * assumed |
---|
| 52 | */ |
---|
| 53 | static void readCameraParams(FILE *fp, double *params) |
---|
| 54 | { |
---|
| 55 | const int cnp=7; |
---|
| 56 | int lineno, n, ch; |
---|
| 57 | |
---|
| 58 | lineno=0; |
---|
| 59 | while(!feof(fp)){ |
---|
| 60 | if((ch=fgetc(fp))=='#'){ /* skip comments */ |
---|
| 61 | SKIP_LINE(fp); |
---|
| 62 | ++lineno; |
---|
| 63 | continue; |
---|
| 64 | } |
---|
| 65 | |
---|
| 66 | if(feof(fp)) break; |
---|
| 67 | |
---|
| 68 | ungetc(ch, fp); |
---|
| 69 | ++lineno; |
---|
| 70 | n=fscanf(fp, "%lf%lf%lf%lf%lf%lf%lf\n", params, params+1, params+2, params+3, params+4, params+5, params+6); |
---|
| 71 | if(n!=cnp){ |
---|
| 72 | fprintf(stderr, "readCameraParams(): line %d contains %d parameters, expected %d!\n", lineno, n, cnp); |
---|
| 73 | exit(1); |
---|
| 74 | } |
---|
| 75 | if(ferror(fp)){ |
---|
| 76 | fprintf(stderr, "readNcameras(): error reading input file, line %d\n", lineno); |
---|
| 77 | exit(1); |
---|
| 78 | } |
---|
| 79 | |
---|
| 80 | params+=cnp; |
---|
| 81 | } |
---|
| 82 | } |
---|
| 83 | |
---|
| 84 | |
---|
| 85 | /* determines the number of 3D points contained in a points parameter file as well as the |
---|
| 86 | * total number of their 2D image projections across all images. The file format is |
---|
| 87 | * X Y Z nframes frame0 x0 y0 frame1 x1 y1 ... |
---|
| 88 | */ |
---|
| 89 | static void readNpointsAndNprojections(FILE *fp, int *n3Dpts, int *nprojs) |
---|
| 90 | { |
---|
| 91 | int lineno, npts, nframes, ch; |
---|
| 92 | |
---|
| 93 | *n3Dpts=*nprojs=lineno=npts=0; |
---|
| 94 | while(!feof(fp)){ |
---|
| 95 | if((ch=fgetc(fp))=='#'){ /* skip comments */ |
---|
| 96 | SKIP_LINE(fp); |
---|
| 97 | ++lineno; |
---|
| 98 | continue; |
---|
| 99 | } |
---|
| 100 | |
---|
| 101 | if(feof(fp)) break; |
---|
| 102 | |
---|
| 103 | ungetc(ch, fp); |
---|
| 104 | ++lineno; |
---|
| 105 | fscanf(fp, "%*g%*g%*g%d", &nframes); |
---|
| 106 | if(ferror(fp)){ |
---|
| 107 | fprintf(stderr, "readNpointsAndNprojections(): error reading input file, line %d\n", lineno); |
---|
| 108 | exit(1); |
---|
| 109 | } |
---|
| 110 | SKIP_LINE(fp); |
---|
| 111 | *nprojs+=nframes; |
---|
| 112 | ++npts; |
---|
| 113 | } |
---|
| 114 | |
---|
| 115 | *n3Dpts=npts; |
---|
| 116 | } |
---|
| 117 | |
---|
| 118 | |
---|
| 119 | /* reads a points parameter file. |
---|
| 120 | * "params", "projs" & "vmask" are assumed preallocated, pointing to |
---|
| 121 | * memory blocks large enough to hold the parameters of 3D points, |
---|
| 122 | * their projections in all images and the point visibility mask, respectively. |
---|
| 123 | * File format is X Y Z nframes frame0 x0 y0 frame1 x1 y1 ... |
---|
| 124 | */ |
---|
| 125 | static void readPointParamsAndProjections(FILE *fp, double *params, double *projs, char *vmask, int ncams) |
---|
| 126 | { |
---|
| 127 | int nframes, ch, lineno, ptno, frameno, n; |
---|
| 128 | register int i; |
---|
| 129 | |
---|
| 130 | lineno=ptno=0; |
---|
| 131 | while(!feof(fp)){ |
---|
| 132 | if((ch=fgetc(fp))=='#'){ /* skip comments */ |
---|
| 133 | SKIP_LINE(fp); |
---|
| 134 | lineno++; |
---|
| 135 | |
---|
| 136 | continue; |
---|
| 137 | } |
---|
| 138 | |
---|
| 139 | if(feof(fp)) break; |
---|
| 140 | |
---|
| 141 | ungetc(ch, fp); |
---|
| 142 | |
---|
| 143 | fscanf(fp, "%lf%lf%lf", params, params+1, params+2); /* read in X Y Z */ |
---|
| 144 | params+=3; |
---|
| 145 | |
---|
| 146 | fscanf(fp, "%d", &nframes); /* read in number of image projections */ |
---|
| 147 | |
---|
| 148 | for(i=0; i<nframes; ++i){ |
---|
| 149 | n=fscanf(fp, "%d%lf%lf", &frameno, projs, projs+1); /* read in image projection */ |
---|
| 150 | if(n!=3){ |
---|
| 151 | fprintf(stderr, "readPointParamsAndProjections(): error reading image projections from line %d [n=%d].\n" |
---|
| 152 | "Line contains fewer than %d projections?\n", lineno+1, n, nframes); |
---|
| 153 | exit(1); |
---|
| 154 | } |
---|
| 155 | |
---|
| 156 | if(frameno>=ncams){ |
---|
| 157 | fprintf(stderr, "readPointParamsAndProjections(): line %d contains an image projection for frame %d " |
---|
| 158 | "but only %d cameras have been specified!\n", lineno+1, frameno, ncams); |
---|
| 159 | exit(1); |
---|
| 160 | } |
---|
| 161 | |
---|
| 162 | projs+=2; |
---|
| 163 | vmask[ptno*ncams+frameno]=1; |
---|
| 164 | } |
---|
| 165 | |
---|
| 166 | fscanf(fp, "\n"); // consume trailing newline |
---|
| 167 | |
---|
| 168 | lineno++; |
---|
| 169 | ptno++; |
---|
| 170 | } |
---|
| 171 | } |
---|
| 172 | |
---|
| 173 | |
---|
| 174 | /* combines the above routines to read the initial estimates of the motion + structure parameters from text files. |
---|
| 175 | * Also, it loads the projections of 3D points across images. The routine dynamically allocates the required amount |
---|
| 176 | * of memory (last 3 arguments). |
---|
| 177 | */ |
---|
| 178 | void readInitialSBAEstimate(char *camsfname, char *ptsfname, int *ncams, int *n3Dpts, int *n2Dprojs, double **motstruct, double **imgpts, char **vmask) |
---|
| 179 | { |
---|
| 180 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 181 | pnp=3, /* euclidean 3D points */ |
---|
| 182 | mnp=2; /* img ponts are 2D */ |
---|
| 183 | |
---|
| 184 | FILE *fpc, *fpp; |
---|
| 185 | |
---|
| 186 | if((fpc=fopen(camsfname, "r"))==NULL){ |
---|
| 187 | fprintf(stderr, "cannot open file %s, exiting\n", camsfname); |
---|
| 188 | exit(1); |
---|
| 189 | } |
---|
| 190 | |
---|
| 191 | if((fpp=fopen(ptsfname, "r"))==NULL){ |
---|
| 192 | fprintf(stderr, "cannot open file %s, exiting\n", ptsfname); |
---|
| 193 | exit(1); |
---|
| 194 | } |
---|
| 195 | |
---|
| 196 | *ncams=readNcameras(fpc); |
---|
| 197 | readNpointsAndNprojections(fpp, n3Dpts, n2Dprojs); |
---|
| 198 | |
---|
| 199 | *motstruct=(double *)malloc((*ncams*cnp + *n3Dpts*pnp)*sizeof(double)); |
---|
| 200 | if(*motstruct==NULL){ |
---|
| 201 | fprintf(stderr, "memory allocation failed in readInitialSBAEstimate()\n"); |
---|
| 202 | exit(1); |
---|
| 203 | } |
---|
| 204 | *imgpts=(double *)malloc(*n2Dprojs*mnp*sizeof(double)); |
---|
| 205 | if(*imgpts==NULL){ |
---|
| 206 | fprintf(stderr, "memory allocation failed in readInitialSBAEstimate()\n"); |
---|
| 207 | exit(1); |
---|
| 208 | } |
---|
| 209 | *vmask=(char *)malloc(*n3Dpts * *ncams * sizeof(char)); |
---|
| 210 | if(*vmask==NULL){ |
---|
| 211 | fprintf(stderr, "memory allocation failed in readInitialSBAEstimate()\n"); |
---|
| 212 | exit(1); |
---|
| 213 | } |
---|
| 214 | memset(*vmask, 0, *n3Dpts * *ncams * sizeof(char)); /* clear vmask */ |
---|
| 215 | |
---|
| 216 | |
---|
| 217 | /* prepare for re-reading files */ |
---|
| 218 | rewind(fpc); |
---|
| 219 | rewind(fpp); |
---|
| 220 | |
---|
| 221 | readCameraParams(fpc, *motstruct); |
---|
| 222 | readPointParamsAndProjections(fpp, *motstruct+*ncams*cnp, *imgpts, *vmask, *ncams); |
---|
| 223 | |
---|
| 224 | fclose(fpc); |
---|
| 225 | fclose(fpp); |
---|
| 226 | } |
---|
| 227 | |
---|
| 228 | /* reads the 3x3 intrinsic calibration matrix contained in a file */ |
---|
| 229 | void readCalibParams(char *fname, double ical[9]) |
---|
| 230 | { |
---|
| 231 | FILE *fp; |
---|
| 232 | int i; |
---|
| 233 | |
---|
| 234 | if((fp=fopen(fname, "r"))==NULL){ |
---|
| 235 | fprintf(stderr, "cannot open file %s, exiting\n", fname); |
---|
| 236 | exit(1); |
---|
| 237 | } |
---|
| 238 | |
---|
| 239 | for(i=0; i<3; i++){ |
---|
| 240 | fscanf(fp, "%lf%lf%lf\n", ical, ical+1, ical+2); |
---|
| 241 | ical+=3; |
---|
| 242 | } |
---|
| 243 | |
---|
| 244 | fclose(fp); |
---|
| 245 | } |
---|
| 246 | |
---|
| 247 | /* prints the initial estimates of the motion + structure parameters. It also prints the projections |
---|
| 248 | * of 3D points across images. For debugging purposes only. |
---|
| 249 | */ |
---|
| 250 | void printSBAData(double *motstruct, int ncams, int n3Dpts, double *imgpts, int n2Dprojs, char *vmask) |
---|
| 251 | { |
---|
| 252 | const int cnp=7, /* 4 rot params + 3 trans params */ |
---|
| 253 | pnp=3, /* euclidean 3D points */ |
---|
| 254 | mnp=2; /* img ponts are 2D */ |
---|
| 255 | |
---|
| 256 | register int i; |
---|
| 257 | |
---|
| 258 | printf("Motion parameters:\n"); |
---|
| 259 | for(i=0; i<ncams*cnp; ++i) |
---|
| 260 | printf("%lf ", motstruct[i]); |
---|
| 261 | |
---|
| 262 | motstruct+=i; |
---|
| 263 | printf("\n\nStructure parameters:\n"); |
---|
| 264 | for(i=0; i<n3Dpts*pnp; ++i) |
---|
| 265 | printf("%lf ", motstruct[i]); |
---|
| 266 | |
---|
| 267 | printf("\n\nImage projections:\n"); |
---|
| 268 | for(i=0; i<n2Dprojs*mnp; ++i) |
---|
| 269 | printf("%lf ", imgpts[i]); |
---|
| 270 | |
---|
| 271 | printf("\n\nVisibility mask\n"); |
---|
| 272 | for(i=0; i<ncams*n3Dpts; ++i) |
---|
| 273 | printf("%d%s", (int)vmask[i], ((i+1)%ncams)? " " : "\n"); |
---|
| 274 | printf("\n"); |
---|
| 275 | } |
---|