detect_gate.c File Reference

#include <stdio.h>
#include "detect_gate.h"
#include "modules/computer_vision/lib/vision/image.h"
#include "modules/computer_vision/lib/vision/PnP_AHRS.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_simple_matrix.h"
#include "subsystems/abi.h"
#include "subsystems/abi_sender_ids.h"
#include "modules/computer_vision/snake_gate_detection.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for detect_gate.c:

Go to the source code of this file.

Data Structures
struct	vision_relative_position_struct

Macros
#define	DETECT_GATE_JUST_FILTER   0
#define	DETECT_GATE_FPS   0
	Default FPS (zero means run at camera fps) More...
#define	DETECT_GATE_CAMERA   "front_camera"
#define	DETECT_GATE_N_SAMPLES   2000
#define	DETECT_GATE_MIN_N_SIDES   3
#define	DETECT_GATE_MIN_PIX_SIZE   30
#define	DETECT_GATE_MIN_GATE_QUALITY   0.15
#define	DETECT_GATE_GATE_THICKNESS   0.0f
#define	DETECT_GATE_EXCLUDE_PIXELS_TOP   0
#define	DETECT_GATE_EXCLUDE_PIXELS_BOTTOM   0
#define	DETECT_GATE_SIMPLIFIED_PNP   0

Functions
static struct image_t *	detect_gate_func (struct image_t *img)
void	detect_gate_event (void)
static void	send_detect_gate_visual_position (struct transport_tx *trans, struct link_device *dev)
void	detect_gate_init (void)

Variables
int	just_filtering
int	n_samples
int	min_n_sides
int	min_px_size
float	min_gate_quality
float	gate_thickness
uint8_t	color_Ym
uint8_t	color_YM
uint8_t	color_Um
uint8_t	color_UM
uint8_t	color_Vm
uint8_t	color_VM
int	exclude_top
int	exclude_bottom
struct FloatVect3	drone_position
struct gate_img	best_gate
struct gate_img	gates_c [MAX_GATES]
struct FloatVect3	world_corners [4]
float	gate_size_m = 1.4
float	gate_center_height = 0.0
int	n_corners = 3
struct FloatEulers	cam_body
volatile int	detect_gate_has_new_data
volatile float	detect_gate_x
volatile float	detect_gate_y
volatile float	detect_gate_z
static pthread_mutex_t	gate_detect_mutex
	Mutex lock fo thread safety. More...
struct vision_relative_position_struct	detectgate_vision_position = {false, 0, 0.0f, 0.0f, 0.0f}

---

Data Structure Documentation

struct vision_relative_position_struct

Definition at line 123 of file detect_gate.c.

Data Fields
int	cnt
int	received
float	x
float	y
float	z

Macro Definition Documentation

#define DETECT_GATE_CAMERA   "front_camera"

Definition at line 38 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

#define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM   0

Definition at line 73 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_EXCLUDE_PIXELS_TOP   0

Definition at line 68 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_FPS   0

Default FPS (zero means run at camera fps)

Definition at line 33 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_GATE_THICKNESS   0.0f

Definition at line 63 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_JUST_FILTER   0

Definition at line 28 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_MIN_GATE_QUALITY   0.15

Definition at line 58 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_MIN_N_SIDES   3

Definition at line 48 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_MIN_PIX_SIZE   30

Definition at line 53 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_N_SAMPLES   2000

Definition at line 43 of file detect_gate.c.

Referenced by detect_gate_init().

#define DETECT_GATE_SIMPLIFIED_PNP   0

Definition at line 78 of file detect_gate.c.

Referenced by detect_gate_func().

Function Documentation

void detect_gate_event

(

void

)

Definition at line 264 of file detect_gate.c.

References vision_relative_position_struct::cnt, DETECT_GATE_ABI_ID, detect_gate_has_new_data, detect_gate_x, detect_gate_y, detect_gate_z, detectgate_vision_position, gate_detect_mutex, vision_relative_position_struct::received, vision_relative_position_struct::x, vision_relative_position_struct::y, and vision_relative_position_struct::z.

static struct image_t* detect_gate_func ( struct image_t *  img )

static

Definition at line 134 of file detect_gate.c.

References best_gate, cam_body, color_Um, color_UM, color_Vm, color_VM, color_Ym, color_YM, DETECT_GATE_CAMERA, detect_gate_has_new_data, DETECT_GATE_SIMPLIFIED_PNP, detect_gate_x, detect_gate_y, detect_gate_z, drone_position, exclude_bottom, exclude_top, gate_detect_mutex, gate_size_m, gate_thickness, gates_c, get_world_position_from_image_points(), image_yuv422_colorfilt(), just_filtering, min_gate_quality, min_n_sides, min_px_size, n_corners, n_samples, gate_img::quality, snake_gate_detection(), world_corners, FloatVect3::x, gate_img::x_corners, FloatVect3::y, gate_img::y_corners, and FloatVect3::z.

