Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
snake_gate_detection.c
Go to the documentation of this file.
1 
2 /*
3  * Copyright (C) 2018, Guido de Croon and Michael Ozo
4  *
5  * This file is part of Paparazzi.
6  *
7  * Paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * Paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with Paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
41 // Own header
43 #include <stdio.h>
44 #include <stdlib.h>
46 #include "paparazzi.h"
47 
48 // to debug the algorithm, uncomment the define:
49 // #define DEBUG_SNAKE_GATE
50 
51 // the return values of the main detection function:
52 #define SUCCESS_DETECT 1
53 #define FAIL_DETECT 0
54 
55 // whether to filter the image and show the best gate(s):
56 #define FILTER_IMAGE 0
57 #define DRAW_GATE 1
58 
59 // Standard colors in UYVY:
60 uint8_t green_color[4] = {255, 128, 255, 128};
61 uint8_t blue_color[4] = {0, 128, 0, 128};
62 uint8_t white_color[4] = {255, 255, 255, 255};
63 
64 // Filter Settings
71 
72 // Other settings:
74 
75 // variable used to count the number of color checks,
76 // so that we can better restrain the total number of samples taken:
78 
79 // Result
81 struct image_t img_result;
82 float best_quality = 0;
83 float best_fitness = 100000;
84 
85 // Support functions:
86 int cmpfunc(const void *a, const void *b);
87 int cmp_i(const void *a, const void *b);
88 float segment_length(struct point_t Q1, struct point_t Q2);
89 
90 int cmpfunc(const void *a, const void *b)
91 {
92  return (*(const int *)a - * (const int *)b);
93 }
94 int *array;
95 //return indexes
96 int cmp_i(const void *a, const void *b)
97 {
98  int ia = *(const int *)a;
99  int ib = *(const int *)b;
100  return array[ia] < array[ib] ? -1 : array[ia] > array[ib];
101 }
102 
103 
104 // TODO: NOT FOR A FIRST PULL REQUEST: Since coordinates matter here, we have to deal with the strange sensor mounting in the Parrot Bebop.
105 // This leads to checks such as x < im->h... This is a quite fundamental problem, with not a clear solution. However, if a normally
106 // mounted sensor is used, the functions here will fail on this exact point...
107 
136  float gate_thickness, int min_n_sides,
138  struct gate_img *best_gate, struct gate_img *gates_c, int *n_gates, int exclude_top, int exclude_bottom)
139 {
140 
141  static int last_frame_detection = 0;
142  static int repeat_gate = 0;
143  static struct gate_img previous_best_gate = {0};
144  static struct gate_img last_gate;
145 
146  bool check_initial_square = false;
147  float iou_threshold = 0.7; // when bigger than this, gates are assumed to represent the same gate
148 
149 
150  (*n_gates) = 0;
151  // For a new image, set the total number of samples to 0:
152  // This number is augmented when checking the color of a pixel.
153  n_total_samples = 0;
154 
162 
163  int x, y;
164  best_quality = 0;
165  best_gate->quality = 0;
166  (*n_gates) = 0;
167 
168  // variables for snake gate detection:
169  int y_low = 0;
170  int x_l = 0;
171  int y_high = 0;
172  int x_h = 0;
173  int x_low1 = 0;
174  int y_l1 = 0;
175  int x_high1 = 0;
176  int y_h1 = 0;
177  int x_low2 = 0;
178  int y_l2 = 0;
179  int x_high2 = 0;
180  int y_h2 = 0;
181  int sz = 0;
182  int szx1 = 0;
183  int szx2 = 0;
184 
185  //for (int i = 0; i < n_samples; i++) {
186  while (n_total_samples < n_samples) {
187 
188  // TODO: would it work better to scan different lines in the image?
189  // get a random coordinate:
190  x = rand() % img->h;
191  y = exclude_top + rand() % (img->w - exclude_top - exclude_bottom);
192 
193  // check if it has the right color
194  if (check_color_snake_gate_detection(img, x, y)) {
195 
196  // fill histogram (TODO: for a next pull request, in which we add the close-by histogram-detection)
197  // histogram[x]++;
198 
199  // snake up and down:
200  snake_up_and_down(img, x, y, &x_l, &y_low, &x_h, &y_high);
201 
202  // This assumes the gate to be square:
203  sz = y_high - y_low;
204  y_low = y_low + (sz * gate_thickness);
205  y_high = y_high - (sz * gate_thickness);
206  y = (y_high + y_low) / 2;
207 
208  // if the found part of the gate is large enough
209  if (sz > min_pixel_size) {
210 
211  // snake left and right, both for the top and bottom part of the gate:
212  snake_left_and_right(img, x_l, y_low, &x_low1, &y_l1, &x_high1, &y_h1);
213  snake_left_and_right(img, x_h, y_high, &x_low2, &y_l2, &x_high2, &y_h2);
214  x_low1 = x_low1 + (sz * gate_thickness);
215  x_high1 = x_high1 - (sz * gate_thickness);
216  x_low2 = x_low2 + (sz * gate_thickness);
217  x_high2 = x_high2 - (sz * gate_thickness);
218 
219  // sizes of the left-right stretches: in y pixel coordinates
220  szx1 = (x_high1 - x_low1);
221  szx2 = (x_high2 - x_low2);
222 
223  // set the size according to the biggest detection:
224  if (szx1 > szx2) {
225  // determine the center x based on the bottom part:
226  x = (x_high1 + x_low1) / 2;
227  // set the size to the largest line found:
228  sz = (sz > szx1) ? sz : szx1;
229  } else {
230  // determine the center x based on the top part:
231  x = (x_high2 + x_low2) / 2;
232  // set the size to the largest line found:
233  sz = (sz > szx2) ? sz : szx2;
234  }
235 
236  if (sz > min_pixel_size) {
237  // create the gate:
238  gates_c[(*n_gates)].x = x;
239  gates_c[(*n_gates)].y = y;
240  // store the half gate size:
241  gates_c[(*n_gates)].sz = sz / 2;
242 
243  if (check_initial_square) {
244 
245  // check the gate quality:
246  check_gate_initial(img, gates_c[(*n_gates)], &gates_c[(*n_gates)].quality, &gates_c[(*n_gates)].n_sides);
247 
248  } else {
249 
250  // The first two corners have a high y:
251  gates_c[(*n_gates)].x_corners[0] = x_low2;
252  gates_c[(*n_gates)].y_corners[0] = y_l2;
253  gates_c[(*n_gates)].x_corners[1] = x_high2;
254  gates_c[(*n_gates)].y_corners[1] = y_h2;
255 
256  // The third and fourth corner have a low y:
257  gates_c[(*n_gates)].x_corners[2] = x_high1;
258  gates_c[(*n_gates)].y_corners[2] = y_h1;
259  gates_c[(*n_gates)].x_corners[3] = x_low1;
260  gates_c[(*n_gates)].y_corners[3] = y_l1;
261 
262  // check the polygon:
263  check_gate_outline(img, gates_c[(*n_gates)], &gates_c[(*n_gates)].quality, &gates_c[(*n_gates)].n_sides);
264  }
265 
266  if (gates_c[(*n_gates)].quality > best_quality) {
267  best_quality = gates_c[(*n_gates)].quality;
268  }
269 
270  // set the corners to make a square gate for now:
271  set_gate_points(&gates_c[(*n_gates)]);
272 
273 
274  bool add_gate = true;
275  float iou;
276  for (int g = 0; g < (*n_gates); g++) {
278  gates_c[(*n_gates)].y_corners);
279  if (iou > iou_threshold) {
280  // we are looking at an existing gate:
281  add_gate = false;
282 
283  if (gates_c[g].quality > gates_c[(*n_gates)].quality) {
284  // throw the current gate away:
285  break;
286  } else {
287  // throw the old gate away:
288  // TODO: consider making a function for doing this "deep" copy
289  add_gate = true;
290  gates_c[g].x = gates_c[(*n_gates)].x;
291  gates_c[g].y = gates_c[(*n_gates)].y;
292  gates_c[g].sz = gates_c[(*n_gates)].sz;
293  gates_c[g].quality = gates_c[(*n_gates)].quality;
294  memcpy(gates_c[g].x_corners, gates_c[(*n_gates)].x_corners, sizeof(int) * 4);
295  memcpy(gates_c[g].y_corners, gates_c[(*n_gates)].y_corners, sizeof(int) * 4);
296  }
297  }
298  }
299 
300  if (add_gate) {
301  (*n_gates)++;
302  }
303  }
304 
305  if ((*n_gates) >= MAX_GATES) {
306  break;
307  }
308  }
309  }
310  }
311 
312 #ifdef DEBUG_SNAKE_GATE
313  // draw all candidates:
314  printf("(*n_gates):%d\n", (*n_gates));
315  for (int i = 0; i < (*n_gates); i++) {
316  //draw_gate_color_square(img, gates_c[i], white_color);
318  }
319 #endif
320 
321  //init best gate
322  best_gate->quality = 0;
323  best_gate->n_sides = 0;
324  repeat_gate = 0;
325  float sz1 =0;
326  float sz2 = 0;
327 
328  // do an additional fit to improve the gate detection:
329  if ((best_quality > min_gate_quality && (*n_gates) > 0) || last_frame_detection) {
330 
331  // go over all remaining gates:
332  for (int gate_nr = 0; gate_nr < (*n_gates); gate_nr++) {
333 
334  // get gate information:
335  gate_refine_corners(img, gates_c[gate_nr].x_corners, gates_c[gate_nr].y_corners, gates_c[gate_nr].sz);
336 
337  // also get the color fitness
338  check_gate_outline(img, gates_c[gate_nr], &gates_c[gate_nr].quality, &gates_c[gate_nr].n_sides);
339 
340  // If the gate is good enough:
341  float sz1g, sz2g;
342  sz1g = (float) (gates_c[gate_nr].x_corners[1] - gates_c[gate_nr].x_corners[0]);
343  sz2g = (float) (gates_c[gate_nr].y_corners[1] - gates_c[gate_nr].y_corners[2]);
344 
345  // Don't accept gates that look too rectangular (not square enough)
346  float ratio;
347  static float limit_ratio = 1.5;
348  if(sz1g > 0.1 && sz2g > 0.1) {
349  ratio = (sz1g >= sz2g) ? sz1g / sz2g : sz2g / sz1g;
350  }
351  else {
352  ratio = limit_ratio + 0.1;
353  }
354 
355  // Old way: prefer the highest quality gate (most orange one):
356  // if (gates_c[gate_nr].n_sides >= min_n_sides && gates_c[gate_nr].quality > best_gate->quality) {
357 
358  // Prefer a bigger gate as best gate:
359  if (sz1g*sz2g > sz1*sz2 && gates_c[gate_nr].quality > min_gate_quality * 2 && gates_c[gate_nr].n_sides >= min_n_sides && ratio <= limit_ratio) {
360  // store the information in the gate:
361  best_gate->x = gates_c[gate_nr].x;
362  best_gate->y = gates_c[gate_nr].y;
363  best_gate->sz = gates_c[gate_nr].sz;
364  best_gate->sz_left = gates_c[gate_nr].sz_left;
365  best_gate->sz_right = gates_c[gate_nr].sz_right;
366  best_gate->quality = gates_c[gate_nr].quality;
367  best_gate->n_sides = gates_c[gate_nr].n_sides;
368  memcpy(best_gate->x_corners, gates_c[gate_nr].x_corners, sizeof(best_gate->x_corners));
369  memcpy(best_gate->y_corners, gates_c[gate_nr].y_corners, sizeof(best_gate->y_corners));
370  sz1 = (float) (best_gate->x_corners[1] - best_gate->x_corners[0]);
371  sz2 = (float) (best_gate->y_corners[1] - best_gate->y_corners[2]);
372 
373  }
374  }
375 
376  // if the best gate is not good enough, but we did have a detection in the previous image:
377  if ((best_gate->quality == 0 && best_gate->n_sides == 0) && last_frame_detection == 1) {
378 
379  // TODO: is it really important to do this sorting here to get the maximum size? Is the sz property not accurate enough?
380  // Or can we not assume the standard arrangement of the corners?
381  int x_values[4];
382  int y_values[4];
383  memcpy(x_values, last_gate.x_corners, sizeof(x_values));
384  memcpy(y_values, last_gate.y_corners, sizeof(y_values));
385  //sort small to large
386  qsort(x_values, 4, sizeof(int), cmpfunc);
387  qsort(y_values, 4, sizeof(int), cmpfunc);
388  //check x size, maybe use y also later?
389  int radius_p = x_values[3] - x_values[0];
390  // TODO: is 2*radius_p not huge?
391  gate_refine_corners(img, last_gate.x_corners, last_gate.y_corners, 2 * radius_p);
392 
393  // also get the color fitness
394  check_gate_outline(img, last_gate, &last_gate.quality, &last_gate.n_sides);
395 
396  // if the refined detection is good enough:
397  if (last_gate.n_sides >= min_n_sides && last_gate.quality > best_gate->quality) {
398  repeat_gate = 1;
399  best_gate->quality = last_gate.quality;
400  best_gate->n_sides = last_gate.n_sides;
401  memcpy(best_gate->x_corners, last_gate.x_corners, sizeof(best_gate->x_corners));
402  memcpy(best_gate->y_corners, last_gate.y_corners, sizeof(best_gate->y_corners));
403  }
404  }
405 
406 #ifdef DEBUG_SNAKE_GATE
407  // draw the best gate:
408  draw_gate(img, (*best_gate));
409 #endif
410  }
411 
412  // prepare for the next time:
413  previous_best_gate.x = best_gate->x;
414  previous_best_gate.y = best_gate->y;
415  previous_best_gate.sz = best_gate->sz;
416  previous_best_gate.sz_left = best_gate->sz_left;
417  previous_best_gate.sz_right = best_gate->sz_right;
418  previous_best_gate.quality = best_gate->quality;
419  previous_best_gate.n_sides = best_gate->n_sides;
420  memcpy(previous_best_gate.x_corners, best_gate->x_corners, sizeof(best_gate->x_corners));
421  memcpy(previous_best_gate.y_corners, best_gate->y_corners, sizeof(best_gate->y_corners));
422 
423  //color filtered version of image for overlay and debugging
424  if (FILTER_IMAGE) { //filter) {
426  }
427 
429  // successful detection
430  last_frame_detection = 1;
431 
432  //draw_gate_color(img, best_gate, blue_color);
433  if (DRAW_GATE) {
434  for (int gate_nr = 0; gate_nr < (*n_gates); gate_nr++) {
435  if (gates_c[gate_nr].n_sides >= min_n_sides && gates_c[gate_nr].quality > 2 * min_gate_quality) {
436  // draw the best gate:
437  // draw_gate(img, gates_c[gate_nr]);
439  }
440  }
441 
442  int size_crosshair = 10;
443  if (repeat_gate == 0) {
445  } else if (repeat_gate == 1) {
447  for (int i = 0; i < 3; i++) {
448  struct point_t loc = { .x = last_gate.x_corners[i], .y = last_gate.y_corners[i] };
449  image_draw_crosshair(img, &loc, blue_color, size_crosshair);
450  }
451  }
452  }
453  //save for next iteration
454  memcpy(last_gate.x_corners, best_gate->x_corners, sizeof(best_gate->x_corners));
455  memcpy(last_gate.y_corners, best_gate->y_corners, sizeof(best_gate->y_corners));
456  //previous best snake gate
457  last_gate.x = best_gate->x;
458  last_gate.y = best_gate->y;
459  last_gate.sz = best_gate->sz;
460 
461  //SIGNAL NEW DETECTION AVAILABLE
462  return SUCCESS_DETECT;
463 
464  } else {
465  //no detection
466  last_frame_detection = 0;
467  return FAIL_DETECT;
468  }
469 }
470 
471 
478 void draw_gate(struct image_t *im, struct gate_img gate)
479 {
481 }
482 
483 
491 void draw_gate_color_polygon(struct image_t *im, struct gate_img gate, uint8_t *color)
492 {
493  // Please note that here we use functions in image.h, so we have to inverse the coordinates:
494  // draw four lines and a crosshair on the image:
495  struct point_t from, to;
496 
497  // a cross at the center
498  from.x = gate.y;
499  from.y = gate.x;
500  image_draw_crosshair(im, &from, color, 10);
501 
502  // the four lines:
503  from.x = gate.y_corners[0];
504  from.y = gate.x_corners[0];
505  to.x = gate.y_corners[1];
506  to.y = gate.x_corners[1];
507  image_draw_line_color(im, &from, &to, color);
508 
509  from.x = gate.y_corners[1];
510  from.y = gate.x_corners[1];
511  to.x = gate.y_corners[2];
512  to.y = gate.x_corners[2];
513  image_draw_line_color(im, &from, &to, color);
514 
515  from.x = gate.y_corners[2];
516  from.y = gate.x_corners[2];
517  to.x = gate.y_corners[3];
518  to.y = gate.x_corners[3];
519  image_draw_line_color(im, &from, &to, color);
520 
521  from.x = gate.y_corners[3];
522  from.y = gate.x_corners[3];
523  to.x = gate.y_corners[0];
524  to.y = gate.x_corners[0];
525  image_draw_line_color(im, &from, &to, color);
526 
527 }
528 
529 
530 
538 void draw_gate_color_square(struct image_t *im, struct gate_img gate, uint8_t *color)
539 {
540  // Please note that here we use functions in image.h, so we have to inverse the coordinates:
541  // draw four lines and a crosshair on the image:
542  struct point_t from, to;
543 
544  from.x = gate.y;
545  from.y = gate.x;
546  image_draw_crosshair(im, &from, color, 10);
547 
548  if (gate.sz_left == 0) { gate.sz_left = gate.sz; }
549  if (gate.sz_right == 0) { gate.sz_right = gate.sz; }
550 
551  from.x = gate.y - gate.sz_left;
552  from.y = gate.x - gate.sz;
553  to.x = gate.y + gate.sz_left;
554  to.y = gate.x - gate.sz;
555  image_draw_line_color(im, &from, &to, color);
556  from.x = gate.y + gate.sz_left;
557  from.y = gate.x - gate.sz;
558  to.x = gate.y + gate.sz_right;
559  to.y = gate.x + gate.sz;
560  image_draw_line_color(im, &from, &to, color);
561  from.x = gate.y + gate.sz_right;
562  from.y = gate.x + gate.sz;
563  to.x = gate.y - gate.sz_right;
564  to.y = gate.x + gate.sz;
565  image_draw_line_color(im, &from, &to, color);
566  from.x = gate.y - gate.sz_right;
567  from.y = gate.x + gate.sz;
568  to.x = gate.y - gate.sz_left;
569  to.y = gate.x - gate.sz;
570  image_draw_line_color(im, &from, &to, color);
571 }
572 
581 void check_gate_outline(struct image_t *im, struct gate_img gate, float *quality, int *n_sides)
582 {
583  int n_points, n_colored_points;
584  n_points = 0;
585  n_colored_points = 0;
586  int np, nc;
587 
588  // how much of the side should be visible to count as a detected side?
589  // TODO: make this a setting. Note: the number is different from the check_gate_initial function.
590  float min_ratio_side = 0.4;
591  (*n_sides) = 0;
592 
593  float min_segment_length = min_pixel_size;
594 
595  // check the four lines of which the gate consists:
596  struct point_t from, to;
597 
598  from.x = gate.x_corners[0];
599  from.y = gate.y_corners[0];
600  to.x = gate.x_corners[1];
601  to.y = gate.y_corners[1];
602  check_line(im, from, to, &np, &nc);
603  if ((float) nc / (float) np >= min_ratio_side && segment_length(from, to) > min_segment_length) {
604  (*n_sides)++;
605  }
606  n_points += np;
607  n_colored_points += nc;
608 
609  from.x = gate.x_corners[1];
610  from.y = gate.y_corners[1];
611  to.x = gate.x_corners[2];
612  to.y = gate.y_corners[2];
613  check_line(im, from, to, &np, &nc);
614  if ((float) nc / (float) np >= min_ratio_side && segment_length(from, to) > min_segment_length) {
615  (*n_sides)++;
616  }
617  n_points += np;
618  n_colored_points += nc;
619 
620  from.x = gate.x_corners[2];
621  from.y = gate.y_corners[2];
622  to.x = gate.x_corners[3];
623  to.y = gate.y_corners[3];
624  check_line(im, from, to, &np, &nc);
625  if ((float) nc / (float) np >= min_ratio_side && segment_length(from, to) > min_segment_length) {
626  (*n_sides)++;
627  }
628  n_points += np;
629  n_colored_points += nc;
630 
631  from.x = gate.x_corners[3];
632  from.y = gate.y_corners[3];
633  to.x = gate.x_corners[0];
634  to.y = gate.y_corners[0];
635  check_line(im, from, to, &np, &nc);
636  if ((float) nc / (float) np >= min_ratio_side && segment_length(from, to) > min_segment_length) {
637  (*n_sides)++;
638  }
639 
640  n_points += np;
641  n_colored_points += nc;
642 
643  // the quality is the ratio of colored points / number of points:
644  if (n_points == 0) {
645  (*quality) = 0;
646  } else {
647  (*quality) = ((float) n_colored_points) / ((float) n_points);
648  }
649 
650 
651  // Check the inside of the gate - again:
652  static int n_samples_in = 100;
653  static float center_discard_threshold = 0.25;
654  gate.sz = gate.x_corners[1] - gate.x_corners[0];
655  float center_factor = check_inside(im, gate.x, gate.y, gate.sz, n_samples_in);
656  if (center_factor > center_discard_threshold) {
657  (*quality) = 0;
658  }
659 }
660 
661 
673 extern void check_gate_initial(struct image_t *im, struct gate_img gate, float *quality, int *n_sides)
674 {
675  int n_points, n_colored_points;
676  n_points = 0;
677  n_colored_points = 0;
678  int np, nc;
679 
680  // how much of the side should be visible to count as a detected side?
681  float min_ratio_side = 0.30;
682  (*n_sides) = 0;
683 
684  // check the four lines of which the gate consists:
685  struct point_t from, to;
686 
687  from.x = gate.x - gate.sz;
688  from.y = gate.y - gate.sz_left;
689  to.x = gate.x - gate.sz;
690  to.y = gate.y + gate.sz_left;
691  check_line(im, from, to, &np, &nc);
692  if ((float) nc / (float) np >= min_ratio_side) {
693  (*n_sides)++;
694  }
695  n_points += np;
696  n_colored_points += nc;
697 
698  from.x = gate.x - gate.sz;
699  from.y = gate.y + gate.sz_left;
700  to.x = gate.x + gate.sz;
701  to.y = gate.y + gate.sz_right;
702  check_line(im, from, to, &np, &nc);
703  if ((float) nc / (float) np >= min_ratio_side) {
704  (*n_sides)++;
705  }
706  n_points += np;
707  n_colored_points += nc;
708 
709  from.x = gate.x + gate.sz;
710  from.y = gate.y + gate.sz_right;
711  to.x = gate.x + gate.sz;
712  to.y = gate.y - gate.sz_right;
713  check_line(im, from, to, &np, &nc);
714  if ((float) nc / (float) np >= min_ratio_side) {
715  (*n_sides)++;
716  }
717  n_points += np;
718  n_colored_points += nc;
719 
720  from.x = gate.x + gate.sz;
721  from.y = gate.y - gate.sz_right;
722  to.x = gate.x - gate.sz;
723  to.y = gate.y - gate.sz_left;
724  check_line(im, from, to, &np, &nc);
725  if ((float) nc / (float) np >= min_ratio_side) {
726  (*n_sides)++;
727  }
728 
729  n_points += np;
730  n_colored_points += nc;
731 
732 
733  // the quality is the ratio of colored points / number of points:
734  if (n_points == 0) {
735  (*quality) = 0;
736  } else {
737  (*quality) = ((float) n_colored_points) / ((float) n_points);
738  }
739 
740  // check that the inside of the gate is not of the target color as well:
741  int n_samples_in = 100;
742  float center_discard_threshold = 0.25;
743  float center_factor = check_inside(im, gate.x, gate.y, gate.sz, n_samples_in);
744  if (center_factor > center_discard_threshold) {
745  (*quality) = 0;
746  }
747 
748 
749 }
750 
751 /* Check inside of a gate, in order to exclude solid areas.
752  *
753  * @param[out] center_factor The ratio of pixels inside the box that are of the right color.
754  * @param[in] im The YUV422 image.
755  * @param[in] x The center x-coordinate of the gate
756  * @param[in] y The center y-coordinate of the gate
757  * @param[in] sz The size of the gate - when approximated as square.
758  * @param[in] n_samples_in The number of samples used to determine the ratio.
759  */
760 
761 float check_inside(struct image_t *im, int x, int y, int sz, int n_samples_in)
762 {
763  int num_color_center = 0;
764  int n_samples = 0;
765 
766  if (sz == 0) {
767  return 1.0f;
768  }
769 
770  for (int i = 0; i < n_samples_in; i++) {
771  // get a random coordinate:
772  int x_in = x + (rand() % sz) - (0.5 * sz);
773  int y_in = y + (rand() % sz) - (0.5 * sz);
774 
775  if (y_in >= 0 && y_in < im->w && x_in >= 0 && x_in < im->h) {
776  n_samples++;
777  // check if it has the right color
778  if (check_color_snake_gate_detection(im, x_in, y_in)) {
779  num_color_center ++;
780  }
781  }
782  }
783 
784  //how much center pixels colored?
785  if (n_samples == 0) {
786  return 1.0f;
787  }
788 
789  float center_factor = 0;
790  if (n_samples != 0) {
791  center_factor = num_color_center / (float)n_samples;
792  }
793  return center_factor;
794 }
795 
802 float segment_length(struct point_t Q1, struct point_t Q2)
803 {
804 
805  float r = sqrtf((Q1.x - Q2.x) * (Q1.x - Q2.x) + (Q1.y - Q2.y) * (Q1.y - Q2.y));
806  return r;
807 }
808 
818 void check_line(struct image_t *im, struct point_t Q1, struct point_t Q2, int *n_points, int *n_colored_points)
819 {
820 
821  (*n_points) = 0;
822  (*n_colored_points) = 0;
823 
824  // t_step determines how many samples are taken (1.0 / t_step)
825  float t_step = 0.05;
826  int x, y;
827  float t;
828  // go from Q1 to Q2 in 1/t_step steps:
829  for (t = 0.0f; t < 1.0f; t += t_step) {
830  // determine integer coordinate on the line:
831  x = (int)(t * Q1.x + (1.0f - t) * Q2.x);
832  y = (int)(t * Q1.y + (1.0f - t) * Q2.y);
833 
834  // Is the point in the image?
835  if (x >= 0 && x < im->h && y >= 0 && y < im->w) {
836  // augment number of checked points:
837  (*n_points)++;
838 
840  // the point is of the right color:
841  (*n_colored_points)++;
842  }
843  }
844  }
845 }
846 
859 void snake_up_and_down(struct image_t *im, int x, int y, int *x_low, int *y_low, int *x_high, int *y_high)
860 {
861  int done = 0;
862  int x_initial = x;
863  (*y_low) = y;
864 
865  // TODO: perhaps it is better to put the big steps first, as to reduce computation.
866  // snake towards negative y
867  while ((*y_low) > 0 && !done) {
868  if (check_color_snake_gate_detection(im, x, (*y_low) - 1)) {
869  (*y_low)--;
870  } else if ((*y_low) - 2 >= 0 && check_color_snake_gate_detection(im, x, (*y_low) - 2)) {
871  (*y_low) -= 2;
872  } else if (x + 1 < im->h && check_color_snake_gate_detection(im, x + 1, (*y_low) - 1)) {
873  x++;
874  (*y_low)--;
875  } else if (x - 1 >= 0 && check_color_snake_gate_detection(im, x - 1, (*y_low) - 1)) {
876  x--;
877  (*y_low)--;
878  } else {
879  done = 1;
880  (*x_low) = x;
881  }
882  }
883 
884  // snake towards positive y
885  x = x_initial;
886  (*y_high) = y;
887  done = 0;
888  while ((*y_high) < im->w - 1 && !done) {
889  if (check_color_snake_gate_detection(im, x, (*y_high) + 1)) {
890  (*y_high)++;
891  } else if ((*y_high) < im->w - 2 && check_color_snake_gate_detection(im, x, (*y_high) + 2)) {
892  (*y_high) += 2;
893  } else if (x < im->h - 1 && check_color_snake_gate_detection(im, x + 1, (*y_high) + 1)) {
894  x++;
895  (*y_high)++;
896  } else if (x > 0 && check_color_snake_gate_detection(im, x - 1, (*y_high) + 1)) {
897  x--;
898  (*y_high)++;
899  } else {
900  done = 1;
901  (*x_high) = x;
902  }
903  }
904 }
905 
906 
919 void snake_left_and_right(struct image_t *im, int x, int y, int *x_low, int *y_low, int *x_high, int *y_high)
920 {
921  int done = 0;
922  int y_initial = y;
923  (*x_low) = x;
924 
925  // snake towards negative x (left)
926  while ((*x_low) > 0 && !done) {
927  if (check_color_snake_gate_detection(im, (*x_low) - 1, y)) {
928  (*x_low)--;
929  } else if ((*x_low) > 1 && check_color_snake_gate_detection(im, (*x_low) - 2, y)) {
930  (*x_low) -= 2;
931  } else if (y < im->w - 1 && check_color_snake_gate_detection(im, (*x_low) - 1, y + 1)) {
932  y++;
933  (*x_low)--;
934  } else if (y > 0 && check_color_snake_gate_detection(im, (*x_low) - 1, y - 1)) {
935  y--;
936  (*x_low)--;
937  } else {
938  done = 1;
939  (*y_low) = y;
940  }
941  }
942 
943  y = y_initial;
944  (*x_high) = x;
945  done = 0;
946  // snake towards positive x (right)
947  while ((*x_high) < im->h - 1 && !done) {
948  if (check_color_snake_gate_detection(im, (*x_high) + 1, y)) {
949  (*x_high)++;
950  } else if ((*x_high) < im->h - 2 && check_color_snake_gate_detection(im, (*x_high) + 2, y)) {
951  (*x_high) += 2;
952  } else if (y < im->w - 1 && check_color_snake_gate_detection(im, (*x_high) + 1, y++)) {
953  y++;
954  (*x_high)++;
955  } else if (y > 0 && check_color_snake_gate_detection(im, (*x_high) + 1, y - 1)) {
956  y--;
957  (*x_high)++;
958  } else {
959  done = 1;
960  (*y_high) = y;
961  }
962  }
963 }
964 
971 void set_gate_points(struct gate_img *gate)
972 {
973  // In Parrot Bebop coordinates, this goes from bottom-right CCW:
974  gate->x_corners[0] = gate->x - gate->sz;
975  gate->y_corners[0] = gate->y + gate->sz;
976  gate->x_corners[1] = gate->x + gate->sz;
977  gate->y_corners[1] = gate->y + gate->sz;
978  gate->x_corners[2] = gate->x + gate->sz;
979  gate->y_corners[2] = gate->y - gate->sz;
980  gate->x_corners[3] = gate->x - gate->sz;
981  gate->y_corners[3] = gate->y - gate->sz;
982 
983 }
984 
993 void gate_refine_corners(struct image_t *color_image, int *x_points, int *y_points, int size)
994 {
995 
996  // TODO: make parameter?
997  float corner_area = 0.3f;
998  refine_single_corner(color_image, x_points, y_points, size, corner_area);
999  refine_single_corner(color_image, &(x_points[1]), &(y_points[1]), size, corner_area);
1000  refine_single_corner(color_image, &(x_points[2]), &(y_points[2]), size, corner_area);
1001  refine_single_corner(color_image, &(x_points[3]), &(y_points[3]), size, corner_area);
1002 }
1003 
1013 void refine_single_corner(struct image_t *im, int *corner_x, int *corner_y, int size, float size_factor)
1014 {
1015 
1016  float x_corner_f = (float)(*corner_x);
1017  float y_corner_f = (float)(*corner_y);
1018  float size_f = (float)size;
1019  size_factor = 0.4f;
1020 
1021  int x_l = (int)(x_corner_f - size_f * size_factor);
1022  Bound(x_l, 0, im->h);
1023  int x_r = (int)(x_corner_f + size_f * size_factor);
1024  Bound(x_r, 0, im->h);
1025  int y_h = (int)(y_corner_f + size_f * size_factor);
1026  Bound(y_h, 0, im->w);
1027  int y_l = (int)(y_corner_f - size_f * size_factor);
1028  Bound(y_l, 0, im->w);
1029 
1030 
1031 #ifdef DEBUG_SNAKE_GATE
1032  // draw the box of refinement:
1033  struct gate_img box;
1034  box.x_corners[0] = x_l;
1035  box.y_corners[0] = y_l;
1036  box.x_corners[1] = x_l;
1037  box.y_corners[1] = y_h;
1038  box.x_corners[2] = x_r;
1039  box.y_corners[2] = y_h;
1040  box.x_corners[3] = x_r;
1041  box.y_corners[3] = y_l;
1042  draw_gate_color_polygon(im, box, green_color); // becomes grey, since it is called before the filtering...
1043 #endif
1044 
1045  int x_size = x_r - x_l + 1;
1046  int y_size = y_h - y_l + 1;
1047 
1048  int x_hist[x_size];
1049  int y_hist[y_size];
1050  memset(x_hist, 0, sizeof(int)*x_size);
1051  memset(y_hist, 0, sizeof(int)*y_size);
1052 
1053 
1054  int best_x = 0;
1055  int best_x_loc = x_l;
1056  int x_best_start = x_l;
1057  int best_y = 0;
1058  int best_y_loc = y_l;
1059  int y_best_start = y_l;
1060 
1061 
1062  for (int y_pix = y_l; y_pix < y_h; y_pix++) {
1063  for (int x_pix = x_l; x_pix < x_r; x_pix++) {
1064  if (check_color_snake_gate_detection(im, x_pix, y_pix) > 0) {
1065 
1066  int cur_x = x_hist[x_pix - x_l];
1067  int cur_y = y_hist[y_pix - y_l];
1068  x_hist[x_pix - x_l] = cur_x + 1;
1069  y_hist[y_pix - y_l] = cur_y + 1;
1070 
1071  if (x_hist[x_pix - x_l] > best_x) {
1072  best_x = x_hist[x_pix - x_l];
1073  best_x_loc = x_pix;
1074  x_best_start = x_pix;
1075  } else if (cur_x == best_x) {
1076  best_x_loc = (x_pix + x_best_start) / 2;
1077  }
1078  if (y_hist[y_pix - y_l] > best_y) {
1079  best_y = y_hist[y_pix - y_l];
1080  best_y_loc = y_pix;
1081  y_best_start = y_pix;
1082  } else if (cur_y == best_y) {
1083  best_y_loc = (y_pix + y_best_start) / 2;
1084  }
1085 
1086  }
1087  }
1088  }
1089 
1090  // Update the corner location:
1091  *corner_x = best_x_loc;
1092  *corner_y = best_y_loc;
1093 }
1094 
1095 
1096 /* Check the color of a pixel, within the snake gate detection scheme
1097  * @param[in] *im The YUV422 color image
1098  * @param[in] x The image x-coordinate of the pixel
1099  * @param[in] y The image y-coordinate of the pixel
1100  * @param[out] success Whether the pixel is the right color (1) or not (0)
1101  */
1102 int check_color_snake_gate_detection(struct image_t *im, int x, int y)
1103 {
1104 
1105  // Call the function in image.c with the color thresholds:
1106  // Please note that we have to switch x and y around here, due to the strange sensor mounting in the Bebop:
1108  color_V_max);
1109  n_total_samples++;
1110  /*
1111  #ifdef DEBUG_SNAKE_GATE
1112  if(success) {
1113  set_color_yuv422(im, y, x, 0, 0, 0);
1114  }
1115  #endif
1116  */
1117 
1118  return success;
1119 }
1120 
1121 
1122 /* Calculate the intersection over union of two boxes. The boxes are organized as the locations of the gate's corners.
1123  * So, from top left clock-wise.
1124  *
1125  * @param[in] x_box_1 The x-coordinates of the first box's corners
1126  * @param[in] y_box_1 The y-coordinates of the first box's corners
1127  * @param[in] x_box_2 The x-coordinates of the second box's corners
1128  * @param[in] y_box_2 The y-coordinates of the second box's corners
1129  * @param[out] The ratio of the intersection of the two boxes divided by their union.
1130  */
1131 float intersection_over_union(int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
1132 {
1133 
1134  // TODO: please note that the order of the indices here depends on the set_gate_points function.
1135  // A future pull request might adapt the indexing automatically to the chosen order in that function.
1136  float iou;
1137 
1138  // intersection:
1139  int intersection = intersection_boxes(x_box_1, y_box_1, x_box_2, y_box_2);
1140 
1141  // union:
1142  int w1, h1, w2, h2, un;
1143  w1 = x_box_1[1] - x_box_1[0];
1144  h1 = y_box_1[0] - y_box_1[2];
1145  w2 = x_box_2[1] - x_box_2[0];
1146  h2 = y_box_2[0] - y_box_2[2];
1147  un = w1 * h1 + w2 * h2 - intersection;
1148 
1149  // ratio of intersection over union:
1150  if (un == 0) {
1151  iou = 1.0f;
1152  } else {
1153  iou = (float) intersection / (float) un;
1154  }
1155 
1156  return iou;
1157 }
1158 
1159 /* Calculate the intersection of two boxes.
1160  *
1161  * @param[in] x_box_1 The x-coordinates of the first box's corners
1162  * @param[in] y_box_1 The y-coordinates of the first box's corners
1163  * @param[in] x_box_2 The x-coordinates of the second box's corners
1164  * @param[in] y_box_2 The y-coordinates of the second box's corners
1165  * @param[out] The number of pixels in the intersection area.
1166  */
1167 int intersection_boxes(int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
1168 {
1169 
1170  int width = overlap_intervals(x_box_1[0], x_box_1[1], x_box_2[0], x_box_2[1]);
1171  int height = overlap_intervals(y_box_1[2], y_box_1[0], y_box_2[2], y_box_2[0]);
1172 
1173  return width * height;
1174 }
1175 
1176 /* Calculate the overlap of two 1-dimensional intervals.
1177  * @param[in] val_low_1 The low value of the first interval.
1178  * @param[in] val_high_1 The high value of the first interval.
1179  * @param[in] val_low_2 The low value of the second interval.
1180  * @param[in] val_high_2 The low value of the second interval.
1181  * @param[out] int Number of overlapping units (pixels).
1182  */
1183 int overlap_intervals(int val_low_1, int val_high_1, int val_low_2, int val_high_2)
1184 {
1185 
1186  int overlap;
1187  int min_val;
1188  if (val_low_2 < val_low_1) {
1189  if (val_high_2 < val_low_1) {
1190  overlap = 0;
1191  } else {
1192  min_val = (val_high_1 > val_high_2) ? val_high_2 : val_high_1;
1193  overlap = min_val - val_low_1;
1194  }
1195  } else {
1196  if (val_high_1 < val_low_2) {
1197  overlap = 0;
1198  } else {
1199  min_val = (val_high_1 > val_high_2) ? val_high_2 : val_high_1;
1200  overlap = min_val - val_low_2;
1201  }
1202  }
1203 
1204  return overlap;
1205 }
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
float g
int n_samples
Definition: detect_gate.c:85
uint8_t color_Um
Definition: detect_gate.c:92
float gate_thickness
Definition: detect_gate.c:89
uint8_t color_Vm
Definition: detect_gate.c:94
uint8_t color_VM
Definition: detect_gate.c:95
int min_px_size
Definition: detect_gate.c:87
uint8_t color_YM
Definition: detect_gate.c:91
struct gate_img best_gate
Definition: detect_gate.c:101
struct gate_img gates_c[MAX_GATES]
Definition: detect_gate.c:103
int exclude_top
Definition: detect_gate.c:96
int exclude_bottom
Definition: detect_gate.c:97
float min_gate_quality
Definition: detect_gate.c:88
uint8_t color_UM
Definition: detect_gate.c:93
int min_n_sides
Definition: detect_gate.c:86
uint8_t color_Ym
Definition: detect_gate.c:90
void image_draw_crosshair(struct image_t *img, struct point_t *loc, const uint8_t *color, uint32_t size_crosshair)
Draw a cross-hair on the image.
Definition: image.c:891
void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, const uint8_t *color)
Draw a line on the image.
Definition: image.c:932
int check_color_yuv422(struct image_t *im, int x, int y, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
Checks the color of a single pixel in a YUV422 image.
Definition: image.c:237
uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
Filter colors in an YUV422 image.
Definition: image.c:173
Image helper functions like resizing, color filter, converters...
uint32_t x
The x coordinate of the point.
Definition: image.h:59
uint32_t y
The y coordinate of the point.
Definition: image.h:60
uint16_t h
Image height.
Definition: image.h:47
uint16_t w
Image width.
Definition: image.h:46
Definition: image.h:44
Definition: image.h:58
int * array
uint8_t blue_color[4]
uint8_t color_U_min
int check_color_snake_gate_detection(struct image_t *im, int x, int y)
int overlap_intervals(int val_low_1, int val_high_1, int val_low_2, int val_high_2)
void draw_gate_color_polygon(struct image_t *im, struct gate_img gate, uint8_t *color)
Draw the gate on an image, using the corner points, possibly resulting in a polygon.
int intersection_boxes(int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
int cmp_i(const void *a, const void *b)
int n_total_samples
void gate_refine_corners(struct image_t *color_image, int *x_points, int *y_points, int size)
Refine the four corners of the gate, based on the color around the supposed corner locations.
float best_fitness
struct image_t img_result
uint8_t color_V_min
void check_line(struct image_t *im, struct point_t Q1, struct point_t Q2, int *n_points, int *n_colored_points)
Checks whether points on a line between two 2D-points are of a given color.
int cmpfunc(const void *a, const void *b)
struct gate_img temp_check_gate
uint8_t color_Y_min
int snake_gate_detection(struct image_t *img, int n_samples, int min_px_size, float min_gate_quality, float gate_thickness, int min_n_sides, uint8_t color_Ym, uint8_t color_YM, uint8_t color_Um, uint8_t color_UM, uint8_t color_Vm, uint8_t color_VM, struct gate_img *best_gate, struct gate_img *gates_c, int *n_gates, int exclude_top, int exclude_bottom)
Run snake gate detection on an image.
int min_pixel_size
float best_quality
#define SUCCESS_DETECT
void check_gate_initial(struct image_t *im, struct gate_img gate, float *quality, int *n_sides)
Check the outline and the center of the gate.
#define FILTER_IMAGE
uint8_t color_V_max
float segment_length(struct point_t Q1, struct point_t Q2)
Determine the segment length between two 2D-points.
#define DRAW_GATE
void draw_gate_color_square(struct image_t *im, struct gate_img gate, uint8_t *color)
Draw the gate on an image, using only the center coordinate and sizes - resulting in a square gate.
void snake_up_and_down(struct image_t *im, int x, int y, int *x_low, int *y_low, int *x_high, int *y_high)
The actual snaking.
uint8_t white_color[4]
void check_gate_outline(struct image_t *im, struct gate_img gate, float *quality, int *n_sides)
Check only the outline of the gate.
float check_inside(struct image_t *im, int x, int y, int sz, int n_samples_in)
void set_gate_points(struct gate_img *gate)
Determine and set the corner locations in gate.x_corners, g.y_corners, based on the center of the gat...
float intersection_over_union(int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
void snake_left_and_right(struct image_t *im, int x, int y, int *x_low, int *y_low, int *x_high, int *y_high)
The actual snaking.
uint8_t color_Y_max
uint8_t green_color[4]
#define FAIL_DETECT
void refine_single_corner(struct image_t *im, int *corner_x, int *corner_y, int size, float size_factor)
Refine a single corner, based on the color around the coordinate.
uint8_t color_U_max
void draw_gate(struct image_t *im, struct gate_img gate)
Draw the gate on an image.
Detects gates as used in the IROS drone races, i.e., square colored gates.
int sz
Half the image size of the gate.
int n_sides
How many sides are orange (to prevent detecting a small gate in the corner of a big one partially out...
float sz_left
Half the image size of the left side.
int x
The image x coordinate of the gate center.
float sz_right
Half the image size of the right side.
int y
The image y coordinate of the gate center.
int x_corners[4]
Array of corner x coordinates.
int y_corners[4]
Array of corner y coordinates.
#define MAX_GATES
float quality
gate quality
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202