Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
snake_gate_detection.c File Reference

Detects gates as used in the IROS drone races, i.e., square colored gates. More...

#include "modules/computer_vision/snake_gate_detection.h"
#include <stdio.h>
#include <stdlib.h>
#include "modules/computer_vision/lib/vision/image.h"
#include "paparazzi.h"
+ Include dependency graph for snake_gate_detection.c:

Go to the source code of this file.

Macros

#define SUCCESS_DETECT   1
 
#define FAIL_DETECT   0
 
#define FILTER_IMAGE   0
 
#define DRAW_GATE   1
 

Functions

int cmpfunc (const void *a, const void *b)
 
int cmp_i (const void *a, const void *b)
 
float segment_length (struct point_t Q1, struct point_t Q2)
 Determine the segment length between two 2D-points. More...
 
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. More...
 
void draw_gate (struct image_t *im, struct gate_img gate)
 Draw the gate on an image. More...
 
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. More...
 
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. More...
 
void check_gate_outline (struct image_t *im, struct gate_img gate, float *quality, int *n_sides)
 Check only the outline of the gate. More...
 
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. More...
 
float check_inside (struct image_t *im, int x, int y, int sz, int n_samples_in)
 
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. More...
 
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. More...
 
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. More...
 
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 gate and its size, assuming a square window. More...
 
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. More...
 
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. More...
 
int check_color_snake_gate_detection (struct image_t *im, int x, int y)
 
