1 |
greg |
2.1 |
#ifndef lint |
2 |
greg |
2.13 |
static const char RCSid[] = "$Id: rcode_depth.c,v 2.12 2022/06/30 00:16:49 greg Exp $"; |
3 |
greg |
2.1 |
#endif |
4 |
|
|
/* |
5 |
greg |
2.3 |
* Encode and decode depth map using 16-bit integers |
6 |
greg |
2.1 |
*/ |
7 |
|
|
|
8 |
|
|
#include "copyright.h" |
9 |
|
|
|
10 |
|
|
#include <stdlib.h> |
11 |
|
|
#include <math.h> |
12 |
|
|
#include <ctype.h> |
13 |
|
|
#include "platform.h" |
14 |
|
|
#include "rtio.h" |
15 |
|
|
#include "fvect.h" |
16 |
|
|
#include "depthcodec.h" |
17 |
|
|
|
18 |
|
|
enum {CV_FWD, CV_REV, CV_PTS}; |
19 |
|
|
|
20 |
|
|
|
21 |
|
|
/* Report usage error and exit */ |
22 |
|
|
static void |
23 |
|
|
usage_exit(int code) |
24 |
|
|
{ |
25 |
|
|
fputs("Usage: ", stderr); |
26 |
|
|
fputs(progname, stderr); |
27 |
|
|
fputs( |
28 |
greg |
2.8 |
" [-d ref_depth/unit][-h[io]][-H[io]][-f[afd]][-x xr -y yr] [input [output.dpt]]\n", |
29 |
greg |
2.1 |
stderr); |
30 |
|
|
fputs(" Or: ", stderr); |
31 |
|
|
fputs(progname, stderr); |
32 |
|
|
fputs( |
33 |
greg |
2.4 |
" {-r|-p} [-i][-u][-h[io]][-H[io]][-f[afd]] [input.dpt [output]]\n", |
34 |
greg |
2.1 |
stderr); |
35 |
|
|
exit(code); |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
|
39 |
|
|
/* Encode depths from input stream */ |
40 |
|
|
static int |
41 |
|
|
encode_depths(DEPTHCODEC *dcp) |
42 |
|
|
{ |
43 |
greg |
2.3 |
long nexpected = (long)dcp->res.xr * dcp->res.yr; |
44 |
greg |
2.1 |
|
45 |
|
|
if (dcp->inpfmt[0]) { |
46 |
greg |
2.10 |
if (!strcmp(dcp->inpfmt, "ascii")) |
47 |
greg |
2.1 |
dcp->format = 'a'; |
48 |
greg |
2.10 |
else if (!strcmp(dcp->inpfmt, "float")) |
49 |
greg |
2.1 |
dcp->format = 'f'; |
50 |
greg |
2.10 |
else if (!strcmp(dcp->inpfmt, "double")) |
51 |
greg |
2.1 |
dcp->format = 'd'; |
52 |
|
|
else { |
53 |
|
|
fputs(dcp->inpname, stderr); |
54 |
|
|
fputs(": unsupported input format: ", stderr); |
55 |
|
|
fputs(dcp->inpfmt, stderr); |
56 |
|
|
fputc('\n', stderr); |
57 |
|
|
return 0; |
58 |
|
|
} |
59 |
|
|
} |
60 |
greg |
2.11 |
if (dcp->format == 'a') |
61 |
|
|
SET_FILE_TEXT(dcp->finp); |
62 |
greg |
2.3 |
|
63 |
greg |
2.1 |
do { |
64 |
|
|
int ok = 0; |
65 |
|
|
float f; |
66 |
|
|
double d; |
67 |
|
|
|
68 |
|
|
switch (dcp->format) { |
69 |
|
|
case 'a': |
70 |
|
|
ok = (fscanf(dcp->finp, "%lf", &d) == 1); |
71 |
|
|
break; |
72 |
|
|
case 'f': |
73 |
|
|
ok = (getbinary(&f, sizeof(f), 1, dcp->finp) == 1); |
74 |
greg |
2.6 |
if (dcp->swapped) |
75 |
|
|
swap32((char *)&f, 1); |
76 |
greg |
2.1 |
d = f; |
77 |
|
|
break; |
78 |
|
|
case 'd': |
79 |
|
|
ok = (getbinary(&d, sizeof(d), 1, dcp->finp) == 1); |
80 |
greg |
2.6 |
if (dcp->swapped) |
81 |
|
|
swap64((char *)&d, 1); |
82 |
greg |
2.1 |
break; |
83 |
|
|
} |
84 |
|
|
if (!ok) |
85 |
|
|
break; |
86 |
greg |
2.3 |
|
87 |
greg |
2.1 |
putint(depth2code(d, dcp->refdepth), 2, stdout); |
88 |
greg |
2.3 |
|
89 |
greg |
2.1 |
} while (--nexpected); |
90 |
|
|
|
91 |
|
|
if (nexpected > 0) { |
92 |
|
|
fputs(dcp->inpname, stderr); |
93 |
|
|
fputs(": fewer depths than expected\n", stderr); |
94 |
|
|
return 0; |
95 |
|
|
} |
96 |
|
|
return 1; |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
|
100 |
greg |
2.3 |
/* Convert and output the given depth to stdout */ |
101 |
greg |
2.1 |
static void |
102 |
|
|
output_depth(DEPTHCODEC *dcp, double d) |
103 |
|
|
{ |
104 |
|
|
float f; |
105 |
|
|
|
106 |
|
|
switch (dcp->format) { |
107 |
|
|
case 'a': |
108 |
|
|
printf("%.5e\n", d); |
109 |
|
|
break; |
110 |
|
|
case 'f': |
111 |
|
|
f = d; |
112 |
|
|
putbinary(&f, sizeof(f), 1, stdout); |
113 |
|
|
break; |
114 |
|
|
case 'd': |
115 |
|
|
putbinary(&d, sizeof(d), 1, stdout); |
116 |
|
|
break; |
117 |
|
|
} |
118 |
|
|
} |
119 |
|
|
|
120 |
|
|
|
121 |
|
|
/* Decode depths from input stream */ |
122 |
|
|
static int |
123 |
|
|
decode_depths(DEPTHCODEC *dcp) |
124 |
|
|
{ |
125 |
greg |
2.3 |
long nexpected = (long)dcp->res.xr * dcp->res.yr; |
126 |
greg |
2.1 |
|
127 |
|
|
if (!check_decode_depths(dcp)) |
128 |
|
|
return 0; |
129 |
|
|
|
130 |
|
|
do { |
131 |
greg |
2.3 |
double d = decode_depth_next(dcp); |
132 |
greg |
2.1 |
if (d < -FTINY) |
133 |
|
|
break; |
134 |
|
|
output_depth(dcp, d); |
135 |
|
|
|
136 |
|
|
} while (--nexpected); |
137 |
|
|
|
138 |
|
|
if (nexpected > 0) { |
139 |
|
|
fputs(dcp->inpname, stderr); |
140 |
|
|
fputs(": unexpected EOF\n", stderr); |
141 |
|
|
return 0; |
142 |
|
|
} |
143 |
|
|
return 1; |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
|
147 |
|
|
/* Decode depths at particular pixels given on standard input */ |
148 |
|
|
static int |
149 |
|
|
pixel_depths(DEPTHCODEC *dcp, int unbuf) |
150 |
|
|
{ |
151 |
|
|
int xy[2]; |
152 |
|
|
double d; |
153 |
|
|
|
154 |
|
|
if (dcp->finp == stdin) { |
155 |
|
|
fputs(progname, stderr); |
156 |
|
|
fputs(": <stdin> used for encoded depths\n", stderr); |
157 |
|
|
return 0; |
158 |
|
|
} |
159 |
|
|
if (!check_decode_depths(dcp)) |
160 |
|
|
return 0; |
161 |
|
|
|
162 |
|
|
while (scanf("%d %d", &xy[0], &xy[1]) == 2) { |
163 |
greg |
2.12 |
loc2pix(xy, &dcp->res, xy[0]/(double)dcp->res.xr, |
164 |
|
|
xy[1]/(double)dcp->res.yr); |
165 |
greg |
2.1 |
d = decode_depth_pix(dcp, xy[0], xy[1]); |
166 |
|
|
if (d < -FTINY) |
167 |
|
|
return 0; |
168 |
|
|
output_depth(dcp, d); |
169 |
|
|
if (unbuf && fflush(stdout) == EOF) { |
170 |
|
|
fputs(progname, stderr); |
171 |
|
|
fputs(": write error on output\n", stderr); |
172 |
|
|
return 0; |
173 |
|
|
} |
174 |
|
|
} |
175 |
|
|
if (!feof(stdin)) { |
176 |
|
|
fputs(progname, stderr); |
177 |
|
|
fputs(": non-integer as pixel index\n", stderr); |
178 |
|
|
return 0; |
179 |
|
|
} |
180 |
|
|
return 1; |
181 |
|
|
} |
182 |
|
|
|
183 |
|
|
|
184 |
greg |
2.3 |
/* Output the given world position to stdout */ |
185 |
greg |
2.1 |
static void |
186 |
|
|
output_worldpos(DEPTHCODEC *dcp, FVECT wpos) |
187 |
|
|
{ |
188 |
|
|
switch (dcp->format) { |
189 |
|
|
case 'a': |
190 |
greg |
2.3 |
printf("%.5e %.5e %.5e\n", wpos[0], wpos[1], wpos[2]); |
191 |
greg |
2.1 |
break; |
192 |
|
|
#ifdef SMLFLT |
193 |
|
|
case 'f': |
194 |
|
|
putbinary(wpos, sizeof(*wpos), 3, stdout); |
195 |
|
|
break; |
196 |
|
|
case 'd': { |
197 |
|
|
double wpd[3]; |
198 |
|
|
VCOPY(wpd, wpos); |
199 |
|
|
putbinary(wpd, sizeof(*wpd), 3, stdout); |
200 |
|
|
} |
201 |
|
|
break; |
202 |
|
|
#else |
203 |
|
|
case 'f': { |
204 |
|
|
float wpf[3]; |
205 |
|
|
VCOPY(wpf, wpos); |
206 |
|
|
putbinary(wpf, sizeof(*wpf), 3, stdout); |
207 |
|
|
} |
208 |
|
|
break; |
209 |
|
|
case 'd': |
210 |
|
|
putbinary(wpos, sizeof(*wpos), 3, stdout); |
211 |
|
|
break; |
212 |
|
|
#endif |
213 |
|
|
} |
214 |
|
|
} |
215 |
|
|
|
216 |
|
|
|
217 |
|
|
/* Decode world points from input stream */ |
218 |
|
|
static int |
219 |
|
|
decode_points(DEPTHCODEC *dcp) |
220 |
|
|
{ |
221 |
|
|
long n = (long)dcp->res.xr * dcp->res.yr; |
222 |
|
|
|
223 |
|
|
if (!check_decode_worldpos(dcp)) |
224 |
|
|
return 0; |
225 |
|
|
|
226 |
|
|
while (n-- > 0) { |
227 |
|
|
FVECT wpos; |
228 |
|
|
if (decode_worldpos_next(wpos, dcp) < 0) { |
229 |
|
|
if (feof(dcp->finp)) { |
230 |
|
|
fputs(dcp->inpname, stderr); |
231 |
|
|
fputs(": unexpected EOF\n", stderr); |
232 |
|
|
} |
233 |
|
|
return 0; |
234 |
|
|
} |
235 |
|
|
output_worldpos(dcp, wpos); |
236 |
|
|
} |
237 |
|
|
return 1; |
238 |
|
|
} |
239 |
|
|
|
240 |
|
|
|
241 |
|
|
/* Decode world points at particular pixels given on standard input */ |
242 |
|
|
static int |
243 |
|
|
pixel_points(DEPTHCODEC *dcp, int unbuf) |
244 |
|
|
{ |
245 |
|
|
int xy[2]; |
246 |
|
|
FVECT wpos; |
247 |
|
|
|
248 |
|
|
if (dcp->finp == stdin) { |
249 |
|
|
fputs(progname, stderr); |
250 |
|
|
fputs(": <stdin> used for encoded depths\n", stderr); |
251 |
|
|
return 0; |
252 |
|
|
} |
253 |
|
|
if (!check_decode_worldpos(dcp)) |
254 |
|
|
return 0; |
255 |
greg |
2.11 |
|
256 |
greg |
2.1 |
while (scanf("%d %d", &xy[0], &xy[1]) == 2) { |
257 |
greg |
2.12 |
loc2pix(xy, &dcp->res, xy[0]/(double)dcp->res.xr, |
258 |
|
|
xy[1]/(double)dcp->res.yr); |
259 |
greg |
2.1 |
if (get_worldpos_pix(wpos, dcp, xy[0], xy[1]) < 0) |
260 |
|
|
return 0; |
261 |
|
|
output_worldpos(dcp, wpos); |
262 |
|
|
if (unbuf && fflush(stdout) == EOF) { |
263 |
|
|
fputs(progname, stderr); |
264 |
|
|
fputs(": write error on output\n", stderr); |
265 |
|
|
return 0; |
266 |
|
|
} |
267 |
|
|
} |
268 |
|
|
if (!feof(stdin)) { |
269 |
|
|
fputs(progname, stderr); |
270 |
|
|
fputs(": non-integer as pixel index\n", stderr); |
271 |
|
|
return 0; |
272 |
|
|
} |
273 |
|
|
return 1; |
274 |
|
|
} |
275 |
|
|
|
276 |
|
|
|
277 |
|
|
int |
278 |
|
|
main(int argc, char *argv[]) |
279 |
|
|
{ |
280 |
greg |
2.7 |
int xres=0, yres=0; |
281 |
greg |
2.1 |
int conversion = CV_FWD; |
282 |
|
|
int bypixel = 0; |
283 |
|
|
int unbuffered = 0; |
284 |
|
|
DEPTHCODEC dc; |
285 |
|
|
int a; |
286 |
greg |
2.13 |
/* set global progname */ |
287 |
|
|
fixargv0(argv[0]); |
288 |
greg |
2.1 |
set_dc_defaults(&dc); |
289 |
|
|
dc.hdrflags = HF_ALL; |
290 |
|
|
for (a = 1; a < argc && argv[a][0] == '-'; a++) |
291 |
|
|
switch (argv[a][1]) { |
292 |
|
|
case 'd': |
293 |
|
|
strlcpy(dc.depth_unit, argv[++a], sizeof(dc.depth_unit)); |
294 |
|
|
dc.refdepth = atof(dc.depth_unit); |
295 |
|
|
if (dc.refdepth <= .0) { |
296 |
|
|
fputs(progname, stderr); |
297 |
|
|
fputs(": illegal -d reference depth\n", stderr); |
298 |
|
|
return 1; |
299 |
|
|
} |
300 |
|
|
break; |
301 |
|
|
case 'r': |
302 |
|
|
conversion = CV_REV; |
303 |
|
|
break; |
304 |
|
|
case 'p': |
305 |
|
|
conversion = CV_PTS; |
306 |
|
|
break; |
307 |
|
|
case 'f': |
308 |
|
|
switch (argv[a][2]) { |
309 |
|
|
case 'f': |
310 |
|
|
case 'd': |
311 |
|
|
case 'a': |
312 |
|
|
dc.format = argv[a][2]; |
313 |
|
|
break; |
314 |
|
|
default: |
315 |
|
|
fputs(progname, stderr); |
316 |
|
|
fputs(": unsupported -f? format\n", stderr); |
317 |
|
|
usage_exit(1); |
318 |
|
|
} |
319 |
|
|
break; |
320 |
|
|
case 'h': |
321 |
|
|
switch (argv[a][2]) { |
322 |
|
|
case '\0': |
323 |
|
|
dc.hdrflags &= ~(HF_HEADIN|HF_HEADOUT); |
324 |
|
|
break; |
325 |
|
|
case 'i': |
326 |
|
|
case 'I': |
327 |
|
|
dc.hdrflags &= ~HF_HEADIN; |
328 |
|
|
break; |
329 |
|
|
case 'o': |
330 |
|
|
case 'O': |
331 |
|
|
dc.hdrflags &= ~HF_HEADOUT; |
332 |
|
|
break; |
333 |
|
|
default: |
334 |
|
|
usage_exit(1); |
335 |
|
|
} |
336 |
|
|
break; |
337 |
|
|
case 'H': |
338 |
|
|
switch (argv[a][2]) { |
339 |
|
|
case '\0': |
340 |
|
|
dc.hdrflags &= ~(HF_RESIN|HF_RESOUT); |
341 |
|
|
break; |
342 |
|
|
case 'i': |
343 |
|
|
case 'I': |
344 |
|
|
dc.hdrflags &= ~HF_RESIN; |
345 |
|
|
break; |
346 |
|
|
case 'o': |
347 |
|
|
case 'O': |
348 |
|
|
dc.hdrflags &= ~HF_RESOUT; |
349 |
|
|
break; |
350 |
|
|
default: |
351 |
|
|
usage_exit(1); |
352 |
|
|
} |
353 |
|
|
break; |
354 |
greg |
2.7 |
case 'x': |
355 |
|
|
xres = atoi(argv[++a]); |
356 |
|
|
break; |
357 |
|
|
case 'y': |
358 |
|
|
yres = atoi(argv[++a]); |
359 |
|
|
break; |
360 |
greg |
2.1 |
case 'i': |
361 |
|
|
bypixel++; |
362 |
|
|
break; |
363 |
|
|
case 'u': |
364 |
|
|
unbuffered++; |
365 |
|
|
break; |
366 |
|
|
default: |
367 |
|
|
usage_exit(1); |
368 |
|
|
} |
369 |
|
|
dc.hdrflags |= (conversion == CV_FWD) * HF_ENCODE; |
370 |
|
|
|
371 |
greg |
2.7 |
if ((xres > 0) & (yres > 0)) { |
372 |
|
|
dc.hdrflags &= ~HF_RESIN; |
373 |
|
|
dc.res.rt = PIXSTANDARD; |
374 |
|
|
dc.res.xr = xres; |
375 |
|
|
dc.res.yr = yres; |
376 |
|
|
} else if ((dc.hdrflags & (HF_RESIN|HF_RESOUT)) == HF_RESOUT) { |
377 |
greg |
2.1 |
fputs(progname, stderr); |
378 |
|
|
fputs(": unknown resolution for output\n", stderr); |
379 |
|
|
return 1; |
380 |
|
|
} |
381 |
|
|
if (bypixel) { |
382 |
|
|
if (conversion == CV_FWD) { |
383 |
|
|
fputs(progname, stderr); |
384 |
|
|
fputs(": -i only supported with -r or -p option\n", |
385 |
|
|
stderr); |
386 |
|
|
usage_exit(1); |
387 |
|
|
} |
388 |
|
|
if (a >= argc) { |
389 |
|
|
fputs(progname, stderr); |
390 |
|
|
fputs(": -i option requires input file\n", stderr); |
391 |
|
|
usage_exit(1); |
392 |
|
|
} |
393 |
|
|
if (!(dc.hdrflags & HF_RESIN)) { |
394 |
|
|
fputs(progname, stderr); |
395 |
|
|
fputs(": -i option requires input resolution\n", stderr); |
396 |
|
|
usage_exit(1); |
397 |
|
|
} |
398 |
greg |
2.4 |
dc.hdrflags &= ~HF_RESOUT; |
399 |
greg |
2.1 |
} |
400 |
|
|
if (a < argc-2) { |
401 |
|
|
fputs(progname, stderr); |
402 |
|
|
fputs(": too many file arguments\n", stderr); |
403 |
|
|
usage_exit(1); |
404 |
|
|
} |
405 |
|
|
if (a < argc && !(dc.finp = fopen(dc.inpname=argv[a], "r"))) { |
406 |
|
|
fputs(dc.inpname, stderr); |
407 |
|
|
fputs(": cannot open for reading\n", stderr); |
408 |
|
|
return 1; |
409 |
|
|
} |
410 |
|
|
if (a+1 < argc && !freopen(argv[a+1], "w", stdout)) { |
411 |
|
|
fputs(argv[a+1], stderr); |
412 |
|
|
fputs(": cannot open for writing\n", stderr); |
413 |
|
|
return 1; |
414 |
|
|
} |
415 |
greg |
2.11 |
SET_FILE_BINARY(dc.finp); /* starting assumption */ |
416 |
|
|
SET_FILE_BINARY(stdout); |
417 |
greg |
2.1 |
#ifdef getc_unlocked /* avoid stupid semaphores */ |
418 |
|
|
flockfile(dc.finp); |
419 |
|
|
flockfile(stdout); |
420 |
|
|
#endif |
421 |
|
|
/* read/copy header */ |
422 |
greg |
2.2 |
if (!process_dc_header(&dc, a, argv)) |
423 |
greg |
2.1 |
return 1; |
424 |
greg |
2.11 |
|
425 |
|
|
if ((conversion != CV_FWD) & (dc.format == 'a')) |
426 |
|
|
SET_FILE_TEXT(stdout); |
427 |
greg |
2.1 |
/* process data */ |
428 |
|
|
switch (conversion) { |
429 |
|
|
case CV_FWD: /* distance -> depth code */ |
430 |
greg |
2.9 |
if (!strcmp(dc.depth_unit, "1")) { |
431 |
|
|
fputs(progname, stderr); |
432 |
|
|
fputs(": warning - using reference depth of 1.0\n", |
433 |
|
|
stderr); |
434 |
|
|
} |
435 |
greg |
2.1 |
if (!encode_depths(&dc)) |
436 |
|
|
return 1; |
437 |
|
|
break; |
438 |
|
|
case CV_REV: /* depth code -> distance */ |
439 |
|
|
if (!(bypixel ? pixel_depths(&dc, unbuffered) : |
440 |
|
|
decode_depths(&dc))) |
441 |
|
|
return 1; |
442 |
|
|
break; |
443 |
|
|
case CV_PTS: /* depth code -> world points */ |
444 |
|
|
if (!(bypixel ? pixel_points(&dc, unbuffered) : |
445 |
|
|
decode_points(&dc))) |
446 |
|
|
return 1; |
447 |
|
|
break; |
448 |
|
|
} |
449 |
|
|
if (fflush(stdout) == EOF) { |
450 |
|
|
fputs(progname, stderr); |
451 |
|
|
fputs(": error writing output\n", stderr); |
452 |
|
|
return 1; |
453 |
|
|
} |
454 |
|
|
return 0; |
455 |
|
|
} |