Paparazzi UAS  v6.2_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
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 }
check_color_snake_gate_detection
int check_color_snake_gate_detection(struct image_t *im, int x, int y)
Definition: snake_gate_detection.c:1102
gate_img::y
int y
The image y coordinate of the gate center.
Definition: snake_gate_detection.h:52
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
image_draw_line_color
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
color_YM
uint8_t color_YM
Definition: detect_gate.c:91
snake_gate_detection
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.
Definition: snake_gate_detection.c:135
segment_length
float segment_length(struct point_t Q1, struct point_t Q2)
Determine the segment length between two 2D-points.
Definition: snake_gate_detection.c:802
gate_img::x
int x
The image x coordinate of the gate center.
Definition: snake_gate_detection.h:51
cmpfunc
int cmpfunc(const void *a, const void *b)
Definition: snake_gate_detection.c:90
gates_c
struct gate_img gates_c[MAX_GATES]
Definition: detect_gate.c:103
gate_img::quality
float quality
gate quality
Definition: snake_gate_detection.h:56
refine_single_corner
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.
Definition: snake_gate_detection.c:1013
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
gate_img::x_corners
int x_corners[4]
Array of corner x coordinates.
Definition: snake_gate_detection.h:53
b
float b
Definition: wedgebug.c:202
min_pixel_size
int min_pixel_size
Definition: snake_gate_detection.c:73
color_U_max
uint8_t color_U_max
Definition: snake_gate_detection.c:68
FAIL_DETECT
#define FAIL_DETECT
Definition: snake_gate_detection.c:53
img_result
struct image_t img_result
Definition: snake_gate_detection.c:81
gate_img::n_sides
int n_sides
How many sides are orange (to prevent detecting a small gate in the corner of a big one partially out...
Definition: snake_gate_detection.h:57
DRAW_GATE
#define DRAW_GATE
Definition: snake_gate_detection.c:57
color_VM
uint8_t color_VM
Definition: detect_gate.c:95
best_fitness
float best_fitness
Definition: snake_gate_detection.c:83
MAX_GATES
#define MAX_GATES
Definition: snake_gate_detection.h:47
best_gate
struct gate_img best_gate
Definition: detect_gate.c:101
snake_up_and_down
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.
Definition: snake_gate_detection.c:859
check_inside
float check_inside(struct image_t *im, int x, int y, int sz, int n_samples_in)
Definition: snake_gate_detection.c:761
array
int * array
Definition: snake_gate_detection.c:94
min_gate_quality
float min_gate_quality
Definition: detect_gate.c:88
image_t::w
uint16_t w
Image width.
Definition: image.h:46
gate_refine_corners
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.
Definition: snake_gate_detection.c:993
gate_img::sz
int sz
Half the image size of the gate.
Definition: snake_gate_detection.h:55
color_Vm
uint8_t color_Vm
Definition: detect_gate.c:94
paparazzi.h
intersection_over_union
float intersection_over_union(int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
Definition: snake_gate_detection.c:1131
cmp_i
int cmp_i(const void *a, const void *b)
Definition: snake_gate_detection.c:96
color_V_max
uint8_t color_V_max
Definition: snake_gate_detection.c:70
snake_gate_detection.h
white_color
uint8_t white_color[4]
Definition: snake_gate_detection.c:62
draw_gate_color_polygon
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.
Definition: snake_gate_detection.c:491
image_t::h
uint16_t h
Image height.
Definition: image.h:47
draw_gate_color_square
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.
Definition: snake_gate_detection.c:538
exclude_top
int exclude_top
Definition: detect_gate.c:96
n_total_samples
int n_total_samples
Definition: snake_gate_detection.c:77
color_UM
uint8_t color_UM
Definition: detect_gate.c:93
snake_left_and_right
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.
Definition: snake_gate_detection.c:919
temp_check_gate
struct gate_img temp_check_gate
Definition: snake_gate_detection.c:80
set_gate_points
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...
Definition: snake_gate_detection.c:971
gate_thickness
float gate_thickness
Definition: detect_gate.c:89
green_color
uint8_t green_color[4]
Definition: snake_gate_detection.c:60
image_yuv422_colorfilt
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
color_Ym
uint8_t color_Ym
Definition: detect_gate.c:90
color_Um
uint8_t color_Um
Definition: detect_gate.c:92
check_color_yuv422
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
image.h
SUCCESS_DETECT
#define SUCCESS_DETECT
Definition: snake_gate_detection.c:52
check_line
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.
Definition: snake_gate_detection.c:818
best_quality
float best_quality
Definition: snake_gate_detection.c:82
n_samples
int n_samples
Definition: detect_gate.c:85
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
point_t::x
uint32_t x
The x coordinate of the point.
Definition: image.h:59
color_V_min
uint8_t color_V_min
Definition: snake_gate_detection.c:69
min_px_size
int min_px_size
Definition: detect_gate.c:87
image_draw_crosshair
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
color_Y_min
uint8_t color_Y_min
Definition: snake_gate_detection.c:65
check_gate_outline
void check_gate_outline(struct image_t *im, struct gate_img gate, float *quality, int *n_sides)
Check only the outline of the gate.
Definition: snake_gate_detection.c:581
point_t::y
uint32_t y
The y coordinate of the point.
Definition: image.h:60
exclude_bottom
int exclude_bottom
Definition: detect_gate.c:97
blue_color
uint8_t blue_color[4]
Definition: snake_gate_detection.c:61
gate_img::sz_left
float sz_left
Half the image size of the left side.
Definition: snake_gate_detection.h:58
gate_img
Definition: snake_gate_detection.h:50
draw_gate
void draw_gate(struct image_t *im, struct gate_img gate)
Draw the gate on an image.
Definition: snake_gate_detection.c:478
gate_img::sz_right
float sz_right
Half the image size of the right side.
Definition: snake_gate_detection.h:59
color_U_min
uint8_t color_U_min
Definition: snake_gate_detection.c:67
intersection_boxes
int intersection_boxes(int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
Definition: snake_gate_detection.c:1167
point_t
Definition: image.h:58
color_Y_max
uint8_t color_Y_max
Definition: snake_gate_detection.c:66
gate_img::y_corners
int y_corners[4]
Array of corner y coordinates.
Definition: snake_gate_detection.h:54
image_t
Definition: image.h:44
check_gate_initial
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.
Definition: snake_gate_detection.c:673
FILTER_IMAGE
#define FILTER_IMAGE
Definition: snake_gate_detection.c:56
overlap_intervals
int overlap_intervals(int val_low_1, int val_high_1, int val_low_2, int val_high_2)
Definition: snake_gate_detection.c:1183
min_n_sides
int min_n_sides
Definition: detect_gate.c:86