float intersection_over_union (int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
 
int intersection_boxes (int x_box_1[4], int y_box_1[4], int x_box_2[4], int y_box_2[4])
 
int overlap_intervals (int val_low_1, int val_high_1, int val_low_2, int val_high_2)
 

Variables

uint8_t green_color [4] = {255, 128, 255, 128}
 
uint8_t blue_color [4] = {0, 128, 0, 128}
 
uint8_t white_color [4] = {255, 255, 255, 255}
 
uint8_t color_Y_min
 
uint8_t color_Y_max
 
uint8_t color_U_min
 
uint8_t color_U_max
 
uint8_t color_V_min
 
uint8_t color_V_max
 
int min_pixel_size
 
int n_total_samples
 
struct gate_img temp_check_gate
 
struct image_t img_result
 
float best_quality = 0
 
float best_fitness = 100000
 
int * array
 

Detailed Description

Detects gates as used in the IROS drone races, i.e., square colored gates.

It does so with snake gate detection, a computationally efficient method that works onboard of the computationally constrained Parrot Bebop 1/2 drones.

An initial version of this algorithm ran in the drone race 2016. The algorithm was first described in: First autonomous multi-room exploration with an insect-inspired flapping wing vehicle, May 2018, IEEE International Conference on Robotics and Automation (ICRA 2018), Brisbane, Australia by Kirk Scheper, Matej Karasek, Christophe De Wagter, Bart Remes, and Guido de Croon https://www.researchgate.net/publication/327228053_First_autonomous_multi-room_exploration_with_an_insect-inspired_flapping_wing_vehicle

For the drone race, the algorithm and performance are described and analyzed in more detail in: Autonomous drone race: A novel vision-based navigation and control strategy, S.Li, M.M.O.I. Ozo, C. De Wagter, G.C.H.E. de Croon. Submitted.

Definition in file snake_gate_detection.c.

Macro Definition Documentation

◆ DRAW_GATE

#define DRAW_GATE   1

Definition at line 57 of file snake_gate_detection.c.

◆ FAIL_DETECT

#define FAIL_DETECT   0

Definition at line 53 of file snake_gate_detection.c.

◆ FILTER_IMAGE

#define FILTER_IMAGE   0

Definition at line 56 of file snake_gate_detection.c.

◆ SUCCESS_DETECT

#define SUCCESS_DETECT   1

Definition at line 52 of file snake_gate_detection.c.

Function Documentation

◆ check_color_snake_gate_detection()

int check_color_snake_gate_detection ( struct image_t im,
int  x,
int  y 
)

Definition at line 1102 of file snake_gate_detection.c.

References check_color_yuv422(), color_U_max, color_U_min, color_V_max, color_V_min, color_Y_max, color_Y_min, n_total_samples, gate_img::x, and gate_img::y.

Referenced by check_inside(), check_line(), refine_single_corner(), snake_gate_detection(), snake_left_and_right(), and snake_up_and_down().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

The outline should be of the right color. The inner part should be a different color, or else we may be looking at a distractor object. The gate does not yet have to have corners assigned in the struct.

Parameters
[in]imThe input image.
[in]gateThe gate to be checked.
[in]qualityThe ratio of rightly colored pixels, 1.0 is best, 0.0 is worst. If too many pixels inside the gate are of the right color, the quality is also 0.
[in]sidesHow many of the sides are sufficiently colored?

Definition at line 673 of file snake_gate_detection.c.

References check_inside(), check_line(), gate_img::sz, gate_img::sz_left, gate_img::sz_right, point_t::x, gate_img::x, point_t::y, and gate_img::y.

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

The gate must have corners already assigned in the struct.

Parameters
[in]imThe input image.
[in]gateThe gate to be checked.
[in]qualityThe ratio of rightly colored pixels, 1.0 is best, 0.0 is worst.
[in]sidesHow many of the sides are sufficiently colored?

Definition at line 581 of file snake_gate_detection.c.

References check_inside(), check_line(), min_pixel_size, segment_length(), gate_img::sz, point_t::x, gate_img::x, gate_img::x_corners, point_t::y, gate_img::y, and gate_img::y_corners.

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ check_inside()

float check_inside ( struct image_t im,
int  x,
int  y,
int  sz,
int  n_samples_in 
)

Definition at line 761 of file snake_gate_detection.c.

References check_color_snake_gate_detection(), h(), n_samples, point_t::x, and point_t::y.

Referenced by check_gate_initial(), and check_gate_outline().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

Parameters
[in]imThe input image.
[in]Q1Point 1.
[in]Q2Point 2.
[in]n_pointsThe number of sampled points.
[in]n_colored_pointsThe number of sampled points that were of the right color.

Definition at line 818 of file snake_gate_detection.c.

References check_color_snake_gate_detection(), h(), point_t::x, and point_t::y.

Referenced by check_gate_initial(), and check_gate_outline().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ cmp_i()

int cmp_i ( const void *  a,
const void *  b 
)

Definition at line 96 of file snake_gate_detection.c.

References array, and b.

◆ cmpfunc()

int cmpfunc ( const void *  a,
const void *  b 
)

Definition at line 90 of file snake_gate_detection.c.

References b.

Referenced by snake_gate_detection().

+ Here is the caller graph for this function:

◆ draw_gate()

void draw_gate ( struct image_t im,
struct gate_img  gate 
)

Draw the gate on an image.

Parameters
[in]imgThe output image.
[in]gateThe gate to be drawn.

Definition at line 478 of file snake_gate_detection.c.

References draw_gate_color_polygon(), and white_color.

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

Parameters
[in]imgThe output image.
[in]gateThe gate to be drawn.
[in]colorThe color of the lines, in UYVY format.

Definition at line 491 of file snake_gate_detection.c.

References image_draw_crosshair(), image_draw_line_color(), point_t::x, gate_img::x, gate_img::x_corners, point_t::y, gate_img::y, and gate_img::y_corners.

Referenced by draw_gate(), refine_single_corner(), and snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

Parameters
[in]imgThe output image.
[in]gateThe gate to be drawn.
[in]colorThe color of the lines, in UYVY format.

Definition at line 538 of file snake_gate_detection.c.

References image_draw_crosshair(), image_draw_line_color(), gate_img::sz, gate_img::sz_left, gate_img::sz_right, point_t::x, gate_img::x, point_t::y, and gate_img::y.

+ Here is the call graph for this function:

◆ 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.

Parameters
[in]color_imageThe color image.
[in]x_pointsThe x-coordinates of the gate. These will be updated.
[in]y_pointsThe y-coordinates of the gate. These will be updated.
[in]sizeFull size of the gate.

Definition at line 993 of file snake_gate_detection.c.

References refine_single_corner().

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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 at line 1167 of file snake_gate_detection.c.

References overlap_intervals().

Referenced by intersection_over_union().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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 at line 1131 of file snake_gate_detection.c.

References intersection_boxes().

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ overlap_intervals()

int overlap_intervals ( int  val_low_1,
int  val_high_1,
int  val_low_2,
int  val_high_2 
)

Definition at line 1183 of file snake_gate_detection.c.

Referenced by intersection_boxes().

+ Here is the caller graph for this function:

◆ 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.

Parameters
[in]imThe color image.
[in]corner_xThe corner's initial x-coordinate
[in]corner_yThe corner's initial y-coordinate
[in]sizeFull size of the gate.
[in]size_factorThe ratio of the size in which we will look for a better corner location.

Definition at line 1013 of file snake_gate_detection.c.

References check_color_snake_gate_detection(), draw_gate_color_polygon(), green_color, image_t::h, image_t::w, gate_img::x_corners, and gate_img::y_corners.

Referenced by gate_refine_corners().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ segment_length()

float segment_length ( struct point_t  Q1,
struct point_t  Q2 
)

Determine the segment length between two 2D-points.

Parameters
[in]Q1Point 1.
[in]Q2Point 2.

Definition at line 802 of file snake_gate_detection.c.

References point_t::x, and point_t::y.

Referenced by check_gate_outline().

+ Here is the caller graph for this function:

◆ 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 gate and its size, assuming a square window.

Parameters
[in]gateThe gate struct with the relevant information.

Definition at line 971 of file snake_gate_detection.c.

References gate_img::sz, gate_img::x, gate_img::x_corners, gate_img::y, and gate_img::y_corners.

Referenced by snake_gate_detection().

+ Here is the caller graph for this function:

◆ 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.

It assumes that it gets images over time, and remembers previous detections.

Snake gate takes samples from an image. If a sample corresponds to the target color, it will then go up and down, and afterwards left and right, in order to find chains of pixels of the right color. This forms the initial guess for a square approximation to the gate. Then, the estimate is refined, by looking around the supposed corner locations. This leads to a polygon gate.

Parameters
[out]successWhether a gate was detected
[in]imgThe input image. We will draw in it.
[in]n_samplesThe number of samples taken to find a gate - proportional to the computational effort and detection performance.
[in]min_px_sizeThe minimum pixel size an initial, purely square detection should have
[in]min_gate_qualityHow much percentage of the initial square outline needs to have the target color.
[in]gate_thicknessAfter snaking, how much the extreme coordinates have to be adjusted.
[in]color_ymThe Y minimum value
[in]color_yMThe Y maximum value
[in]color_umThe U minimum value
[in]color_uMThe U maximum value
[in]color_vmThe V minimum value
[in]color_vMThe V maximum value
[out]*best_gateThis gate_img struct will be filled with the data of the best detected gate.
[out]*gates_cArray of gates with size MAX_GATES
[in]exclude_topThe number of pixels excluded for sampling at the top of the image.
[in]exclude_bottomThe number of pixels excluded for sampling at the bottom of the image.

Definition at line 135 of file snake_gate_detection.c.

References best_gate, best_quality, blue_color, check_color_snake_gate_detection(), check_gate_initial(), check_gate_outline(), cmpfunc(), color_U_max, color_U_min, color_Um, color_UM, color_V_max, color_V_min, color_Vm, color_VM, color_Y_max, color_Y_min, color_Ym, color_YM, DRAW_GATE, draw_gate(), draw_gate_color_polygon(), exclude_bottom, exclude_top, FAIL_DETECT, FILTER_IMAGE, g, gate_refine_corners(), gate_thickness, gates_c, green_color, image_t::h, image_draw_crosshair(), image_yuv422_colorfilt(), intersection_over_union(), MAX_GATES, min_gate_quality, min_n_sides, min_pixel_size, min_px_size, n_samples, gate_img::n_sides, n_total_samples, gate_img::quality, set_gate_points(), snake_left_and_right(), snake_up_and_down(), SUCCESS_DETECT, gate_img::sz, gate_img::sz_left, gate_img::sz_right, image_t::w, white_color, point_t::x, gate_img::x, gate_img::x_corners, gate_img::y, and gate_img::y_corners.

Referenced by detect_gate_func().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

An "agent" starts at (x,y) and goes as far as possible left and right (x-direction), keeping a chain of "connected" target color pixels. The agent can go slightly to the top and bottom.

Parameters
[in]imThe input image.
[in]xThe initial x-coordinate
[in]yThe initial y-coordinate
[in]x_lowThe current lowest x-estimate
[in]y_lowThe y-coordinate that corresponds to the current x_low
[in]x_highThe current highest x-estimate
[in]y_highThe y-coordinate that corresponds to the current x_high

Definition at line 919 of file snake_gate_detection.c.

References check_color_snake_gate_detection(), image_t::h, point_t::x, and point_t::y.

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ 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.

An "agent" starts at (x,y) and goes as far as possible up and down (y-direction), keeping a chain of "connected" target color pixels. The agent can go slightly to the right and left.

Parameters
[in]imThe input image.
[in]xThe initial x-coordinate
[in]yThe initial y-coordinate
[in]x_lowThe x-coordinate that corresponds to the current y_low.
[in]y_lowThe current lowest y-estimate
[in]x_highThe x-coordinate that corresponds to the current y_high
[in]y_highThe current highest y-estimate

Definition at line 859 of file snake_gate_detection.c.

References check_color_snake_gate_detection(), image_t::h, h(), image_t::w, point_t::x, and point_t::y.

Referenced by snake_gate_detection().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

Variable Documentation

◆ array

◆ best_fitness

float best_fitness = 100000

Definition at line 83 of file snake_gate_detection.c.

◆ best_quality

float best_quality = 0

Definition at line 82 of file snake_gate_detection.c.

Referenced by snake_gate_detection().

◆ blue_color

uint8_t blue_color[4] = {0, 128, 0, 128}

Definition at line 61 of file snake_gate_detection.c.

Referenced by snake_gate_detection().

◆ color_U_max

uint8_t color_U_max

◆ color_U_min

uint8_t color_U_min

◆ color_V_max

uint8_t color_V_max

◆ color_V_min

uint8_t color_V_min

◆ color_Y_max

uint8_t color_Y_max

◆ color_Y_min

uint8_t color_Y_min

◆ green_color

uint8_t green_color[4] = {255, 128, 255, 128}

Definition at line 60 of file snake_gate_detection.c.

Referenced by refine_single_corner(), and snake_gate_detection().

◆ img_result

struct image_t img_result

Definition at line 77 of file snake_gate_detection.c.

◆ min_pixel_size

int min_pixel_size

Definition at line 73 of file snake_gate_detection.c.

Referenced by check_gate_outline(), and snake_gate_detection().

◆ n_total_samples

int n_total_samples

◆ temp_check_gate

struct gate_img temp_check_gate

Definition at line 77 of file snake_gate_detection.c.

◆ white_color

uint8_t white_color[4] = {255, 255, 255, 255}

Definition at line 62 of file snake_gate_detection.c.

Referenced by draw_gate(), and snake_gate_detection().