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