Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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  // aligned memory slightly speeds up any later copies
65 }
66 
71 void image_free(struct image_t *img)
72 {
73  if (img->buf != NULL) {
74  free(img->buf);
75  img->buf = NULL;
76  }
77 }
78 
85 void image_copy(struct image_t *input, struct image_t *output)
86 {
87  if (input->type != output->type) {
88  return;
89  }
90 
91  output->w = input->w;
92  output->h = input->h;
93  output->buf_size = input->buf_size;
94  output->ts = input->ts;
95  output->eulers = input->eulers;
96  output->pprz_ts = input->pprz_ts;
97 
98  memcpy(output->buf, input->buf, input->buf_size);
99 }
100 
108 void image_switch(struct image_t *a, struct image_t *b)
109 {
110  /* Remember everything from image a */
111  struct image_t old_a;
112  memcpy(&old_a, a, sizeof(struct image_t));
113 
114  /* Copy everything from b to a */
115  memcpy(a, b, sizeof(struct image_t));
116 
117  /* Copy everything from the remembered a to b */
118  memcpy(b, &old_a, sizeof(struct image_t));
119 }
120 
127 void image_to_grayscale(struct image_t *input, struct image_t *output)
128 {
129  uint8_t *source = input->buf;
130  uint8_t *dest = output->buf;
131  source++;
132 
133  // Copy the creation timestamp (stays the same)
134  output->ts = input->ts;
135  output->eulers = input->eulers;
136  output->pprz_ts = input->pprz_ts;
137 
138  // Copy the pixels
139  for (int y = 0; y < output->h; y++) {
140  for (int x = 0; x < output->w; x++) {
141  if (output->type == IMAGE_YUV422) {
142  *dest++ = 127; // U / V
143  }
144  *dest++ = *source; // Y
145  source += 2;
146  }
147  }
148 }
149 
162 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,
163  uint8_t u_M, uint8_t v_m, uint8_t v_M)
164 {
165  uint16_t cnt = 0;
166  uint8_t *source = (uint8_t *)input->buf;
167  uint8_t *dest = (uint8_t *)output->buf;
168 
169  // Copy the creation timestamp (stays the same)
170  output->ts = input->ts;
171 
172  // Go trough all the pixels
173  for (uint16_t y = 0; y < output->h; y++) {
174  for (uint16_t x = 0; x < output->w; x += 2) {
175  // Check if the color is inside the specified values
176  if (
177  (dest[1] >= y_m)
178  && (dest[1] <= y_M)
179  && (dest[0] >= u_m)
180  && (dest[0] <= u_M)
181  && (dest[2] >= v_m)
182  && (dest[2] <= v_M)
183  ) {
184  cnt ++;
185  // UYVY
186  dest[0] = 64; // U
187  dest[1] = source[1]; // Y
188  dest[2] = 255; // V
189  dest[3] = source[3]; // Y
190  } else {
191  // UYVY
192  char u = source[0] - 127;
193  u /= 4;
194  dest[0] = 127; // U
195  dest[1] = source[1]; // Y
196  u = source[2] - 127;
197  u /= 4;
198  dest[2] = 127; // V
199  dest[3] = source[3]; // Y
200  }
201 
202  // Go to the next 2 pixels
203  dest += 4;
204  source += 4;
205  }
206  }
207  return cnt;
208 }
209 
226 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)
227 {
228  // odd pixels are uy
229  // even pixels are vy
230  // adapt x, so that we always have u-channel in index 0:
231  if (x % 2 == 1) { x--; }
232 
233  // Is the pixel inside the image?
234  if (x < 0 || x >= im->w || y < 0 || y >= im->h) {
235  return 0;
236  }
237 
238  // Take the right place in the buffer:
239  uint8_t *buf = im->buf;
240  buf += 2 * (y * (im->w) + x); // each pixel has two bytes
241 
242  if (
243  (buf[1] >= y_m)
244  && (buf[1] <= y_M)
245  && (buf[0] >= u_m)
246  && (buf[0] <= u_M)
247  && (buf[2] >= v_m)
248  && (buf[2] <= v_M)
249  ) {
250  // the pixel passes:
251  return 1;
252  } else {
253  // the pixel does not:
254  return 0;
255  }
256 }
257 
268 void set_color_yuv422(struct image_t *im, int x, int y, uint8_t Y, uint8_t U, uint8_t V) {
269 
270  // odd pixels are uy
271  // even pixels are vy
272  // adapt x, so that we always have u-channel in index 0:
273  if (x % 2 == 1) { x--; }
274 
275  // Is the pixel inside the image?
276  if (x < 0 || x >= im->w || y < 0 || y >= im->h) {
277  return;
278  }
279 
280  // Take the right place in the buffer:
281  uint8_t *buf = im->buf;
282  buf += 2 * (y * (im->w) + x); // each pixel has two bytes
283 
284  buf[0] = U;
285  buf[1] = Y;
286  buf[2] = V;
287  buf[3] = Y;
288 }
289 
290 
307 void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint8_t downsample)
308 {
309  if (downsample < 1){
310  downsample = 1;
311  }
312 
313  // bound downsample is a power of 2
314  if((downsample & (downsample - 1)) != 0){
315  for(int8_t i = 7; i > 0; i--){
316  if(downsample & (1<<i)){
317  downsample &= (1<<(i));
318  break;
319  }
320  }
321  downsample *= 2;
322  }
323 
324  uint8_t *source = input->buf;
325  uint8_t *dest = output->buf;
326  uint16_t pixelskip = (downsample - 1) * 2;
327 
328  output->w = input->w / downsample;
329  output->h = input->h / downsample;
330  output->type = input->type;
331 
332  // Copy the creation timestamp (stays the same)
333  output->ts = input->ts;
334 
335  // Go through all the pixels
336  for (uint16_t y = 0; y < output->h; y++) {
337  for (uint16_t x = 0; x < output->w; x += 2) {
338  // YUYV
339  *dest++ = *source++; // U
340  *dest++ = *source++; // Y
341  *dest++ = *source++; // V
342  source += pixelskip;
343  *dest++ = *source++; // Y
344  source += pixelskip;
345  }
346  source += pixelskip * input->w;
347  }
348 }
349 
357 void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
358 {
359  // Create padded image based on input
360  image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
361 
362  uint8_t *input_buf = (uint8_t *)input->buf;
363  uint8_t *output_buf = (uint8_t *)output->buf;
364 
365  // Skip first `border_size` rows, iterate through next input->h rows
366  for (uint16_t i = border_size; i != (output->h - border_size); i++) {
367 
368  // Mirror first `border_size` columns
369  for (uint8_t j = 0; j != border_size; j++) {
370  output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
371  }
372 
373  // Copy corresponding row values from input image
374  memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
375 
376  // Mirror last `border_size` columns
377  for (uint8_t j = 0; j != border_size; j++) {
378  output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size - 1 - j];
379  }
380  }
381 
382  // Mirror first `border_size` and last `border_size` rows
383  for (uint8_t i = 0; i != border_size; i++) {
384  memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w],
385  sizeof(uint8_t) * output->w);
386  memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w],
387  &output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
388  }
389 }
390 
402 void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
403 {
404  // Create output image, new image size is half the size of input image without padding (border)
405  image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size) / 2, input->type);
406 
407  uint8_t *input_buf = (uint8_t *)input->buf;
408  uint8_t *output_buf = (uint8_t *)output->buf;
409 
410  uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
411  uint16_t w = input->w;
412  int32_t sum = 0;
413 
414  for (uint16_t i = 0; i != output->h; i++) {
415 
416  for (uint16_t j = 0; j != output->w; j++) {
417  row = border_size + 2 * i; // First skip border, then every second pixel
418  col = border_size + 2 * j;
419 
420  sum = 39 * (input_buf[(row - 2) * w + (col - 2)] + input_buf[(row - 2) * w + (col + 2)] +
421  input_buf[(row + 2) * w + (col - 2)] + input_buf[(row + 2) * w + (col + 2)]);
422  sum += 156 * (input_buf[(row - 2) * w + (col - 1)] + input_buf[(row - 2) * w + (col + 1)] +
423  input_buf[(row - 1) * w + (col + 2)] + input_buf[(row + 1) * w + (col - 2)]
424  + input_buf[(row + 1) * w + (col + 2)] + input_buf[(row + 2) * w + (col - 1)] + input_buf[(row + 2) * w + (col + 1)] +
425  input_buf[(row - 1) * w + (col - 2)]);
426  sum += 234 * (input_buf[(row - 2) * w + (col)] + input_buf[(row) * w + (col - 2)] +
427  input_buf[(row) * w + (col + 2)] + input_buf[(row + 2) * w + (col)]);
428  sum += 625 * (input_buf[(row - 1) * w + (col - 1)] + input_buf[(row - 1) * w + (col + 1)] +
429  input_buf[(row + 1) * w + (col - 1)] + input_buf[(row + 1) * w + (col + 1)]);
430  sum += 938 * (input_buf[(row - 1) * w + (col)] + input_buf[(row) * w + (col - 1)] +
431  input_buf[(row) * w + (col + 1)] + input_buf[(row + 1) * w + (col)]);
432  sum += 1406 * input_buf[(row) * w + (col)];
433 
434  output_buf[i * output->w + j] = sum / 10000;
435  }
436  }
437 }
438 
439 
449 void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size)
450 {
451  // Pad input image and save it as '0' pyramid level
452  image_add_border(input, &output_array[0], border_size);
453 
454  // Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
455  struct image_t temp;
456 
457  for (uint8_t i = 1; i != pyr_level + 1; i++) {
458  pyramid_next_level(&output_array[i - 1], &temp, border_size);
459  image_add_border(&temp, &output_array[i], border_size);
460  image_free(&temp);
461  }
462 }
463 
476 void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center,
477  uint32_t subpixel_factor, uint8_t border_size)
478 {
479  uint8_t *input_buf = (uint8_t *)input->buf;
480  uint8_t *output_buf = (uint8_t *)output->buf;
481 
482  // Calculate the window size
483  uint16_t half_window = output->w / 2;
484 
485  uint32_t subpixel_w = (input->w - 2) * subpixel_factor;
486  uint32_t subpixel_h = (input->h - 2) * subpixel_factor;
487 
488  // Go through the whole window size in normal coordinates
489  for (uint16_t i = 0; i < output->w; i++) {
490  for (uint16_t j = 0; j < output->h; j++) {
491  // Calculate the subpixel coordinate
492  uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor;
493  uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor;
494 
495  BoundUpper(x, subpixel_w);
496  BoundUpper(y, subpixel_h);
497 
498  // Calculate the original pixel coordinate
499  uint16_t orig_x = x / subpixel_factor;
500  uint16_t orig_y = y / subpixel_factor;
501 
502  // Calculate top left (in subpixel coordinates)
503  uint32_t tl_x = orig_x * subpixel_factor;
504  uint32_t tl_y = orig_y * subpixel_factor;
505 
506  // Check if it is the top left pixel
507  if (tl_x == x && tl_y == y) {
508  output_buf[output->w * j + i] = input_buf[input->w * orig_y + orig_x];
509  } else {
510  // Calculate the difference from the top left
511  uint32_t alpha_x = (x - tl_x);
512  uint32_t alpha_y = (y - tl_y);
513 
514  // Blend from the 4 surrounding pixels
515  uint32_t blend = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + orig_x];
516  blend += alpha_x * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + (orig_x + 1)];
517  blend += (subpixel_factor - alpha_x) * alpha_y * input_buf[input->w * (orig_y + 1) + orig_x];
518  blend += alpha_x * alpha_y * input_buf[input->w * (orig_y + 1) + (orig_x + 1)];
519 
520  // Set the normalized pixel blend
521  output_buf[output->w * j + i] = blend / (subpixel_factor * subpixel_factor);
522  }
523  }
524  }
525 }
526 
534 void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy)
535 {
536  // Fetch the buffers in the correct format
537  uint8_t *input_buf = (uint8_t *)input->buf;
538  int16_t *dx_buf = (int16_t *)dx->buf;
539  int16_t *dy_buf = (int16_t *)dy->buf;
540 
541  // Go trough all pixels except the borders
542  for (uint16_t x = 1; x < input->w - 1; x++) {
543  for (uint16_t y = 1; y < input->h - 1; y++) {
544  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];
545  dy_buf[(y - 1)*dy->w + (x - 1)] = (int16_t)input_buf[(y + 1) * input->w + x] - (int16_t)
546  input_buf[(y - 1) * input->w + x];
547  }
548  }
549 }
550 
558 void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
559 {
560  int32_t sum_dxx = 0, sum_dxy = 0, sum_dyy = 0;
561 
562  // Fetch the buffers in the correct format
563  int16_t *dx_buf = (int16_t *)dx->buf;
564  int16_t *dy_buf = (int16_t *)dy->buf;
565 
566  // Calculate the different sums
567  for (uint16_t x = 0; x < dx->w; x++) {
568  for (uint16_t y = 0; y < dy->h; y++) {
569  sum_dxx += ((int32_t)dx_buf[y * dx->w + x] * dx_buf[y * dx->w + x]);
570  sum_dxy += ((int32_t)dx_buf[y * dx->w + x] * dy_buf[y * dy->w + x]);
571  sum_dyy += ((int32_t)dy_buf[y * dy->w + x] * dy_buf[y * dy->w + x]);
572  }
573  }
574 
575  // output the G vector
576  g[0] = sum_dxx / 255;
577  g[1] = sum_dxy / 255;
578  g[2] = g[1];
579  g[3] = sum_dyy / 255;
580 }
581 
590 uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff)
591 {
592  uint32_t sum_diff2 = 0;
593  int16_t *diff_buf = NULL;
594 
595  // Fetch the buffers in the correct format
596  uint8_t *img_a_buf = (uint8_t *)img_a->buf;
597  uint8_t *img_b_buf = (uint8_t *)img_b->buf;
598 
599  // If we want the difference image back
600  if (diff != NULL) {
601  diff_buf = (int16_t *)diff->buf;
602  }
603 
604  // Go trough the imagge pixels and calculate the difference
605  for (uint16_t x = 0; x < img_b->w; x++) {
606  for (uint16_t y = 0; y < img_b->h; y++) {
607  int16_t diff_c = img_a_buf[(y + 1) * img_a->w + (x + 1)] - img_b_buf[y * img_b->w + x];
608  sum_diff2 += diff_c * diff_c;
609 
610  // Set the difference image
611  if (diff_buf != NULL) {
612  diff_buf[y * diff->w + x] = diff_c;
613  }
614  }
615  }
616 
617  return sum_diff2;
618 }
619 
628 int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult)
629 {
630  int32_t sum = 0;
631  int16_t *img_a_buf = (int16_t *)img_a->buf;
632  int16_t *img_b_buf = (int16_t *)img_b->buf;
633  int16_t *mult_buf = NULL;
634 
635  // When we want an output
636  if (mult != NULL) {
637  mult_buf = (int16_t *)mult->buf;
638  }
639 
640  // Calculate the multiplication
641  for (uint16_t x = 0; x < img_a->w; x++) {
642  for (uint16_t y = 0; y < img_a->h; y++) {
643  int32_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
644  sum += mult_c;
645 
646  // Set the difference image
647  if (mult_buf != NULL) {
648  mult_buf[y * mult->w + x] = mult_c;
649  }
650  }
651  }
652 
653  return sum;
654 }
655 
664 void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt)
665 {
666  uint8_t color[4];
667  color[0] = 255;
668  color[1] = 255;
669  color[2] = 255;
670  color[3] = 255;
671 
672  image_show_points_color(img, points, points_cnt, color);
673 
674 }
675 
686 void image_show_points_color(struct image_t *img, struct point_t *points, uint16_t points_cnt, uint8_t *color)
687 {
688  uint8_t *img_buf = (uint8_t *)img->buf;
689  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
690 
691  int cross_hair = 1;
692  int size_crosshair = 5;
693 
694  // Go trough all points and color them
695  for (int i = 0; i < points_cnt; i++) {
696  if (!cross_hair) {
697  uint32_t idx = pixel_width * points[i].y * img->w + points[i].x * pixel_width;
698  img_buf[idx] = 255;
699 
700  // YUV422 consists of 2 pixels
701  if (img->type == IMAGE_YUV422) {
702  idx++;
703  img_buf[idx] = 255;
704  }
705  } else {
706  image_draw_crosshair(img, &(points[i]), color, size_crosshair);
707  }
708  }
709 }
710 
711 void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
712 {
713  static uint8_t color[4] = {255, 255, 255, 255};
714  static uint8_t bad_color[4] = {0, 0, 0, 0};
715  image_show_flow_color(img, vectors, points_cnt, subpixel_factor, color, bad_color);
716 }
717 
728 void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor,
729  const uint8_t *color, const uint8_t *bad_color)
730 {
731  static int size_crosshair = 5;
732 
733  // Go through all the points
734  for (uint16_t i = 0; i < points_cnt; i++) {
735  // Draw a line from the original position with the flow vector
736  struct point_t from = {
737  .x = vectors[i].pos.x / subpixel_factor,
738  .y = vectors[i].pos.y / subpixel_factor
739  };
740  struct point_t to = {
741  .x = (uint32_t)roundf(((float)vectors[i].pos.x + vectors[i].flow_x) / subpixel_factor),
742  .y = (uint32_t)roundf(((float)vectors[i].pos.y + vectors[i].flow_y) / subpixel_factor)
743  };
744 
745  if (vectors[i].error >= LARGE_FLOW_ERROR) {
746  image_draw_crosshair(img, &to, bad_color, size_crosshair);
747  image_draw_line_color(img, &from, &to, bad_color);
748  } else {
749  image_draw_crosshair(img, &to, color, size_crosshair);
750  image_draw_line_color(img, &from, &to, color);
751  }
752  }
753 }
762 void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int *dy)
763 {
764  // create the simple and sobel filter only once:
765 
766  int gradient_x, gradient_y, index;
767  gradient_x = 0;
768  gradient_y = 0;
769 
770  // get image buffer and take into account YUV vs. grayscale:
771  uint8_t *img_buf = (uint8_t *)img->buf;
772  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
773  uint8_t add_ind = pixel_width - 1;
774 
775  // check if all pixels will fall in the image:
776  if (loc->x >= 1 && (loc->x + 1) < img->w && loc->y >= 1 && (loc->y + 1) < img->h) {
777  if (method == 0) {
778 
779  // *************
780  // Simple method
781  // *************
782 
783  // dx:
784  index = loc->y * img->w * pixel_width + (loc->x - 1) * pixel_width;
785  gradient_x -= (int) img_buf[index + add_ind];
786  index = loc->y * img->w * pixel_width + (loc->x + 1) * pixel_width;
787  gradient_x += (int) img_buf[index + add_ind];
788  // dy:
789  index = (loc->y - 1) * img->w * pixel_width + loc->x * pixel_width;
790  gradient_y -= (int) img_buf[index + add_ind];
791  index = (loc->y + 1) * img->w * pixel_width + loc->x * pixel_width;
792  gradient_y += (int) img_buf[index + add_ind];
793  } else {
794 
795  // *****
796  // Sobel
797  // *****
798  static int Sobel[9] = { -1, 0, 1, -2, 0, 2, -1, 0, 1};
799  static int total_sobel = 8;
800 
801  int filt_ind_y = 0;
802  int filt_ind_x;
803  for (int x = -1; x <= 1; x++) {
804  for (int y = -1; y <= 1; y++) {
805  index = (loc->y + y) * img->w * pixel_width + (loc->x + x) * pixel_width;
806  if (x != 0) {
807  filt_ind_x = (x + 1) % 3 + (y + 1) * 3;
808  gradient_x += Sobel[filt_ind_x] * (int) img_buf[index + add_ind];
809  }
810  if (y != 0) {
811  gradient_y += Sobel[filt_ind_y] * (int) img_buf[index + add_ind];
812  }
813  filt_ind_y++;
814  }
815  }
816  gradient_x /= total_sobel;
817  }
818  }
819 
820  // TODO: more efficient would be to use dx, dy directly:
821  (*dx) = gradient_x;
822  (*dy) = gradient_y;
823 }
824 
834 void image_draw_rectangle(struct image_t *img, int x_min, int x_max, int y_min, int y_max, uint8_t *color)
835 {
836  struct point_t from, to;
837 
838  // bottom from left to right:
839  from.x = x_min;
840  from.y = y_min;
841  to.x = x_max;
842  to.y = y_min;
843  image_draw_line_color(img, &from, &to, color);
844 
845  // from bottom right to top right:
846  from.x = x_max;
847  from.y = y_min;
848  to.x = x_max;
849  to.y = y_max;
850  image_draw_line_color(img, &from, &to, color);
851 
852  // from top right to top left:
853  from.x = x_max;
854  from.y = y_max;
855  to.x = x_min;
856  to.y = y_max;
857  image_draw_line_color(img, &from, &to, color);
858 
859  // from top left to bottom left:
860  from.x = x_min;
861  from.y = y_max;
862  to.x = x_min;
863  to.y = y_min;
864  image_draw_line_color(img, &from, &to, color);
865 
866 }
867 
876 void image_draw_crosshair(struct image_t *img, struct point_t *loc, const uint8_t *color, uint32_t size_crosshair)
877 {
878  struct point_t from, to;
879 
880  if (loc->x >= size_crosshair && loc->x < img->w - size_crosshair
881  && loc->y >= size_crosshair && loc->y < img->h - size_crosshair) {
882  // draw the lines:
883  from.x = loc->x - size_crosshair;
884  from.y = loc->y;
885  to.x = loc->x + size_crosshair;
886  to.y = loc->y;
887  image_draw_line_color(img, &from, &to, color);
888  from.x = loc->x;
889  from.y = loc->y - size_crosshair;
890  to.x = loc->x;
891  to.y = loc->y + size_crosshair;
892  image_draw_line_color(img, &from, &to, color);
893  }
894 }
895 
902 void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to)
903 {
904  static uint8_t color[4] = {255, 255, 255, 255};
905  image_draw_line_color(img, from, to, color);
906 }
907 
908 
917 void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, const uint8_t *color)
918 {
919  int xerr = 0, yerr = 0;
920  uint8_t *img_buf = (uint8_t *)img->buf;
921  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
922  uint16_t startx = from->x;
923  uint16_t starty = from->y;
924 
925  uint8_t temp_color[4] = {color[0], color[1], color[2], color[3]};
926 
927  /* compute the distances in both directions */
928  int32_t delta_x = to->x - from->x;
929  int32_t delta_y = to->y - from->y;
930 
931  /* Compute the direction of the increment,
932  an increment of 0 means either a horizontal or vertical
933  line.
934  */
935  int8_t incx, incy;
936  if (delta_x > 0) { incx = 1; }
937  else if (delta_x == 0) { incx = 0; }
938  else { incx = -1; }
939 
940  if (delta_y > 0) { incy = 1; }
941  else if (delta_y == 0) { incy = 0; }
942  else { incy = -1; }
943 
944  /* determine which distance is greater */
945  uint16_t distance = 0;
946  delta_x = abs(delta_x);
947  delta_y = abs(delta_y);
948  if (delta_x > delta_y) { distance = delta_x * 20; }
949  else { distance = delta_y * 20; }
950 
951  /* draw the line */
952  for (uint16_t t = 0; /* starty >= 0 && */ starty < img->h && /* startx >= 0 && */ startx < img->w
953  && t <= distance + 1; t++) {
954 
955  // depending on startx being odd or even, we first have to set U or V
956  if (startx % 2 == 1) {
957  temp_color[0] = color[2];
958  temp_color[2] = color[0];
959  } else {
960  temp_color[0] = color[0];
961  temp_color[2] = color[2];
962  }
963  uint32_t buf_loc = img->w * pixel_width * starty + startx * pixel_width;
964  img_buf[buf_loc] = temp_color[0]; // u (when startx even)
965 
966  if (img->type == IMAGE_YUV422) {
967  img_buf[buf_loc + 1] = temp_color[1]; // y1
968 
969  if (startx + 1 < img->w) {
970  img_buf[buf_loc + 2] = temp_color[2]; // v (when startx even)
971  img_buf[buf_loc + 3] = temp_color[3]; // y2
972  }
973  }
974 
975  xerr += delta_x;
976  yerr += delta_y;
977  if (xerr > distance) {
978  xerr -= distance;
979  startx += incx;
980  }
981  if (yerr > distance) {
982  yerr -= distance;
983  starty += incy;
984  }
985  }
986 }
unsigned short uint16_t
Definition: types.h:16
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:534
static uint32_t idx
static uint8_t dest[]
Definition: w5100.c:99
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:268
uint32_t buf_size
The buffer size.
Definition: image.h:53
image_type
Definition: image.h:35
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:590
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:108
void image_free(struct image_t *img)
Free the image.
Definition: image.c:71
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:917
#define LARGE_FLOW_ERROR
Definition: lucas_kanade.h:37
int32_t flow_y
The y direction flow in subpixels.
Definition: image.h:81
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
Definition: image.h:44
Definition: image.h:78
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:902
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:728
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:85
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:664
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
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
Definition: image.c:711
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:834
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:449
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:402
Image helper functions like resizing, color filter, converters...
An image to hold disparity image data from openCV (int16 per pixel)
Definition: image.h:40
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:762
uint16_t w
Image width.
Definition: image.h:46
unsigned long uint32_t
Definition: types.h:18
uint16_t h
Image height.
Definition: image.h:47
signed short int16_t
Definition: types.h:17
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:357
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
float b
Definition: wedgebug.c:202
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:876
struct point_t pos
The original position the flow comes from in subpixels.
Definition: image.h:79
uint32_t y
The y coordinate of the point.
Definition: image.h:60
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition: image.c:127
Definition: image.h:58
#define CACHE_LINE_LENGTH
Definition: image.c:33
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
efficient fixed-point optical-flow calculation
struct timeval ts
The timestamp of creation.
Definition: image.h:48
UYVY format (uint16 per pixel)
Definition: image.h:36
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:558
signed char int8_t
Definition: types.h:15
An image gradient (int16 per pixel)
Definition: image.h:39
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:162
int32_t flow_x
The x direction flow in subpixels.
Definition: image.h:80
struct FloatEulers eulers
Euler Angles at time of image.
Definition: image.h:49
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:628
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:226
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:476
An JPEG encoded image (not per pixel encoded)
Definition: image.h:38
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:307
enum image_type type
The image type.
Definition: image.h:45
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:686