Paparazzi UAS  v6.0_unstable-53-gfe8bbd3-dirty
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 #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  for (int y = 0; y < output->h; y++) {
144  for (int x = 0; x < output->w; x++) {
145  if (output->type == IMAGE_YUV422) {
146  *dest++ = 127; // U / V
147  }
148  *dest++ = *source; // Y
149  source += 2;
150  }
151  }
152 }
153 
166 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,
167  uint8_t u_M, uint8_t v_m, uint8_t v_M)
168 {
169  uint16_t cnt = 0;
170  uint8_t *source = (uint8_t *)input->buf;
171  uint8_t *dest = (uint8_t *)output->buf;
172 
173  // Copy the creation timestamp (stays the same)
174  output->ts = input->ts;
175 
176  // Go trough all the pixels
177  for (uint16_t y = 0; y < output->h; y++) {
178  for (uint16_t x = 0; x < output->w; x += 2) {
179  // Check if the color is inside the specified values
180  if (
181  (dest[1] >= y_m)
182  && (dest[1] <= y_M)
183  && (dest[0] >= u_m)
184  && (dest[0] <= u_M)
185  && (dest[2] >= v_m)
186  && (dest[2] <= v_M)
187  ) {
188  cnt ++;
189  // UYVY
190  dest[0] = 64; // U
191  dest[1] = source[1]; // Y
192  dest[2] = 255; // V
193  dest[3] = source[3]; // Y
194  } else {
195  // UYVY
196  char u = source[0] - 127;
197  u /= 4;
198  dest[0] = 127; // U
199  dest[1] = source[1]; // Y
200  u = source[2] - 127;
201  u /= 4;
202  dest[2] = 127; // V
203  dest[3] = source[3]; // Y
204  }
205 
206  // Go to the next 2 pixels
207  dest += 4;
208  source += 4;
209  }
210  }
211  return cnt;
212 }
213 
230 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)
231 {
232  // odd pixels are uy
233  // even pixels are vy
234  // adapt x, so that we always have u-channel in index 0:
235  if (x % 2 == 1) { x--; }
236 
237  // Is the pixel inside the image?
238  if (x < 0 || x >= im->w || y < 0 || y >= im->h) {
239  return 0;
240  }
241 
242  // Take the right place in the buffer:
243  uint8_t *buf = im->buf;
244  buf += 2 * (y * (im->w) + x); // each pixel has two bytes
245 
246  if (
247  (buf[1] >= y_m)
248  && (buf[1] <= y_M)
249  && (buf[0] >= u_m)
250  && (buf[0] <= u_M)
251  && (buf[2] >= v_m)
252  && (buf[2] <= v_M)
253  ) {
254  // the pixel passes:
255  return 1;
256  } else {
257  // the pixel does not:
258  return 0;
259  }
260 }
261 
272 void set_color_yuv422(struct image_t *im, int x, int y, uint8_t Y, uint8_t U, uint8_t V) {
273 
274  // odd pixels are uy
275  // even pixels are vy
276  // adapt x, so that we always have u-channel in index 0:
277  if (x % 2 == 1) { x--; }
278 
279  // Is the pixel inside the image?
280  if (x < 0 || x >= im->w || y < 0 || y >= im->h) {
281  return;
282  }
283 
284  // Take the right place in the buffer:
285  uint8_t *buf = im->buf;
286  buf += 2 * (y * (im->w) + x); // each pixel has two bytes
287 
288  buf[0] = U;
289  buf[1] = Y;
290  buf[2] = V;
291  buf[3] = Y;
292 }
293 
294 
311 void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint8_t downsample)
312 {
313  if (downsample < 1){
314  downsample = 1;
315  }
316 
317  // bound downsample is a power of 2
318  if((downsample & (downsample - 1)) != 0){
319  for(int8_t i = 7; i > 0; i--){
320  if(downsample & (1<<i)){
321  downsample &= (1<<(i));
322  break;
323  }
324  }
325  downsample *= 2;
326  }
327 
328  uint8_t *source = input->buf;
329  uint8_t *dest = output->buf;
330  uint16_t pixelskip = (downsample - 1) * 2;
331 
332  output->w = input->w / downsample;
333  output->h = input->h / downsample;
334  output->type = input->type;
335 
336  // Copy the creation timestamp (stays the same)
337  output->ts = input->ts;
338 
339  // Go through all the pixels
340  for (uint16_t y = 0; y < output->h; y++) {
341  for (uint16_t x = 0; x < output->w; x += 2) {
342  // YUYV
343  *dest++ = *source++; // U
344  *dest++ = *source++; // Y
345  *dest++ = *source++; // V
346  source += pixelskip;
347  *dest++ = *source++; // Y
348  source += pixelskip;
349  }
350  source += pixelskip * input->w;
351  }
352 }
353 
361 void image_add_border(struct image_t *input, struct image_t *output, uint8_t border_size)
362 {
363  // Create padded image based on input
364  image_create(output, input->w + 2 * border_size, input->h + 2 * border_size, input->type);
365 
366  uint8_t *input_buf = (uint8_t *)input->buf;
367  uint8_t *output_buf = (uint8_t *)output->buf;
368 
369  // Skip first `border_size` rows, iterate through next input->h rows
370  for (uint16_t i = border_size; i != (output->h - border_size); i++) {
371 
372  // Mirror first `border_size` columns
373  for (uint8_t j = 0; j != border_size; j++) {
374  output_buf[i * output->w + (border_size - 1 - j)] = input_buf[(i - border_size) * input->w + j];
375  }
376 
377  // Copy corresponding row values from input image
378  memcpy(&output_buf[i * output->w + border_size], &input_buf[(i - border_size) * input->w], sizeof(uint8_t) * input->w);
379 
380  // Mirror last `border_size` columns
381  for (uint8_t j = 0; j != border_size; j++) {
382  output_buf[i * output->w + output->w - border_size + j] = output_buf[i * output->w + output->w - border_size - 1 - j];
383  }
384  }
385 
386  // Mirror first `border_size` and last `border_size` rows
387  for (uint8_t i = 0; i != border_size; i++) {
388  memcpy(&output_buf[(border_size - 1) * output->w - i * output->w], &output_buf[border_size * output->w + i * output->w],
389  sizeof(uint8_t) * output->w);
390  memcpy(&output_buf[(output->h - border_size) * output->w + i * output->w],
391  &output_buf[(output->h - border_size - 1) * output->w - i * output->w], sizeof(uint8_t) * output->w);
392  }
393 }
394 
406 void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size)
407 {
408  // Create output image, new image size is half the size of input image without padding (border)
409  image_create(output, (input->w + 1 - 2 * border_size) / 2, (input->h + 1 - 2 * border_size) / 2, input->type);
410 
411  uint8_t *input_buf = (uint8_t *)input->buf;
412  uint8_t *output_buf = (uint8_t *)output->buf;
413 
414  uint16_t row, col; // coordinates of the central pixel; pixel being calculated in input matrix; center of filer matrix
415  uint16_t w = input->w;
416  int32_t sum = 0;
417 
418  for (uint16_t i = 0; i != output->h; i++) {
419 
420  for (uint16_t j = 0; j != output->w; j++) {
421  row = border_size + 2 * i; // First skip border, then every second pixel
422  col = border_size + 2 * j;
423 
424  sum = 39 * (input_buf[(row - 2) * w + (col - 2)] + input_buf[(row - 2) * w + (col + 2)] +
425  input_buf[(row + 2) * w + (col - 2)] + input_buf[(row + 2) * w + (col + 2)]);
426  sum += 156 * (input_buf[(row - 2) * w + (col - 1)] + input_buf[(row - 2) * w + (col + 1)] +
427  input_buf[(row - 1) * w + (col + 2)] + input_buf[(row + 1) * w + (col - 2)]
428  + input_buf[(row + 1) * w + (col + 2)] + input_buf[(row + 2) * w + (col - 1)] + input_buf[(row + 2) * w + (col + 1)] +
429  input_buf[(row - 1) * w + (col - 2)]);
430  sum += 234 * (input_buf[(row - 2) * w + (col)] + input_buf[(row) * w + (col - 2)] +
431  input_buf[(row) * w + (col + 2)] + input_buf[(row + 2) * w + (col)]);
432  sum += 625 * (input_buf[(row - 1) * w + (col - 1)] + input_buf[(row - 1) * w + (col + 1)] +
433  input_buf[(row + 1) * w + (col - 1)] + input_buf[(row + 1) * w + (col + 1)]);
434  sum += 938 * (input_buf[(row - 1) * w + (col)] + input_buf[(row) * w + (col - 1)] +
435  input_buf[(row) * w + (col + 1)] + input_buf[(row + 1) * w + (col)]);
436  sum += 1406 * input_buf[(row) * w + (col)];
437 
438  output_buf[i * output->w + j] = sum / 10000;
439  }
440  }
441 }
442 
443 
453 void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size)
454 {
455  // Pad input image and save it as '0' pyramid level
456  image_add_border(input, &output_array[0], border_size);
457 
458  // Temporary holds 'i' level version of original image to be padded and saved as 'i' pyramid level
459  struct image_t temp;
460 
461  for (uint8_t i = 1; i != pyr_level + 1; i++) {
462  pyramid_next_level(&output_array[i - 1], &temp, border_size);
463  image_add_border(&temp, &output_array[i], border_size);
464  image_free(&temp);
465  }
466 }
467 
480 void image_subpixel_window(struct image_t *input, struct image_t *output, struct point_t *center,
481  uint32_t subpixel_factor, uint8_t border_size)
482 {
483  uint8_t *input_buf = (uint8_t *)input->buf;
484  uint8_t *output_buf = (uint8_t *)output->buf;
485 
486  // Calculate the window size
487  uint16_t half_window = output->w / 2;
488 
489  uint32_t subpixel_w = (input->w - 2) * subpixel_factor;
490  uint32_t subpixel_h = (input->h - 2) * subpixel_factor;
491 
492  // Go through the whole window size in normal coordinates
493  for (uint16_t i = 0; i < output->w; i++) {
494  for (uint16_t j = 0; j < output->h; j++) {
495  // Calculate the subpixel coordinate
496  uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor;
497  uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor;
498 
499  BoundUpper(x, subpixel_w);
500  BoundUpper(y, subpixel_h);
501 
502  // Calculate the original pixel coordinate
503  uint16_t orig_x = x / subpixel_factor;
504  uint16_t orig_y = y / subpixel_factor;
505 
506  // Calculate top left (in subpixel coordinates)
507  uint32_t tl_x = orig_x * subpixel_factor;
508  uint32_t tl_y = orig_y * subpixel_factor;
509 
510  // Check if it is the top left pixel
511  if (tl_x == x && tl_y == y) {
512  output_buf[output->w * j + i] = input_buf[input->w * orig_y + orig_x];
513  } else {
514  // Calculate the difference from the top left
515  uint32_t alpha_x = (x - tl_x);
516  uint32_t alpha_y = (y - tl_y);
517 
518  // Blend from the 4 surrounding pixels
519  uint32_t blend = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + orig_x];
520  blend += alpha_x * (subpixel_factor - alpha_y) * input_buf[input->w * orig_y + (orig_x + 1)];
521  blend += (subpixel_factor - alpha_x) * alpha_y * input_buf[input->w * (orig_y + 1) + orig_x];
522  blend += alpha_x * alpha_y * input_buf[input->w * (orig_y + 1) + (orig_x + 1)];
523 
524  // Set the normalized pixel blend
525  output_buf[output->w * j + i] = blend / (subpixel_factor * subpixel_factor);
526  }
527  }
528  }
529 }
530 
538 void image_gradients(struct image_t *input, struct image_t *dx, struct image_t *dy)
539 {
540  // Fetch the buffers in the correct format
541  uint8_t *input_buf = (uint8_t *)input->buf;
542  int16_t *dx_buf = (int16_t *)dx->buf;
543  int16_t *dy_buf = (int16_t *)dy->buf;
544 
545  // Go trough all pixels except the borders
546  for (uint16_t x = 1; x < input->w - 1; x++) {
547  for (uint16_t y = 1; y < input->h - 1; y++) {
548  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];
549  dy_buf[(y - 1)*dy->w + (x - 1)] = (int16_t)input_buf[(y + 1) * input->w + x] - (int16_t)
550  input_buf[(y - 1) * input->w + x];
551  }
552  }
553 }
554 
562 void image_calculate_g(struct image_t *dx, struct image_t *dy, int32_t *g)
563 {
564  int32_t sum_dxx = 0, sum_dxy = 0, sum_dyy = 0;
565 
566  // Fetch the buffers in the correct format
567  int16_t *dx_buf = (int16_t *)dx->buf;
568  int16_t *dy_buf = (int16_t *)dy->buf;
569 
570  // Calculate the different sums
571  for (uint16_t x = 0; x < dx->w; x++) {
572  for (uint16_t y = 0; y < dy->h; y++) {
573  sum_dxx += ((int32_t)dx_buf[y * dx->w + x] * dx_buf[y * dx->w + x]);
574  sum_dxy += ((int32_t)dx_buf[y * dx->w + x] * dy_buf[y * dy->w + x]);
575  sum_dyy += ((int32_t)dy_buf[y * dy->w + x] * dy_buf[y * dy->w + x]);
576  }
577  }
578 
579  // output the G vector
580  g[0] = sum_dxx / 255;
581  g[1] = sum_dxy / 255;
582  g[2] = g[1];
583  g[3] = sum_dyy / 255;
584 }
585 
594 uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct image_t *diff)
595 {
596  uint32_t sum_diff2 = 0;
597  int16_t *diff_buf = NULL;
598 
599  // Fetch the buffers in the correct format
600  uint8_t *img_a_buf = (uint8_t *)img_a->buf;
601  uint8_t *img_b_buf = (uint8_t *)img_b->buf;
602 
603  // If we want the difference image back
604  if (diff != NULL) {
605  diff_buf = (int16_t *)diff->buf;
606  }
607 
608  // Go trough the imagge pixels and calculate the difference
609  for (uint16_t x = 0; x < img_b->w; x++) {
610  for (uint16_t y = 0; y < img_b->h; y++) {
611  int16_t diff_c = img_a_buf[(y + 1) * img_a->w + (x + 1)] - img_b_buf[y * img_b->w + x];
612  sum_diff2 += diff_c * diff_c;
613 
614  // Set the difference image
615  if (diff_buf != NULL) {
616  diff_buf[y * diff->w + x] = diff_c;
617  }
618  }
619  }
620 
621  return sum_diff2;
622 }
623 
632 int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult)
633 {
634  int32_t sum = 0;
635  int16_t *img_a_buf = (int16_t *)img_a->buf;
636  int16_t *img_b_buf = (int16_t *)img_b->buf;
637  int16_t *mult_buf = NULL;
638 
639  // When we want an output
640  if (mult != NULL) {
641  mult_buf = (int16_t *)mult->buf;
642  }
643 
644  // Calculate the multiplication
645  for (uint16_t x = 0; x < img_a->w; x++) {
646  for (uint16_t y = 0; y < img_a->h; y++) {
647  int32_t mult_c = img_a_buf[y * img_a->w + x] * img_b_buf[y * img_b->w + x];
648  sum += mult_c;
649 
650  // Set the difference image
651  if (mult_buf != NULL) {
652  mult_buf[y * mult->w + x] = mult_c;
653  }
654  }
655  }
656 
657  return sum;
658 }
659 
668 void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt)
669 {
670  uint8_t color[4];
671  color[0] = 255;
672  color[1] = 255;
673  color[2] = 255;
674  color[3] = 255;
675 
676  image_show_points_color(img, points, points_cnt, color);
677 
678 }
679 
690 void image_show_points_color(struct image_t *img, struct point_t *points, uint16_t points_cnt, uint8_t *color)
691 {
692  uint8_t *img_buf = (uint8_t *)img->buf;
693  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
694 
695  int cross_hair = 1;
696  int size_crosshair = 5;
697 
698  // Go trough all points and color them
699  for (int i = 0; i < points_cnt; i++) {
700  if (!cross_hair) {
701  uint32_t idx = pixel_width * points[i].y * img->w + points[i].x * pixel_width;
702  img_buf[idx] = 255;
703 
704  // YUV422 consists of 2 pixels
705  if (img->type == IMAGE_YUV422) {
706  idx++;
707  img_buf[idx] = 255;
708  }
709  } else {
710  image_draw_crosshair(img, &(points[i]), color, size_crosshair);
711  }
712  }
713 }
714 
715 void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
716 {
717  static uint8_t color[4] = {255, 255, 255, 255};
718  static uint8_t bad_color[4] = {0, 0, 0, 0};
719  image_show_flow_color(img, vectors, points_cnt, subpixel_factor, color, bad_color);
720 }
721 
732 void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor,
733  const uint8_t *color, const uint8_t *bad_color)
734 {
735  static int size_crosshair = 5;
736 
737  // Go through all the points
738  for (uint16_t i = 0; i < points_cnt; i++) {
739  // Draw a line from the original position with the flow vector
740  struct point_t from = {
741  .x = vectors[i].pos.x / subpixel_factor,
742  .y = vectors[i].pos.y / subpixel_factor
743  };
744  struct point_t to = {
745  .x = (uint32_t)roundf(((float)vectors[i].pos.x + vectors[i].flow_x) / subpixel_factor),
746  .y = (uint32_t)roundf(((float)vectors[i].pos.y + vectors[i].flow_y) / subpixel_factor)
747  };
748 
749  if (vectors[i].error >= LARGE_FLOW_ERROR) {
750  image_draw_crosshair(img, &to, bad_color, size_crosshair);
751  image_draw_line_color(img, &from, &to, bad_color);
752  } else {
753  image_draw_crosshair(img, &to, color, size_crosshair);
754  image_draw_line_color(img, &from, &to, color);
755  }
756  }
757 }
766 void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int *dy)
767 {
768  // create the simple and sobel filter only once:
769 
770  int gradient_x, gradient_y, index;
771  gradient_x = 0;
772  gradient_y = 0;
773 
774  // get image buffer and take into account YUV vs. grayscale:
775  uint8_t *img_buf = (uint8_t *)img->buf;
776  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
777  uint8_t add_ind = pixel_width - 1;
778 
779  // check if all pixels will fall in the image:
780  if (loc->x >= 1 && (loc->x + 1) < img->w && loc->y >= 1 && (loc->y + 1) < img->h) {
781  if (method == 0) {
782 
783  // *************
784  // Simple method
785  // *************
786 
787  // dx:
788  index = loc->y * img->w * pixel_width + (loc->x - 1) * pixel_width;
789  gradient_x -= (int) img_buf[index + add_ind];
790  index = loc->y * img->w * pixel_width + (loc->x + 1) * pixel_width;
791  gradient_x += (int) img_buf[index + add_ind];
792  // dy:
793  index = (loc->y - 1) * img->w * pixel_width + loc->x * pixel_width;
794  gradient_y -= (int) img_buf[index + add_ind];
795  index = (loc->y + 1) * img->w * pixel_width + loc->x * pixel_width;
796  gradient_y += (int) img_buf[index + add_ind];
797  } else {
798 
799  // *****
800  // Sobel
801  // *****
802  static int Sobel[9] = { -1, 0, 1, -2, 0, 2, -1, 0, 1};
803  static int total_sobel = 8;
804 
805  int filt_ind_y = 0;
806  int filt_ind_x;
807  for (int x = -1; x <= 1; x++) {
808  for (int y = -1; y <= 1; y++) {
809  index = (loc->y + y) * img->w * pixel_width + (loc->x + x) * pixel_width;
810  if (x != 0) {
811  filt_ind_x = (x + 1) % 3 + (y + 1) * 3;
812  gradient_x += Sobel[filt_ind_x] * (int) img_buf[index + add_ind];
813  }
814  if (y != 0) {
815  gradient_y += Sobel[filt_ind_y] * (int) img_buf[index + add_ind];
816  }
817  filt_ind_y++;
818  }
819  }
820  gradient_x /= total_sobel;
821  }
822  }
823 
824  // TODO: more efficient would be to use dx, dy directly:
825  (*dx) = gradient_x;
826  (*dy) = gradient_y;
827 }
828 
838 void image_draw_rectangle(struct image_t *img, int x_min, int x_max, int y_min, int y_max, uint8_t *color)
839 {
840  struct point_t from, to;
841 
842  // bottom from left to right:
843  from.x = x_min;
844  from.y = y_min;
845  to.x = x_max;
846  to.y = y_min;
847  image_draw_line_color(img, &from, &to, color);
848 
849  // from bottom right to top right:
850  from.x = x_max;
851  from.y = y_min;
852  to.x = x_max;
853  to.y = y_max;
854  image_draw_line_color(img, &from, &to, color);
855 
856  // from top right to top left:
857  from.x = x_max;
858  from.y = y_max;
859  to.x = x_min;
860  to.y = y_max;
861  image_draw_line_color(img, &from, &to, color);
862 
863  // from top left to bottom left:
864  from.x = x_min;
865  from.y = y_max;
866  to.x = x_min;
867  to.y = y_min;
868  image_draw_line_color(img, &from, &to, color);
869 
870 }
871 
880 void image_draw_crosshair(struct image_t *img, struct point_t *loc, const uint8_t *color, uint32_t size_crosshair)
881 {
882  struct point_t from, to;
883 
884  if (loc->x >= size_crosshair && loc->x < img->w - size_crosshair
885  && loc->y >= size_crosshair && loc->y < img->h - size_crosshair) {
886  // draw the lines:
887  from.x = loc->x - size_crosshair;
888  from.y = loc->y;
889  to.x = loc->x + size_crosshair;
890  to.y = loc->y;
891  image_draw_line_color(img, &from, &to, color);
892  from.x = loc->x;
893  from.y = loc->y - size_crosshair;
894  to.x = loc->x;
895  to.y = loc->y + size_crosshair;
896  image_draw_line_color(img, &from, &to, color);
897  }
898 }
899 
906 void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *to)
907 {
908  static uint8_t color[4] = {255, 255, 255, 255};
909  image_draw_line_color(img, from, to, color);
910 }
911 
912 
921 void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, const uint8_t *color)
922 {
923  int xerr = 0, yerr = 0;
924  uint8_t *img_buf = (uint8_t *)img->buf;
925  uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1;
926  uint16_t startx = from->x;
927  uint16_t starty = from->y;
928 
929  uint8_t temp_color[4] = {color[0], color[1], color[2], color[3]};
930 
931  /* compute the distances in both directions */
932  int32_t delta_x = to->x - from->x;
933  int32_t delta_y = to->y - from->y;
934 
935  /* Compute the direction of the increment,
936  an increment of 0 means either a horizontal or vertical
937  line.
938  */
939  int8_t incx, incy;
940  if (delta_x > 0) { incx = 1; }
941  else if (delta_x == 0) { incx = 0; }
942  else { incx = -1; }
943 
944  if (delta_y > 0) { incy = 1; }
945  else if (delta_y == 0) { incy = 0; }
946  else { incy = -1; }
947 
948  /* determine which distance is greater */
949  uint16_t distance = 0;
950  delta_x = abs(delta_x);
951  delta_y = abs(delta_y);
952  if (delta_x > delta_y) { distance = delta_x * 20; }
953  else { distance = delta_y * 20; }
954 
955  /* draw the line */
956  for (uint16_t t = 0; /* starty >= 0 && */ starty < img->h && /* startx >= 0 && */ startx < img->w
957  && t <= distance + 1; t++) {
958 
959  // depending on startx being odd or even, we first have to set U or V
960  if (startx % 2 == 1) {
961  temp_color[0] = color[2];
962  temp_color[2] = color[0];
963  } else {
964  temp_color[0] = color[0];
965  temp_color[2] = color[2];
966  }
967  uint32_t buf_loc = img->w * pixel_width * starty + startx * pixel_width;
968  img_buf[buf_loc] = temp_color[0]; // u (when startx even)
969 
970  if (img->type == IMAGE_YUV422) {
971  img_buf[buf_loc + 1] = temp_color[1]; // y1
972 
973  if (startx + 1 < img->w) {
974  img_buf[buf_loc + 2] = temp_color[2]; // v (when startx even)
975  img_buf[buf_loc + 3] = temp_color[3]; // y2
976  }
977  }
978 
979  xerr += delta_x;
980  yerr += delta_y;
981  if (xerr > distance) {
982  xerr -= distance;
983  startx += incx;
984  }
985  if (yerr > distance) {
986  yerr -= distance;
987  starty += incy;
988  }
989  }
990 }
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:538
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:272
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:594
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_free(struct image_t *img)
Free the image.
Definition: image.c:75
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:921
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
#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
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
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:906
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:732
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 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:668
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:715
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:838
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:453
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:406
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:766
uint16_t w
Image width.
Definition: image.h:46
uint16_t h
Image height.
Definition: image.h:47
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:361
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
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:880
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:131
Definition: image.h:58
#define CACHE_LINE_LENGTH
Definition: image.c:33
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:562
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:166
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:632
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:230
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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:480
An JPEG encoded image (not per pixel encoded)
Definition: image.h:38
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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:311
enum image_type type
The image type.
Definition: image.h:45
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
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:690