1 /* Copyright (C) 2013-2016, The Regents of The University of Michigan.
2 All rights reserved.
3 This software was developed in the APRIL Robotics Lab under the
4 direction of Edwin Olson, ebolson@umich.edu. This software may be
5 available under alternative licensing terms; contact the address above.
6 Redistribution and use in source and binary forms, with or without
7 modification, are permitted provided that the following conditions are met:
8 1. Redistributions of source code must retain the above copyright notice, this
9 list of conditions and the following disclaimer.
10 2. Redistributions in binary form must reproduce the above copyright notice,
11 this list of conditions and the following disclaimer in the documentation
12 and/or other materials provided with the distribution.
13 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
14 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
15 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
16 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
17 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
18 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
20 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
21 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
22 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 The views and conclusions contained in the software and documentation are those
24 of the authors and should not be interpreted as representing official policies,
25 either expressed or implied, of the Regents of The University of Michigan.
26 */
27
28 #include <assert.h>
29 #include <stdio.h>
30 #include <stdlib.h>
31 #include <string.h>
32 #include <math.h>
33 #include <algorithm>
34
35 #include "common/image_u8.h"
36 #include "common/pnm.h"
37 #include "common/math_util.h"
38
39 // least common multiple of 64 (sandy bridge cache line) and 24 (stride
40 // needed for RGB in 8-wide vector processing)
41 #define DEFAULT_ALIGNMENT_U8 96
42
image_u8_create_stride(unsigned int width,unsigned int height,unsigned int stride)43 image_u8_t *image_u8_create_stride(unsigned int width, unsigned int height, unsigned int stride)
44 {
45 uint8_t *buf = (uint8_t *)calloc((size_t)(height)*(size_t)(stride), sizeof(uint8_t));
46
47 // const initializer
48 image_u8_t tmp = { (int32_t)width, (int32_t)height, (int32_t)stride, buf };
49
50 image_u8_t *im = (image_u8_t *)calloc(1, sizeof(image_u8_t));
51 memcpy(im, &tmp, sizeof(image_u8_t));
52 return im;
53 }
54
image_u8_create(unsigned int width,unsigned int height)55 image_u8_t *image_u8_create(unsigned int width, unsigned int height)
56 {
57 return image_u8_create_alignment(width, height, DEFAULT_ALIGNMENT_U8);
58 }
59
image_u8_create_alignment(unsigned int width,unsigned int height,unsigned int alignment)60 image_u8_t *image_u8_create_alignment(unsigned int width, unsigned int height, unsigned int alignment)
61 {
62 int stride = width;
63
64 if ((stride % alignment) != 0)
65 stride += alignment - (stride % alignment);
66
67 return image_u8_create_stride(width, height, stride);
68 }
69
image_u8_copy(const image_u8_t * in)70 image_u8_t *image_u8_copy(const image_u8_t *in)
71 {
72 uint8_t *buf = (uint8_t *)malloc((size_t)(in->height)*(size_t)(in->stride)*sizeof(uint8_t));
73 memcpy(buf, in->buf, (size_t)(in->height) * (size_t)(in->stride) * sizeof(uint8_t));
74
75 // const initializer
76 image_u8_t tmp = { (int32_t)in->width, (int32_t)in->height, (int32_t)in->stride, buf };
77
78 image_u8_t *copy = (image_u8_t *)calloc(1, sizeof(image_u8_t));
79 memcpy(copy, &tmp, sizeof(image_u8_t));
80 return copy;
81 }
82
image_u8_destroy(image_u8_t * im)83 void image_u8_destroy(image_u8_t *im)
84 {
85 if (!im)
86 return;
87
88 free(im->buf);
89 free(im);
90 }
91
92 ////////////////////////////////////////////////////////////
93 // PNM file i/o
image_u8_create_from_pnm(const char * path)94 image_u8_t *image_u8_create_from_pnm(const char *path)
95 {
96 return image_u8_create_from_pnm_alignment(path, DEFAULT_ALIGNMENT_U8);
97 }
98
image_u8_create_from_pnm_alignment(const char * path,int alignment)99 image_u8_t *image_u8_create_from_pnm_alignment(const char *path, int alignment)
100 {
101 pnm_t *pnm = pnm_create_from_file(path);
102 if (pnm == NULL)
103 return NULL;
104
105 image_u8_t *im = NULL;
106
107 switch (pnm->format) {
108 case PNM_FORMAT_GRAY: {
109 im = image_u8_create_alignment(pnm->width, pnm->height, alignment);
110
111 if (pnm->max == 255) {
112 for (int y = 0; y < im->height; y++)
113 memcpy(&im->buf[y*im->stride], &pnm->buf[y*im->width], im->width);
114 } else if (pnm->max == 65535) {
115 for (int y = 0; y < im->height; y++)
116 for (int x = 0; x < im->width; x++)
117 im->buf[y*im->stride + x] = pnm->buf[2*(y*im->width + x)];
118 } else {
119 assert(0);
120 }
121
122 break;
123 }
124
125 case PNM_FORMAT_RGB: {
126 im = image_u8_create_alignment(pnm->width, pnm->height, alignment);
127
128 if (pnm->max == 255) {
129 // Gray conversion for RGB is gray = (r + g + g + b)/4
130 for (int y = 0; y < im->height; y++) {
131 for (int x = 0; x < im->width; x++) {
132 uint8_t gray = (pnm->buf[y*im->width*3 + 3*x+0] + // r
133 pnm->buf[y*im->width*3 + 3*x+1] + // g
134 pnm->buf[y*im->width*3 + 3*x+1] + // g
135 pnm->buf[y*im->width*3 + 3*x+2]) // b
136 / 4;
137
138 im->buf[y*im->stride + x] = gray;
139 }
140 }
141 } else if (pnm->max == 65535) {
142 for (int y = 0; y < im->height; y++) {
143 for (int x = 0; x < im->width; x++) {
144 int r = pnm->buf[6*(y*im->width + x) + 0];
145 int g = pnm->buf[6*(y*im->width + x) + 2];
146 int b = pnm->buf[6*(y*im->width + x) + 4];
147
148 im->buf[y*im->stride + x] = (r + g + g + b) / 4;
149 }
150 }
151 } else {
152 assert(0);
153 }
154
155 break;
156 }
157
158 case PNM_FORMAT_BINARY: {
159 im = image_u8_create_alignment(pnm->width, pnm->height, alignment);
160
161 // image is padded to be whole bytes on each row.
162
163 // how many bytes per row on the input?
164 int pbmstride = (im->width + 7) / 8;
165
166 for (int y = 0; y < im->height; y++) {
167 for (int x = 0; x < im->width; x++) {
168 int byteidx = y * pbmstride + x / 8;
169 int bitidx = 7 - (x & 7);
170
171 // ack, black is one according to pbm docs!
172 if ((pnm->buf[byteidx] >> bitidx) & 1)
173 im->buf[y*im->stride + x] = 0;
174 else
175 im->buf[y*im->stride + x] = 255;
176 }
177 }
178 break;
179 }
180 }
181
182 pnm_destroy(pnm);
183 return im;
184 }
185
image_u8_create_from_f32(image_f32_t * fim)186 image_u8_t *image_u8_create_from_f32(image_f32_t *fim)
187 {
188 image_u8_t *im = image_u8_create(fim->width, fim->height);
189
190 for (int y = 0; y < fim->height; y++) {
191 for (int x = 0; x < fim->width; x++) {
192 float v = fim->buf[y*fim->stride + x];
193 im->buf[y*im->stride + x] = (int) (255 * v);
194 }
195 }
196
197 return im;
198 }
199
200
image_u8_write_pnm(const image_u8_t * im,const char * path)201 int image_u8_write_pnm(const image_u8_t *im, const char *path)
202 {
203 FILE *f = fopen(path, "wb");
204 int res = 0;
205
206 if (f == NULL) {
207 res = -1;
208 goto finish;
209 }
210
211 // Only outputs to grayscale
212 fprintf(f, "P5\n%d %d\n255\n", im->width, im->height);
213
214 for (int y = 0; y < im->height; y++) {
215 if (im->width != fwrite(&im->buf[y*im->stride], 1, im->width, f)) {
216 res = -2;
217 goto finish;
218 }
219 }
220
221 finish:
222 if (f != NULL)
223 fclose(f);
224
225 return res;
226 }
227
image_u8_draw_circle(image_u8_t * im,float x0,float y0,float r,int v)228 void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v)
229 {
230 r = r*r;
231
232 for (int y = y0-r; y <= y0+r; y++) {
233 for (int x = x0-r; x <= x0+r; x++) {
234 float d = (x-x0)*(x-x0) + (y-y0)*(y-y0);
235 if (d > r)
236 continue;
237
238 if (x >= 0 && x < im->width && y >= 0 && y < im->height) {
239 int idx = y*im->stride + x;
240 im->buf[idx] = v;
241 }
242 }
243 }
244 }
245
image_u8_draw_annulus(image_u8_t * im,float x0,float y0,float r0,float r1,int v)246 void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r1, int v)
247 {
248 r0 = r0*r0;
249 r1 = r1*r1;
250
251 assert(r0 < r1);
252
253 for (int y = y0-r1; y <= y0+r1; y++) {
254 for (int x = x0-r1; x <= x0+r1; x++) {
255 float d = (x-x0)*(x-x0) + (y-y0)*(y-y0);
256 if (d < r0 || d > r1)
257 continue;
258
259 int idx = y*im->stride + x;
260 im->buf[idx] = v;
261 }
262 }
263 }
264
265 // only widths 1 and 3 supported (and 3 only badly)
image_u8_draw_line(image_u8_t * im,float x0,float y0,float x1,float y1,int v,int width)266 void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width)
267 {
268 double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
269 double delta = 0.5 / dist;
270
271 // terrible line drawing code
272 for (float f = 0; f <= 1; f += delta) {
273 int x = ((int) (x1 + (x0 - x1) * f));
274 int y = ((int) (y1 + (y0 - y1) * f));
275
276 if (x < 0 || y < 0 || x >= im->width || y >= im->height)
277 continue;
278
279 int idx = y*im->stride + x;
280 im->buf[idx] = v;
281 if (width > 1) {
282 im->buf[idx+1] = v;
283 im->buf[idx+im->stride] = v;
284 im->buf[idx+1+im->stride] = v;
285 }
286 }
287 }
288
image_u8_darken(image_u8_t * im)289 void image_u8_darken(image_u8_t *im)
290 {
291 for (int y = 0; y < im->height; y++) {
292 for (int x = 0; x < im->width; x++) {
293 im->buf[im->stride*y+x] /= 2;
294 }
295 }
296 }
297
convolve(const uint8_t * x,uint8_t * y,int sz,const uint8_t * k,int ksz)298 static void convolve(const uint8_t *x, uint8_t *y, int sz, const uint8_t *k, int ksz)
299 {
300 assert((ksz&1)==1);
301
302 for (int i = 0; i < ksz/2 && i < sz; i++)
303 y[i] = x[i];
304
305 for (int i = 0; i < sz - ksz; i++) {
306 uint32_t acc = 0;
307
308 for (int j = 0; j < ksz; j++)
309 acc += k[j]*x[i+j];
310
311 y[ksz/2 + i] = acc >> 8;
312 }
313
314 for (int i = sz - ksz + ksz/2; i < sz; i++)
315 y[i] = x[i];
316 }
317
image_u8_convolve_2D(image_u8_t * im,const uint8_t * k,int ksz)318 void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz)
319 {
320 assert((ksz & 1) == 1); // ksz must be odd.
321
322 for (int y = 0; y < im->height; y++) {
323
324 uint8_t *x = (uint8_t *)malloc(sizeof(uint8_t)*im->stride);
325 memcpy(x, &im->buf[y*im->stride], im->stride);
326
327 convolve(x, &im->buf[y*im->stride], im->width, k, ksz);
328 free(x);
329 }
330
331 for (int x = 0; x < im->width; x++) {
332 uint8_t *xb = (uint8_t *)malloc(sizeof(uint8_t)*im->height);
333 uint8_t *yb = (uint8_t *)malloc(sizeof(uint8_t)*im->height);
334
335 for (int y = 0; y < im->height; y++)
336 xb[y] = im->buf[y*im->stride + x];
337
338 convolve(xb, yb, im->height, k, ksz);
339 free(xb);
340
341 for (int y = 0; y < im->height; y++)
342 im->buf[y*im->stride + x] = yb[y];
343 free(yb);
344 }
345 }
346
image_u8_gaussian_blur(image_u8_t * im,double sigma,int ksz)347 void image_u8_gaussian_blur(image_u8_t *im, double sigma, int ksz)
348 {
349 if (sigma == 0)
350 return;
351
352 assert((ksz & 1) == 1); // ksz must be odd.
353
354 // build the kernel.
355 double *dk = (double *)malloc(sizeof(double) * ksz);
356
357 // for kernel of length 5:
358 // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2)
359 for (int i = 0; i < ksz; i++) {
360 int x = -ksz/2 + i;
361 double v = exp(-.5*sq(x / sigma));
362 dk[i] = v;
363 }
364
365 // normalize
366 double acc = 0;
367 for (int i = 0; i < ksz; i++)
368 acc += dk[i];
369
370 for (int i = 0; i < ksz; i++)
371 dk[i] /= acc;
372
373 uint8_t *k = (uint8_t *)malloc(sizeof(uint8_t)*ksz);
374 for (int i = 0; i < ksz; i++)
375 k[i] = dk[i]*255;
376
377 if (0) {
378 for (int i = 0; i < ksz; i++)
379 printf("%d %15f %5d\n", i, dk[i], k[i]);
380 }
381 free(dk);
382
383 image_u8_convolve_2D(im, k, ksz);
384 free(k);
385 }
386
image_u8_rotate(const image_u8_t * in,double rad,uint8_t pad)387 image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad)
388 {
389 int iwidth = in->width, iheight = in->height;
390 rad = -rad; // interpret y as being "down"
391
392 float c = cos(rad), s = sin(rad);
393
394 float p[][2] = { { 0, 0}, { (float)iwidth, 0 }, { (float)iwidth, (float)iheight }, { 0, (float)iheight} };
395
396 //float xmin = HUGE_VALF, xmax = -HUGE_VALF, ymin = HUGE_VALF, ymax = -HUGE_VALF;
397 float xmin = float_pos_inf();//std::numeric_limits<float>::infinity(),
398 float xmax = float_neg_inf();//std::numeric_limits<float>::infinity(),
399 float ymin = float_pos_inf();//std::numeric_limits<float>::infinity(),
400 float ymax = float_neg_inf();//std::numeric_limits<float>::infinity();
401
402 float icx = iwidth / 2.0, icy = iheight / 2.0;
403
404 for (int i = 0; i < 4; i++) {
405 float px = p[i][0] - icx;
406 float py = p[i][1] - icy;
407
408 float nx = px*c - py*s;
409 float ny = px*s + py*c;
410
411 xmin = fmin(xmin, nx);
412 xmax = fmax(xmax, nx);
413 ymin = fmin(ymin, ny);
414 ymax = fmax(ymax, ny);
415 }
416
417 int owidth = ceil(xmax-xmin), oheight = ceil(ymax - ymin);
418 image_u8_t *out = image_u8_create(owidth, oheight);
419
420 // iterate over output pixels.
421 for (int oy = 0; oy < oheight; oy++) {
422 for (int ox = 0; ox < owidth; ox++) {
423 // work backwards from destination coordinates...
424 // sample pixel centers.
425 float sx = ox - owidth / 2.0 + .5;
426 float sy = oy - oheight / 2.0 + .5;
427
428 // project into input-image space
429 int ix = floor(sx*c + sy*s + icx);
430 int iy = floor(-sx*s + sy*c + icy);
431
432 if (ix >= 0 && iy >= 0 && ix < iwidth && iy < iheight)
433 out->buf[oy*out->stride+ox] = in->buf[iy*in->stride + ix];
434 else
435 out->buf[oy*out->stride+ox] = pad;
436 }
437 }
438
439 return out;
440 }
441
image_u8_decimate(image_u8_t * im,float ffactor)442 image_u8_t *image_u8_decimate(image_u8_t *im, float ffactor)
443 {
444 int width = im->width, height = im->height;
445
446 if (ffactor == 1.5) {
447 int swidth = width / 3 * 2, sheight = height / 3 * 2;
448
449 image_u8_t *decim = image_u8_create(swidth, sheight);
450
451 int y = 0, sy = 0;
452 while (sy < sheight) {
453 int x = 0, sx = 0;
454 while (sx < swidth) {
455
456 // a b c
457 // d e f
458 // g h i
459 uint8_t a = im->buf[(y+0)*im->stride + (x+0)];
460 uint8_t b = im->buf[(y+0)*im->stride + (x+1)];
461 uint8_t c = im->buf[(y+0)*im->stride + (x+2)];
462
463 uint8_t d = im->buf[(y+1)*im->stride + (x+0)];
464 uint8_t e = im->buf[(y+1)*im->stride + (x+1)];
465 uint8_t f = im->buf[(y+1)*im->stride + (x+2)];
466
467 uint8_t g = im->buf[(y+2)*im->stride + (x+0)];
468 uint8_t h = im->buf[(y+2)*im->stride + (x+1)];
469 uint8_t i = im->buf[(y+2)*im->stride + (x+2)];
470
471 decim->buf[(sy+0)*decim->stride + (sx + 0)] =
472 (4*a+2*b+2*d+e)/9;
473 decim->buf[(sy+0)*decim->stride + (sx + 1)] =
474 (4*c+2*b+2*f+e)/9;
475
476 decim->buf[(sy+1)*decim->stride + (sx + 0)] =
477 (4*g+2*d+2*h+e)/9;
478 decim->buf[(sy+1)*decim->stride + (sx + 1)] =
479 (4*i+2*f+2*h+e)/9;
480
481 x += 3;
482 sx += 2;
483 }
484
485 y += 3;
486 sy += 2;
487 }
488
489 return decim;
490 }
491
492 int factor = (int) ffactor;
493
494 int swidth = 1 + (width - 1)/factor;
495 int sheight = 1 + (height - 1)/factor;
496 image_u8_t *decim = image_u8_create(swidth, sheight);
497 int sy = 0;
498 for (int y = 0; y < height; y += factor) {
499 int sx = 0;
500 for (int x = 0; x < width; x += factor) {
501 decim->buf[sy*decim->stride + sx] = im->buf[y*im->stride + x];
502 sx++;
503 }
504 sy++;
505 }
506 return decim;
507 }
508
image_u8_fill_line_max(image_u8_t * im,const image_u8_lut_t * lut,const float * xy0,const float * xy1)509 void image_u8_fill_line_max(image_u8_t *im, const image_u8_lut_t *lut, const float *xy0, const float *xy1)
510 {
511 // what is the maximum distance that will result in drawing into our LUT?
512 float max_dist2 = (lut->nvalues-1)/lut->scale;
513 float max_dist = sqrt(max_dist2);
514
515 // the orientation of the line
516 double theta = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]);
517 double v = sin(theta), u = cos(theta);
518
519 int ix0 = iclamp(fmin(xy0[0], xy1[0]) - max_dist, 0, im->width-1);
520 int ix1 = iclamp(fmax(xy0[0], xy1[0]) + max_dist, 0, im->width-1);
521
522 int iy0 = iclamp(fmin(xy0[1], xy1[1]) - max_dist, 0, im->height-1);
523 int iy1 = iclamp(fmax(xy0[1], xy1[1]) + max_dist, 0, im->height-1);
524
525 // the line segment xy0---xy1 can be parameterized in terms of line coordinates.
526 // We fix xy0 to be at line coordinate 0.
527 float xy1_line_coord = (xy1[0]-xy0[0])*u + (xy1[1]-xy0[1])*v;
528
529 float min_line_coord = fmin(0, xy1_line_coord);
530 float max_line_coord = fmax(0, xy1_line_coord);
531
532 for (int iy = iy0; iy <= iy1; iy++) {
533 float y = iy+.5;
534
535 for (int ix = ix0; ix <= ix1; ix++) {
536 float x = ix+.5;
537
538 // compute line coordinate of this pixel.
539 float line_coord = (x - xy0[0])*u + (y - xy0[1])*v;
540
541 // find point on line segment closest to our current pixel.
542 if (line_coord < min_line_coord)
543 line_coord = min_line_coord;
544 else if (line_coord > max_line_coord)
545 line_coord = max_line_coord;
546
547 float px = xy0[0] + line_coord*u;
548 float py = xy0[1] + line_coord*v;
549
550 double dist2 = (x-px)*(x-px) + (y-py)*(y-py);
551
552 // not in our LUT?
553 int idx = dist2 * lut->scale;
554 if (idx >= lut->nvalues)
555 continue;
556
557 uint8_t lut_value = lut->values[idx];
558 uint8_t old_value = im->buf[iy*im->stride + ix];
559 if (lut_value > old_value)
560 im->buf[iy*im->stride + ix] = lut_value;
561 }
562 }
563 }
564