Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
image.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "image.h"
28 #include <stdlib.h>
29 #include <string.h>
30 #include "lucas_kanade.h"
31 
32 #ifndef CACHE_LINE_LENGTH
33 #define CACHE_LINE_LENGTH 64
34 #endif
35 
43 void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
44 {
45  // Set the variables
46  img->type = type;
47  img->w = width;
48  img->h = height;
49 
50  // Depending on the type the size differs
51  if (type == IMAGE_YUV422) {
52  img->buf_size = sizeof(uint8_t) * 2 * width * height;
53  } else if (type == IMAGE_JPEG) {
54  img->buf_size = sizeof(uint8_t) * 2 * width * height; // At maximum quality this is enough
55  } else if (type == IMAGE_GRADIENT) {
56  img->buf_size = sizeof(int16_t) * width * height;
57  } else if (type == IMAGE_INT16) {
58  img->buf_size = sizeof(int16_t) * width * height;
59  } else {
60  img->buf_size = sizeof(uint8_t) * width * height;
61  }
62 
63 #if __GLIBC__ > 2 || (__GLIBC__ >= 2 && __GLIBC_MINOR__ >= 16)
64  // aligned memory slightly speeds up any later copies
66 #else
67  img->buf = malloc(img->buf_size);
68 #endif
69 }
70 
75 void image_free(struct image_t *img)
76 {
77  if (img->buf != NULL) {
78  free(img->buf);
79  img->buf = NULL;
80  }
81 }
82 
89 void image_copy(struct image_t *input, struct image_t *output)
90 {
91  if (input->type != output->type) {
92  return;
93  }
94 
95  output->w = input->w;
96  output->h = input->h;
97  output->buf_size = input->buf_size;
98  output->ts = input->ts;
99  output->eulers = input->eulers;
100  output->pprz_ts = input->pprz_ts;
101 
102  memcpy(output->buf, input->buf, input->buf_size);
103 }
104 
112 void image_switch(struct image_t *a, struct image_t *b)
113 {
114  /* Remember everything from image a */
115  struct image_t old_a;
116  memcpy(&old_a, a, sizeof(struct image_t));
117 
118  /* Copy everything from b to a */
119  memcpy(a, b, sizeof(struct image_t));
120 
121  /* Copy everything from the remembered a to b */
122  memcpy(b, &old_a, sizeof(struct image_t));
123 }
124 
131 void image_to_grayscale(struct image_t *input, struct image_t *output)
132 {
133  uint8_t *source = input->buf;
134  uint8_t *dest = output->buf;
135  source++;
136 
137  // Copy the creation timestamp (stays the same)
138  output->ts = input->ts;
139  output->eulers = input->eulers;
140  output->pprz_ts = input->pprz_ts;
141 
142  // Copy the pixels
143  int height = output->h;
144  int width = output->w;
145  if (output->type == IMAGE_YUV422) {
146  for (int y = 0; y < height; y++) {
147  for (int x = 0; x < width; x++) {
148  *dest++ = 127; // U / V
149  *dest++ = *source; // Y
150  source += 2;
151  }
152  }
153  } else {
154  for (int y = 0; y < height * width; y++) {
155  *dest++ = *source++; // Y
156  source++;
157  }
158  }
159 }
160 
173 uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
174  uint8_t u_M, uint8_t v_m, uint8_t v_M)
175 {
176  uint16_t cnt = 0;
177  uint8_t *source = (uint8_t *)input->buf;
178  uint8_t *dest = (uint8_t *)output->buf;
179 
180  // Copy the creation timestamp (stays the same)
181  output->ts = input->ts;
182 
183  // Go trough all the pixels
184  for (uint16_t y = 0; y < output->h; y++) {
185  for (uint16_t x = 0; x < output->w; x += 2) {
186  // Check if the color is inside the specified values
187  if (
188  (dest[1] >= y_m)
189  && (dest[1] <= y_M)
190  && (dest[0] >= u_m)
191  && (dest[0] <= u_M)
192  && (dest[2] >= v_m)
193  && (dest[2] <= v_M)
194  ) {
195  cnt ++;
196  // UYVY
197  dest[0] = 64; // U
198  dest[1] = source[1]; // Y
199  dest[2] = 255; // V
200  dest[3] = source[3]; // Y
201  } else {
202  // UYVY
203  char u = source[0] - 127;
204  u /= 4;
205  dest[0] = 127; // U
206  dest[1] = source[1]; // Y
207  u = source[2] - 127;
208  u /= 4;
209  dest[2] = 127; // V
210  dest[3] = source[3]; // Y
211  }
212 
213  // Go to the next 2 pixels
214  dest += 4;
215  source += 4;
216  }
217  }
218  return cnt;
219 }
220 
237 int check_color_yuv422(struct image_t *im, int x, int y, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
238 {
239  // odd pixels are uy
240  // even pixels are vy
241  // adapt x, so that we always have u-channel in index 0:
242  if (x % 2 == 1) { x--; }
243 
244  // Is the pixel inside the image?
245  if (x < 0 || x >= im->w || y < 0 || y >= im->h) {
246  return 0;
247  }
248 
249  // Take the right place in the buffer:
250  uint8_t *buf = im->buf;
251  buf += 2 * (y * (im->w) + x); // each pixel has two bytes
252 
253  if (
254  (buf[1] >= y_m)
255  && (buf[1] <= y_M)
256  && (buf[0] >= u_m)
257  && (buf[0] <= u_M)
258  && (buf[2] >= v_m)
259  && (buf[2] <= v_M)
260  ) {
261  // the pixel passes:
262  return 1;
263  } else {
264  // the pixel does not:
265  return 0;
266  }
267 }
268 
279 void set_color_yuv422(struct image_t *im, int x, int y, uint8_t Y, uint8_t U, uint8_t V) {
280 
281  // odd pixels are uy
282  // even pixels are vy
283  // adapt x, so that we always have u-channel in index 0:
284  if (x % 2 == 1) { x--; }
285 
286  // Is the pixel inside the image?
287  if (x < 0 || x >= im->w || y < 0 || y >= im->h) {
288  return;
289  }
290 
291  // Take the right place in the buffer:
292  uint8_t *buf = im->buf;
293  buf += 2 * (y * (im->w) + x); // each pixel has two bytes
294 
295  buf[0] = U;
296  buf[1] = Y;
297  buf[2] = V;
298  buf[3] = Y;
299 }
300 
301 
318 void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint8_t downsample)
319 {
320  if (downsample < 1){
321  downsample = 1;
322  }
323 
324  // bound downsample is a power of 2
325  if((downsample & (downsample - 1)) != 0){
326  for(int8_t i = 7; i > 0; i--){
327  if(downsample & (1<<i)){
328  downsample &= (1<<(i));
329  break;
330  }
331  }
332  downsample *= 2;
333  }
334 
335  uint8_t *source = input->buf;
336  uint8_t *dest = output->buf;
337  uint16_t pixelskip = (downsample - 1) * 2;
338 
339  output->w = input->w / downsample;
340  output->h = input->h / downsample;
341  output->type = input->type;
342 
343  // Copy the creation timestamp (stays the same)
344  output->ts = input->ts;
345 
346  // Go through all the pixels
347  for (uint16_t y = 0; y < output->h; y++) {
348  for (uint16_t x = 0; x < output->w; x += 2) {
349  // YUYV
350  *dest++ = *source++; // U
351  *dest++ = *source++; // Y
352  *dest++ = *source++; // V
353  source += pixelskip;
354  *dest++ = *source++; // Y
355  source += pixelskip;
356  }
357  source += pixelskip * input->w;
358  }
359 }
360 
368 void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
369 {
370  // Create padded image based on input
371  image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
372 
373  uint8_t *input_buf = (uint8_t *)input->buf;
374  uint8_t *output_buf = (uint8_t *)output->buf;
375 
376  // Skip first `border_size` rows, iterate through next input->h rows
377  for (uint16_t i = border_size; i != (output->h - border_size); i++) {
378 
379  // Mirror first `border_size` columns
380  for (uint8_t j = 0; j != border_size; j++) {
381  output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
382  }
383 
384  // Copy corresponding row values from input image
385  memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
386 
387  // Mirror last `border_size` columns
388  for (uint8_t j = 0; j != border_size; j++) {
389  output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size - 1 - j];
390  }
391  }
392 
393  // Mirror first `border_size` and last `border_size` rows
394  for (uint8_t i = 0; i != border_size; i++) {
395  memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w],
396  sizeof(uint8_t) * output->w);
397  memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w],
398  &output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
399  }
400 }
401 
414 void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
415 {
416  // Create output image, new image size is half the size of input image without padding (border)
417  image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size) / 2, input->type);
418 
419  uint8_t *input_buf = (uint8_t *)input->buf;
420  uint8_t *output_buf = (uint8_t *)output->buf;
421 
422  uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
423  uint16_t w = input->w;
424  int32_t sum = 0;
425 
426  // Horizontal convolution
427  for (uint16_t i = 0; i != output->h; i++) {
428  for (uint16_t j = 0; j != output->w; j++) {
429  row = border_size + 2 * i; // First skip border, then every second pixel
430  col = border_size + 2 * j;
431 
432  sum = (input_buf[row * w + col]) >> 1;
433  sum += (input_buf[row * w + col + 1] + input_buf[row * w + col - 1]) >> 2;
434 
435  output_buf[i * output->w + j] = sum;
436  }
437  }
438  // Vertical convolution
439  w = output->w;
440  for (uint16_t i = 0; i != output->h - border_size; i++) {
441  for (uint16_t j = 0; j != output->w - border_size; j++) {
442  // Wrong to add border_size again, but offset of a few px acceptable inaccuracy
443  row = border_size + i;
444  col = border_size + j;
445 
446  sum = (output_buf[row * w + col]) >> 1;
447  sum += (output_buf[(row + 1) * w + col] + output_buf[(row - 1) * w + col]) >> 2;
448 
449  output_buf[i * output->w + j] = sum;
450  }
451  }
452 }
453 
454 
464 void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size)
465 {
466  // Pad input image and save it as '0' pyramid level
467  image_add_border(input, &output_array[0], border_size);
468 
469  // Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
470  struct image_t temp;
471 
472  for (uint8_t i = 1; i != pyr_level + 1; i++) {
473  pyramid_next_level(&output_array[i - 1], &temp, border_size);
474  image_add_border(&temp, &output_array[i], border_size);
475  image_free(&temp);
476  }
477 }
478 
491 void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center,
492  uint32_t subpixel_factor, uint8_t border_size)
493 {
494  uint8_t *input_buf = (uint8_t *)input->buf;
495  uint8_t *output_buf = (uint8_t *)output->buf;
496 
497  // Calculate the window size
498  uint16_t half_window = output->w / 2;
499 
500  uint32_t subpixel_w = (input->w - 2) * subpixel_factor;
501  uint32_t subpixel_h = (input->h - 2) * subpixel_factor;
502 
503  // Go through the whole window size in normal coordinates
504  for (uint16_t i = 0; i < output->w; i++) {
505  for (uint16_t j = 0; j < output->h; j++) {
506  // Calculate the subpixel coordinate
507  uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor;
508  uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor;
509 
510  BoundUpper(x, subpixel_w);
511  BoundUpper(y, subpixel_h);
512 
513  // Calculate the original pixel coordinate
514  uint16_t orig_x = x / subpixel_factor;
515  uint16_t orig_y = y / subpixel_factor;
516 
517  // Calculate top left (in subpixel coordinates)
518  uint32_t tl_x = orig_x * subpixel_factor;
519  uint32_t tl_y = orig_y * subpixel_factor;
520 
521  // Check if it is the top left pixel
522  if (tl_x == x && tl_y == y) {
523  output_buf[output->w * j + i] = input_buf[input->w * orig_y + orig_x];
524  } else {
525  // Calculate the difference from the top left
526  uint32_t alpha_x = (x - tl_x);
527  uint32_t alpha_y = (y - tl_y);
528 
529  // Blend from the 4 surrounding pixels
530  uint32_t blend = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + orig_x];
531  blend += alpha_x * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + (orig_x + 1)];
532  blend += (subpixel_factor - alpha_x) * alpha_y * input_buf[input->w * (orig_y + 1) + orig_x];
533  blend += alpha_x * alpha_y * input_buf[input->w * (orig_y + 1) + (orig_x + 1)];
534 
535  // Set the normalized pixel blend
536  output_buf[output->w * j + i] = blend / (subpixel_factor * subpixel_factor);
537  }
538  }
539  }
540 }
541 
549 void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy)
550 {
551  // Fetch the buffers in the correct format
552  uint8_t *input_buf = (uint8_t *)input->buf;
553  int16_t *dx_buf = (int16_t *)dx->buf;
554  int16_t *dy_buf = (int16_t *)dy->buf;
555 
556  // Go trough all pixels except the borders
557  for (uint16_t x = 1; x < input->w - 1; x++) {
558  for (uint16_t y = 1; y < input->h - 1; y++) {
559  dx_buf[(y - 1)*dx->w + (x - 1)] = (int16_t)input_buf[y * input->w + x + 1] - (int16_t)input_buf[y * input->w + x - 1];
560  dy_buf[(y - 1)*dy->w + (x - 1)] = (int16_t)input_buf[(y + 1) * input->w + x] - (int16_t)
561  input_buf[(y - 1) * input->w + x];
562  }
563  }
564 }
565 
573 void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
574 {
575  int32_t sum_dxx = 0, sum_dxy = 0, sum_dyy = 0;
576 
577  // Fetch the buffers in the correct format
578  int16_t *dx_buf = (int16_t *)dx->buf;
579  int16_t *dy_buf = (int16_t *)dy->buf;
580 
581  // Calculate the different sums
582  for (uint16_t x = 0; x < dx->w; x++) {
583  for (uint16_t y = 0; y < dy->h; y++) {
584  sum_dxx += ((int32_t)dx_buf[y * dx->w + x] * dx_buf[y * dx->w + x]);
585  sum_dxy += ((int32_t)dx_buf[y * dx->w + x] * dy_buf[y * dy->w + x]);
586  sum_dyy += ((int32_t)dy_buf[y * dy->w + x] * dy_buf[y * dy->w + x]);
587  }
588  }
589 
590  // output the G vector
591  g[0] = sum_dxx / 255;
592  g[1] = sum_dxy / 255;
593  g[2] = g[1];
594  g[3] = sum_dyy / 255;
595 }
596 
605 uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff)
606 {
607  uint32_t sum_diff2 = 0;
608  int16_t *diff_buf = NULL;
609 
610  // Fetch the buffers in the correct format
611  uint8_t *img_a_buf = (uint8_t *)img_a->buf;
612  uint8_t *img_b_buf = (uint8_t *)img_b->buf;
613 
614  // If we want the difference image back
615  if (diff != NULL) {
616  diff_buf = (int16_t *)diff->buf;
617  }
618 
619  // Go trough the imagge pixels and calculate the difference
620  for (uint16_t x = 0; x < img_b->w; x++) {
621  for (uint16_t y = 0; y < img_b->h; y++) {
622  int16_t diff_c = img_a_buf[(y + 1) * img_a->w + (x + 1)] - img_b_buf[y * img_b->w + x];
623  sum_diff2 += diff_c * diff_c;
624 
625  // Set the difference image
626  if (diff_buf != NULL) {
627  diff_buf[y * diff->w + x] = diff_c;
628  }
629  }
630  }
631 
632  return sum_diff2;
633 }
634 
643 int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult)
644 {
645  int32_t sum = 0;
646  int16_t *img_a_buf = (int16_t *)img_a->buf;
647  int16_t *img_b_buf = (int16_t *)img_b->buf;
648  int16_t *mult_buf = NULL;
649 
650  // When we want an output
651  if (mult != NULL) {
652  mult_buf = (int16_t *)mult->buf;
653  }
654 
655  // Calculate the multiplication
656  for (uint16_t x = 0; x < img_a->w; x++) {
657  for (uint16_t y = 0; y < img_a->h; y++) {
658  int32_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
659  sum += mult_c;
660 
661  // Set the difference image
662  if (mult_buf != NULL) {
663  mult_buf[y * mult->w + x] = mult_c;
664  }
665  }
666  }
667 
668  return sum;
669 }
670 
679 void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt)
680 {
681  uint8_t color[4];
682  color[0] = 255;
683  color[1] = 255;
684  color[2] = 255;
685  color[3] = 255;
686 
687  image_show_points_color(img, points, points_cnt, color);
688 
689 }
690 
701 void image_show_points_color(struct image_t *img, struct point_t *points, uint16_t points_cnt, uint8_t *color)
702 {
703  uint8_t *img_buf = (uint8_t *)img->buf;
704  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
705 
706  int cross_hair = 1;
707  int size_crosshair = 5;
708 
709  // Go trough all points and color them
710  for (int i = 0; i < points_cnt; i++) {
711  if (!cross_hair) {
712  uint32_t idx = pixel_width * points[i].y * img->w + points[i].x * pixel_width;
713  img_buf[idx] = 255;
714 
715  // YUV422 consists of 2 pixels
716  if (img->type == IMAGE_YUV422) {
717  idx++;
718  img_buf[idx] = 255;
719  }
720  } else {
721  image_draw_crosshair(img, &(points[i]), color, size_crosshair);
722  }
723  }
724 }
725 
726 void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
727 {
728  static uint8_t color[4] = {255, 255, 255, 255};
729  static uint8_t bad_color[4] = {0, 0, 0, 0};
730  image_show_flow_color(img, vectors, points_cnt, subpixel_factor, color, bad_color);
731 }
732 
743 void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor,
744  const uint8_t *color, const uint8_t *bad_color)
745 {
746  static int size_crosshair = 5;
747 
748  // Go through all the points
749  for (uint16_t i = 0; i < points_cnt; i++) {
750  // Draw a line from the original position with the flow vector
751  struct point_t from = {
752  .x = vectors[i].pos.x / subpixel_factor,
753  .y = vectors[i].pos.y / subpixel_factor
754  };
755  struct point_t to = {
756  .x = (uint32_t)roundf(((float)vectors[i].pos.x + vectors[i].flow_x) / subpixel_factor),
757  .y = (uint32_t)roundf(((float)vectors[i].pos.y + vectors[i].flow_y) / subpixel_factor)
758  };
759 
760  if (vectors[i].error >= LARGE_FLOW_ERROR) {
761  image_draw_crosshair(img, &to, bad_color, size_crosshair);
762  image_draw_line_color(img, &from, &to, bad_color);
763  } else {
764  image_draw_crosshair(img, &to, color, size_crosshair);
765  image_draw_line_color(img, &from, &to, color);
766  }
767  }
768 }
777 void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int *dy)
778 {
779  // create the simple and sobel filter only once:
780 
781  int gradient_x, gradient_y, index;
782  gradient_x = 0;
783  gradient_y = 0;
784 
785  // get image buffer and take into account YUV vs. grayscale:
786  uint8_t *img_buf = (uint8_t *)img->buf;
787  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
788  uint8_t add_ind = pixel_width - 1;
789 
790  // check if all pixels will fall in the image:
791  if (loc->x >= 1 && (loc->x + 1) < img->w && loc->y >= 1 && (loc->y + 1) < img->h) {
792  if (method == 0) {
793 
794  // *************
795  // Simple method
796  // *************
797 
798  // dx:
799  index = loc->y * img->w * pixel_width + (loc->x - 1) * pixel_width;
800  gradient_x -= (int) img_buf[index + add_ind];
801  index = loc->y * img->w * pixel_width + (loc->x + 1) * pixel_width;
802  gradient_x += (int) img_buf[index + add_ind];
803  // dy:
804  index = (loc->y - 1) * img->w * pixel_width + loc->x * pixel_width;
805  gradient_y -= (int) img_buf[index + add_ind];
806  index = (loc->y + 1) * img->w * pixel_width + loc->x * pixel_width;
807  gradient_y += (int) img_buf[index + add_ind];
808  } else {
809 
810  // *****
811  // Sobel
812  // *****
813  static int Sobel[9] = { -1, 0, 1, -2, 0, 2, -1, 0, 1};
814  static int total_sobel = 8;
815 
816  int filt_ind_y = 0;
817  int filt_ind_x;
818  for (int x = -1; x <= 1; x++) {
819  for (int y = -1; y <= 1; y++) {
820  index = (loc->y + y) * img->w * pixel_width + (loc->x + x) * pixel_width;
821  if (x != 0) {
822  filt_ind_x = (x + 1) % 3 + (y + 1) * 3;
823  gradient_x += Sobel[filt_ind_x] * (int) img_buf[index + add_ind];
824  }
825  if (y != 0) {
826  gradient_y += Sobel[filt_ind_y] * (int) img_buf[index + add_ind];
827  }
828  filt_ind_y++;
829  }
830  }
831  gradient_x /= total_sobel;
832  }
833  }
834 
835  // TODO: more efficient would be to use dx, dy directly:
836  (*dx) = gradient_x;
837  (*dy) = gradient_y;
838 }
839 
849 void image_draw_rectangle(struct image_t *img, int x_min, int x_max, int y_min, int y_max, uint8_t *color)
850 {
851  struct point_t from, to;
852 
853  // bottom from left to right:
854  from.x = x_min;
855  from.y = y_min;
856  to.x = x_max;
857  to.y = y_min;
858  image_draw_line_color(img, &from, &to, color);
859 
860  // from bottom right to top right:
861  from.x = x_max;
862  from.y = y_min;
863  to.x = x_max;
864  to.y = y_max;
865  image_draw_line_color(img, &from, &to, color);
866 
867  // from top right to top left:
868  from.x = x_max;
869  from.y = y_max;
870  to.x = x_min;
871  to.y = y_max;
872  image_draw_line_color(img, &from, &to, color);
873 
874  // from top left to bottom left:
875  from.x = x_min;
876  from.y = y_max;
877  to.x = x_min;
878  to.y = y_min;
879  image_draw_line_color(img, &from, &to, color);
880 
881 }
882 
891 void image_draw_crosshair(struct image_t *img, struct point_t *loc, const uint8_t *color, uint32_t size_crosshair)
892 {
893  struct point_t from, to;
894 
895  if (loc->x >= size_crosshair && loc->x < img->w - size_crosshair
896  && loc->y >= size_crosshair && loc->y < img->h - size_crosshair) {
897  // draw the lines:
898  from.x = loc->x - size_crosshair;
899  from.y = loc->y;
900  to.x = loc->x + size_crosshair;
901  to.y = loc->y;
902  image_draw_line_color(img, &from, &to, color);
903  from.x = loc->x;
904  from.y = loc->y - size_crosshair;
905  to.x = loc->x;
906  to.y = loc->y + size_crosshair;
907  image_draw_line_color(img, &from, &to, color);
908  }
909 }
910 
917 void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to)
918 {
919  static uint8_t color[4] = {255, 255, 255, 255};
920  image_draw_line_color(img, from, to, color);
921 }
922 
923 
932 void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, const uint8_t *color)
933 {
934  int xerr = 0, yerr = 0;
935  uint8_t *img_buf = (uint8_t *)img->buf;
936  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
937  uint16_t startx = from->x;
938  uint16_t starty = from->y;
939 
940  uint8_t temp_color[4] = {color[0], color[1], color[2], color[3]};
941 
942  /* compute the distances in both directions */
943  int32_t delta_x = to->x - from->x;
944  int32_t delta_y = to->y - from->y;
945 
946  /* Compute the direction of the increment,
947  an increment of 0 means either a horizontal or vertical
948  line.
949  */
950  int8_t incx, incy;
951  if (delta_x > 0) { incx = 1; }
952  else if (delta_x == 0) { incx = 0; }
953  else { incx = -1; }
954 
955  if (delta_y > 0) { incy = 1; }
956  else if (delta_y == 0) { incy = 0; }
957  else { incy = -1; }
958 
959  /* determine which distance is greater */
960  uint16_t distance = 0;
961  delta_x = abs(delta_x);
962  delta_y = abs(delta_y);
963  if (delta_x > delta_y) { distance = delta_x * 20; }
964  else { distance = delta_y * 20; }
965 
966  /* draw the line */
967  for (uint16_t t = 0; /* starty >= 0 && */ starty < img->h && /* startx >= 0 && */ startx < img->w
968  && t <= distance + 1; t++) {
969 
970  // depending on startx being odd or even, we first have to set U or V
971  if (startx % 2 == 1) {
972  temp_color[0] = color[2];
973  temp_color[2] = color[0];
974  } else {
975  temp_color[0] = color[0];
976  temp_color[2] = color[2];
977  }
978  uint32_t buf_loc = img->w * pixel_width * starty + startx * pixel_width;
979  img_buf[buf_loc] = temp_color[0]; // u (when startx even)
980 
981  if (img->type == IMAGE_YUV422) {
982  img_buf[buf_loc + 1] = temp_color[1]; // y1
983 
984  if (startx + 1 < img->w) {
985  img_buf[buf_loc + 2] = temp_color[2]; // v (when startx even)
986  img_buf[buf_loc + 3] = temp_color[3]; // y2
987  }
988  }
989 
990  xerr += delta_x;
991  yerr += delta_y;
992  if (xerr > distance) {
993  xerr -= distance;
994  startx += incx;
995  }
996  if (yerr > distance) {
997  yerr -= distance;
998  starty += incy;
999  }
1000  }
1001 }
1002 
1003 
1013 uint8_t ker_mul_3x3(uint8_t const *source, int_fast8_t const *kernel, uint8_t total, uint8_t setting, int width, int YUV) {
1014  int value;
1015  int offset = 1 + YUV;
1016 
1017  switch (setting) {
1018  default: { // No edge
1019  value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] +
1020  *(source - offset) * kernel[3] + *source * kernel[4] + *(source + offset) * kernel[5] +
1021  *(source - offset + width) * kernel[6] + *(source + width) * kernel[7] + *(source + offset + width) * kernel[8];
1022  break; }
1023  case 1: { // Upper Edge
1024  value = *(source - offset) * kernel[3] + *source * kernel[4] + *(source + offset) * kernel[5] +
1025  *(source - offset + width) * kernel[6] + *(source + width) * kernel[7] + *(source + offset + width) * kernel[8];
1026  break; }
1027  case 2: { // Lower Edge
1028  value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] +
1029  *(source - offset) * kernel[3] + *source * kernel[4] + *(source + offset) * kernel[5];
1030  break; }
1031  case 3: { // Left Edge
1032  value = *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] +
1033  *source * kernel[4] + *(source + offset) * kernel[5] +
1034  *(source + width) * kernel[7] + *(source + offset + width) * kernel[8];
1035  break; }
1036  case 4: { // Right Edge
1037  value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] +
1038  *(source - offset) * kernel[3] + *source * kernel[4] +
1039  *(source - offset + width) * kernel[6] + *(source + width) * kernel[7];
1040  break; }
1041  case 5: {// Top Left Corner
1042  value = *source * kernel[4] + *(source + offset) * kernel[5] +
1043  *(source + width) * kernel[7] + *(source + offset + width) * kernel[8];
1044  break; }
1045  case 6: {// Top Right Corner
1046  value = *(source - offset) * kernel[3] + *source * kernel[4] +
1047  *(source - offset + width) * kernel[6] + *(source + width) * kernel[7];
1048  break; }
1049  case 7: {// Bottom Left Corner
1050  value = *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] +
1051  *source * kernel[4] + *(source + offset) * kernel[5];
1052  break; }
1053  case 8: {// Bottom Right Corner
1054  value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] +
1055  *(source - offset) * kernel[3] + *source * kernel[4];
1056  break; }
1057  }
1058 
1059  return (uint8_t) ((int) abs(value) / total);
1060 }
1061 
1068 void image_convolution_3x3(struct image_t *input, struct image_t *output, int_fast8_t const *kernel, uint8_t kernel_total) {
1069  // Copy buffer pointers
1070  uint8_t *source = input->buf;
1071  uint8_t *dest = output->buf;
1072 
1073  // Copy the creation timestamp (stays the same)
1074  output->ts = input->ts;
1075  output->eulers = input->eulers;
1076  output->pprz_ts = input->pprz_ts;
1077 
1078  int height = output->h;
1079  int width = output->w;
1080 
1081  if (output->type == IMAGE_YUV422) {
1082  // Skip The first U/V value
1083  *dest++ = 127;
1084  source++;
1085 
1086  // Top Left Corner
1087  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 5, 2 * width, 1);
1088  source += 2;
1089  *dest++ = 127;
1090 
1091  // Upper Edge
1092  for (int x = 1; x < width - 1; x++) {
1093  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 1, 2 * width, 1);
1094  source += 2;
1095  *dest++ = 127;
1096  }
1097 
1098  // Top Right Corner
1099  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 6, 2 * width, 1);
1100  source += 2;
1101  *dest++ = 127;
1102 
1103  for (int y = 1; y < height - 1; y++) {
1104  // Left Edge
1105  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 3, 2 * width, 1);
1106  source += 2;
1107  *dest++ = 127;
1108 
1109  // Middle
1110  for (int x = 1; x < width - 1; x++) {
1111  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 0, 2 * width, 1);
1112  source += 2;
1113  *dest++ = 127;
1114  }
1115 
1116  // Right Edge
1117  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 4, 2 * width, 1);
1118  source += 2;
1119  *dest++ = 127;
1120  }
1121 
1122  // Bottom Left Corner
1123  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 7, 2 * width, 1);
1124  source += 2;
1125  *dest++ = 127;
1126 
1127  // Bottom Edge
1128  for (int x = 1; x < width - 1; x++) {
1129  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 2, 2 * width, 1);
1130  source += 2;
1131  *dest++ = 127;
1132  }
1133 
1134  // Bottom Right Corner
1135  *dest = ker_mul_3x3(source, kernel, kernel_total, 8, 2 * width, 1);
1136  } else {
1137  if (output->type == IMAGE_GRAYSCALE) {
1138  // Top Left Corner
1139  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 5, width, 0);
1140  source++;
1141 
1142  // Upper Edge
1143  for (int x = 1; x < width - 1; x++) {
1144  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 1, width, 0);
1145  source++;
1146  }
1147 
1148  // Top Right Corner
1149  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 6, width, 0);
1150  source++;
1151 
1152  for (int y = 1; y < height - 1; y++) {
1153  // Left Edge
1154  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 3, width, 0);
1155  source++;
1156 
1157  // Middle
1158  for (int x = 1; x < width - 1; x++) {
1159  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 0, width, 0);
1160  source++;
1161  }
1162 
1163  // Right Edge
1164  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 4, width, 0);
1165  source++;
1166  }
1167 
1168  // Bottom Left Corner
1169  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 7, width, 0);
1170  source++;
1171 
1172  // Bottom Edge
1173  for (int x = 1; x < width - 1; x++) {
1174  *dest++ = ker_mul_3x3(source, kernel, kernel_total, 2, width, 0);
1175  source++;
1176  }
1177 
1178  // Bottom Right Corner
1179  *dest = ker_mul_3x3(source, kernel, kernel_total, 8, width, 0);
1180  }
1181  }
1182 }
float g
static const float offset[]
void image_copy(struct image_t *input, struct image_t *output)
Copy an image from inut to output This will only work if the formats are the same.
Definition: image.c:89
void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
This function takes previous padded pyramid level and outputs next level of pyramid without padding.
Definition: image.c:414
uint8_t ker_mul_3x3(uint8_t const *source, int_fast8_t const *kernel, uint8_t total, uint8_t setting, int width, int YUV)
Kernel multiplication for a 3 x 3 kernel.
Definition: image.c:1013
void image_convolution_3x3(struct image_t *input, struct image_t *output, int_fast8_t const *kernel, uint8_t kernel_total)
Image convolution for a 3x3 kernel.
Definition: image.c:1068
void image_switch(struct image_t *a, struct image_t *b)
This will switch image *a and *b This is faster as image_copy because it doesn't copy the whole image...
Definition: image.c:112
void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int *dy)
Get the gradient at a pixel location.
Definition: image.c:777
void image_draw_rectangle(struct image_t *img, int x_min, int x_max, int y_min, int y_max, uint8_t *color)
Draw a rectangle on the image.
Definition: image.c:849
void image_draw_crosshair(struct image_t *img, struct point_t *loc, const uint8_t *color, uint32_t size_crosshair)
Draw a cross-hair on the image.
Definition: image.c:891
void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to)
Draw a pink line on the image.
Definition: image.c:917
void set_color_yuv422(struct image_t *im, int x, int y, uint8_t Y, uint8_t U, uint8_t V)
Sets Y,U,V for a single pixel.
Definition: image.c:279
void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center, uint32_t subpixel_factor, uint8_t border_size)
This outputs a subpixel window image in grayscale Currently only works with Grayscale images as input...
Definition: image.c:491
void image_show_points_color(struct image_t *img, struct point_t *points, uint16_t points_cnt, uint8_t *color)
Show points in an image by coloring them through giving the pixels the maximum value.
Definition: image.c:701
void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, const uint8_t *color)
Draw a line on the image.
Definition: image.c:932
void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy)
Calculate the gradients using the following matrix: [0 -1 0; -1 0 1; 0 1 0].
Definition: image.c:549
void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size)
This function populates given array of image_t structs with wanted number of padded pyramids based on...
Definition: image.c:464
void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt)
Show points in an image by coloring them through giving the pixels the maximum value.
Definition: image.c:679
int check_color_yuv422(struct image_t *im, int x, int y, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
Checks the color of a single pixel in a YUV422 image.
Definition: image.c:237
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition: image.c:131
void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint8_t downsample)
Simplified high-speed low CPU downsample function without averaging downsample factor must be 1,...
Definition: image.c:318
uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff)
Calculate the difference between two images and return the error This will only work with grayscale i...
Definition: image.c:605
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
Definition: image.c:726
void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor, const uint8_t *color, const uint8_t *bad_color)
Shows the flow from a specific point to a new point This works on YUV422 and Grayscale images.
Definition: image.c:743
void image_free(struct image_t *img)
Free the image.
Definition: image.c:75
uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
Filter colors in an YUV422 image.
Definition: image.c:173
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
#define CACHE_LINE_LENGTH
Definition: image.c:33
void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
Calculate the G vector of an image gradient This is used for optical flow calculation.
Definition: image.c:573
void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
This function adds padding to input image by mirroring the edge image elements.
Definition: image.c:368
int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult)
Calculate the multiplication between two images and return the error This will only work with image g...
Definition: image.c:643
Image helper functions like resizing, color filter, converters...
struct timeval ts
The timestamp of creation.
Definition: image.h:48
uint32_t buf_size
The buffer size.
Definition: image.h:53
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
uint32_t x
The x coordinate of the point.
Definition: image.h:59
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:50
uint32_t y
The y coordinate of the point.
Definition: image.h:60
uint16_t h
Image height.
Definition: image.h:47
uint16_t w
Image width.
Definition: image.h:46
enum image_type type
The image type.
Definition: image.h:45
struct point_t pos
The original position the flow comes from in subpixels.
Definition: image.h:79
struct FloatEulers eulers
Euler Angles at time of image.
Definition: image.h:49
int32_t flow_y
The y direction flow in subpixels.
Definition: image.h:81
int32_t flow_x
The x direction flow in subpixels.
Definition: image.h:80
image_type
Definition: image.h:35
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
Definition: image.h:37
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
Definition: image.h:36
@ IMAGE_GRADIENT
An image gradient (int16 per pixel)
Definition: image.h:39
@ IMAGE_INT16
An image to hold disparity image data from openCV (int16 per pixel)
Definition: image.h:40
@ IMAGE_JPEG
An JPEG encoded image (not per pixel encoded)
Definition: image.h:38
Definition: image.h:78
Definition: image.h:44
Definition: image.h:58
efficient fixed-point optical-flow calculation
#define LARGE_FLOW_ERROR
Definition: lucas_kanade.h:37
static uint32_t idx
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
static uint8_t dest[]
Definition: w5100.c:99
float b
Definition: wedgebug.c:202