Referenced by detect_gate_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void detect_gate_init

(

void

)

Definition at line 305 of file detect_gate.c.

References cam_body, color_Um, color_UM, color_Vm, color_VM, color_Ym, color_YM, cv_add_to_device(), DefaultPeriodic, DETECT_GATE_CAMERA, DETECT_GATE_EXCLUDE_PIXELS_BOTTOM, DETECT_GATE_EXCLUDE_PIXELS_TOP, DETECT_GATE_FPS, detect_gate_func(), DETECT_GATE_GATE_THICKNESS, detect_gate_has_new_data, DETECT_GATE_JUST_FILTER, DETECT_GATE_MIN_GATE_QUALITY, DETECT_GATE_MIN_N_SIDES, DETECT_GATE_MIN_PIX_SIZE, DETECT_GATE_N_SAMPLES, detect_gate_x, detect_gate_y, detect_gate_z, exclude_bottom, exclude_top, gate_center_height, gate_detect_mutex, gate_size_m, gate_thickness, just_filtering, min_gate_quality, min_n_sides, min_px_size, n_samples, FloatEulers::phi, FloatEulers::psi, register_periodic_telemetry(), send_detect_gate_visual_position(), FloatEulers::theta, VECT3_ASSIGN, and world_corners.

Here is the call graph for this function:

static void send_detect_gate_visual_position ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 288 of file detect_gate.c.

References vision_relative_position_struct::cnt, detectgate_vision_position, foo, vision_relative_position_struct::received, vision_relative_position_struct::x, vision_relative_position_struct::y, and vision_relative_position_struct::z.

Referenced by detect_gate_init().

Here is the caller graph for this function:

Variable Documentation

struct gate_img best_gate

Definition at line 101 of file detect_gate.c.

Referenced by detect_gate_func().

struct FloatEulers cam_body

Definition at line 112 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and get_world_position_from_image_points().

uint8_t color_Um

Definition at line 92 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

uint8_t color_UM

Definition at line 93 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

uint8_t color_Vm

Definition at line 94 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

uint8_t color_VM

Definition at line 95 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

uint8_t color_Ym

Definition at line 90 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

uint8_t color_YM

Definition at line 91 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

volatile int detect_gate_has_new_data

Definition at line 115 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

volatile float detect_gate_x

Definition at line 116 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

volatile float detect_gate_y

Definition at line 117 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

volatile float detect_gate_z

Definition at line 118 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

struct vision_relative_position_struct detectgate_vision_position = {false, 0, 0.0f, 0.0f, 0.0f}

Referenced by detect_gate_event(), and send_detect_gate_visual_position().

struct FloatVect3 drone_position

Definition at line 100 of file detect_gate.c.

Referenced by detect_gate_func().

int exclude_bottom

Definition at line 97 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

int exclude_top

Definition at line 96 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

float gate_center_height = 0.0

Definition at line 108 of file detect_gate.c.

Referenced by detect_gate_init().

pthread_mutex_t gate_detect_mutex

static

Mutex lock fo thread safety.

Definition at line 120 of file detect_gate.c.

Referenced by detect_gate_event(), detect_gate_func(), and detect_gate_init().

float gate_size_m = 1.4

Definition at line 107 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

float gate_thickness

Definition at line 89 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

struct gate_img gates_c[MAX_GATES]

Definition at line 103 of file detect_gate.c.

Referenced by detect_gate_func().

int just_filtering

Definition at line 84 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

float min_gate_quality

Definition at line 88 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

int min_n_sides

Definition at line 86 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().

int min_px_size

Definition at line 87 of file detect_gate.c.

Referenced by detect_gate_func(), detect_gate_init(), and snake_gate_detection().

int n_corners = 3

Definition at line 109 of file detect_gate.c.

Referenced by detect_gate_func(), and get_world_position_from_image_points().

int n_samples

Definition at line 85 of file detect_gate.c.

Referenced by calc_fast9_lukas_kanade(), check_inside(), detect_gate_func(), detect_gate_init(), fit_linear_flow_field(), fit_linear_model(), fit_linear_model_prior(), get_indices_without_replacement(), get_size_divergence(), and RANSAC_linear_model().

struct FloatVect3 world_corners[4]

Definition at line 106 of file detect_gate.c.

Referenced by detect_gate_func(), and detect_gate_init().