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