#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:

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]	im	The input image.
[in]	gate	The gate to be checked.
[in]	quality	The 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]	sides	How 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, gate_img::x, point_t::x, gate_img::y, 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:

◆ 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]	im	The input image.
[in]	gate	The gate to be checked.
[in]	quality	The ratio of rightly colored pixels, 1.0 is best, 0.0 is worst.
[in]	sides	How 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, gate_img::x, point_t::x, gate_img::x_corners, gate_img::y, point_t::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]	im	The input image.
[in]	Q1	Point 1.
[in]	Q2	Point 2.
[in]	n_points	The number of sampled points.
[in]	n_colored_points	The 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(), f, 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]	img	The output image.
[in]	gate	The 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]	img	The output image.
[in]	gate	The gate to be drawn.
[in]	color	The 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(), gate_img::x, point_t::x, gate_img::x_corners, gate_img::y, point_t::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]	img	The output image.
[in]	gate	The gate to be drawn.
[in]	color	The 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, gate_img::x, point_t::x, gate_img::y, and point_t::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_image	The color image.
[in]	x_points	The x-coordinates of the gate. These will be updated.
[in]	y_points	The y-coordinates of the gate. These will be updated.
[in]	size	Full 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]	im	The color image.
[in]	corner_x	The corner's initial x-coordinate
[in]	corner_y	The corner's initial y-coordinate
[in]	size	Full size of the gate.
[in]	size_factor	The 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]	Q1	Point 1.
[in]	Q2	Point 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] gate The 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]	success	Whether a gate was detected
[in]	img	The input image. We will draw in it.
[in]	n_samples	The number of samples taken to find a gate - proportional to the computational effort and detection performance.
[in]	min_px_size	The minimum pixel size an initial, purely square detection should have
[in]	min_gate_quality	How much percentage of the initial square outline needs to have the target color.
[in]	gate_thickness	After snaking, how much the extreme coordinates have to be adjusted.
[in]	color_ym	The Y minimum value
[in]	color_yM	The Y maximum value
[in]	color_um	The U minimum value
[in]	color_uM	The U maximum value
[in]	color_vm	The V minimum value
[in]	color_vM	The V maximum value
[out]	*best_gate	This gate_img struct will be filled with the data of the best detected gate.
[out]	*gates_c	Array of gates with size MAX_GATES
[in]	exclude_top	The number of pixels excluded for sampling at the top of the image.
[in]	exclude_bottom	The 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, 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, gate_img::x, point_t::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]	im	The input image.
[in]	x	The initial x-coordinate
[in]	y	The initial y-coordinate
[in]	x_low	The current lowest x-estimate
[in]	y_low	The y-coordinate that corresponds to the current x_low
[in]	x_high	The current highest x-estimate
[in]	y_high	The 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]	im	The input image.
[in]	x	The initial x-coordinate
[in]	y	The initial y-coordinate
[in]	x_low	The x-coordinate that corresponds to the current y_low.
[in]	y_low	The current lowest y-estimate
[in]	x_high	The x-coordinate that corresponds to the current y_high
[in]	y_high	The 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

int* array

Definition at line 94 of file snake_gate_detection.c.

Referenced by add_array_to_buffer(), cmp_i(), is_sequence_in_array(), mean_f(), mean_i(), seq_in_array(), sum_f(), variance_f(), and variance_i().

◆ 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

Definition at line 68 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

◆ color_U_min

uint8_t color_U_min

Definition at line 67 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

◆ color_V_max

uint8_t color_V_max

Definition at line 70 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

◆ color_V_min

uint8_t color_V_min

Definition at line 69 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

◆ color_Y_max

uint8_t color_Y_max

Definition at line 66 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

◆ color_Y_min

uint8_t color_Y_min

Definition at line 65 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

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

Definition at line 77 of file snake_gate_detection.c.

Referenced by check_color_snake_gate_detection(), and snake_gate_detection().

◆ temp_check_gate

struct gate_img temp_check_gate

Definition at line 80 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().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ DRAW_GATE

◆ FAIL_DETECT

◆ FILTER_IMAGE

◆ SUCCESS_DETECT

Function Documentation

◆ check_color_snake_gate_detection()

◆ check_gate_initial()

◆ check_gate_outline()

◆ check_inside()

◆ check_line()

◆ cmp_i()

◆ cmpfunc()

◆ draw_gate()

◆ draw_gate_color_polygon()

◆ draw_gate_color_square()

◆ gate_refine_corners()

◆ intersection_boxes()

◆ intersection_over_union()

◆ overlap_intervals()

◆ refine_single_corner()

◆ segment_length()

◆ set_gate_points()

◆ snake_gate_detection()

◆ snake_left_and_right()

◆ snake_up_and_down()

Variable Documentation

◆ array

◆ best_fitness

◆ best_quality

◆ blue_color

◆ color_U_max

◆ color_U_min

◆ color_V_max

◆ color_V_min

◆ color_Y_max

◆ color_Y_min

◆ green_color

◆ img_result

◆ min_pixel_size

◆ n_total_samples

◆ temp_check_gate

◆ white_color