27 #ifndef DETECT_GATE_JUST_FILTER
28 #define DETECT_GATE_JUST_FILTER 0
32 #ifndef DETECT_GATE_FPS
33 #define DETECT_GATE_FPS 0
37 #ifndef DETECT_GATE_CAMERA
38 #define DETECT_GATE_CAMERA "front_camera"
42 #ifndef DETECT_GATE_N_SAMPLES
43 #define DETECT_GATE_N_SAMPLES 2000
47 #ifndef DETECT_GATE_MIN_N_SIDES
48 #define DETECT_GATE_MIN_N_SIDES 3
52 #ifndef DETECT_GATE_MIN_PIX_SIZE
53 #define DETECT_GATE_MIN_PIX_SIZE 30
57 #ifndef DETECT_GATE_MIN_GATE_QUALITY
58 #define DETECT_GATE_MIN_GATE_QUALITY 0.15
62 #ifndef DETECT_GATE_GATE_THICKNESS
63 #define DETECT_GATE_GATE_THICKNESS 0.0f
67 #ifndef DETECT_GATE_EXCLUDE_PIXELS_TOP
68 #define DETECT_GATE_EXCLUDE_PIXELS_TOP 0
72 #ifndef DETECT_GATE_EXCLUDE_PIXELS_BOTTOM
73 #define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM 0
77 #ifndef DETECT_GATE_SIMPLIFIED_PNP
78 #define DETECT_GATE_SIMPLIFIED_PNP 0
146 #if !CAMERA_ROTATED_90DEG_RIGHT
151 printf(
"\n**** START DEBUG DETECT GATE ****\n");
153 for (
int i = 0; i < n_gates; i++) {
157 #if !CAMERA_ROTATED_90DEG_RIGHT
177 printf(
"ratio = %f\n", ratio);
181 #if !CAMERA_ROTATED_90DEG_RIGHT
190 printf(
"Detected gate: ");
191 for (
int i = 0; i < 4; i++) {
199 if(simple_position) {
200 float sz1_best, sz2_best;
203 float size = (sz1_best > sz2_best) ? sz1_best : sz2_best;
206 #if !CAMERA_ROTATED_90DEG_RIGHT
222 printf(
"Not simulating, pix_x = %f, pix_y = %f\n", pix_x, pix_y);
232 printf(
"angle_x = %f, angle_y = %f, dist = %f\n", angle_x, angle_y, dist);
233 printf(
"pix_x = %f, pix_y = %f\n", pix_x, pix_y);
247 printf(
"**** END DEBUG DETECT GATE ****\n");
294 pprz_msg_send_VISION_POSITION_ESTIMATE(trans,
dev, AC_ID,
324 #if !CAMERA_ROTATED_90DEG_RIGHT
struct FloatVect3 get_world_position_from_image_points(int *x_corners, int *y_corners, struct FloatVect3 *world_corners, int n_corners, struct camera_intrinsics_t cam_intrinsics, struct FloatEulers cam_body)
Get the world position of the camera, given image coordinates and corresponding world coordinates.
Functions for solving a perspective-n-point problem, using the AHRS to get the relevant angles.
Main include for ABI (AirBorneInterface).
Convenience defines for ABI sender IDs.
#define DETECT_GATE_ABI_ID
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
#define DETECT_GATE_MIN_PIX_SIZE
static void send_detect_gate_visual_position(struct transport_tx *trans, struct link_device *dev)
#define DETECT_GATE_MIN_GATE_QUALITY
void detect_gate_event(void)
static struct image_t * detect_gate_func(struct image_t *img, uint8_t camera_id)
#define DETECT_GATE_JUST_FILTER
#define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM
struct FloatVect3 world_corners[4]
#define DETECT_GATE_N_SAMPLES
#define DETECT_GATE_FPS
Default FPS (zero means run at camera fps)
struct FloatEulers cam_body
#define DETECT_GATE_EXCLUDE_PIXELS_TOP
#define DETECT_GATE_MIN_N_SIDES
static pthread_mutex_t gate_detect_mutex
Mutex lock fo thread safety.
volatile float detect_gate_y
struct gate_img best_gate
volatile float detect_gate_z
struct gate_img gates_c[MAX_GATES]
volatile int detect_gate_has_new_data
struct FloatVect3 drone_position
struct vision_relative_position_struct detectgate_vision_position
volatile float detect_gate_x
#define DETECT_GATE_SIMPLIFIED_PNP
void detect_gate_init(void)
#define DETECT_GATE_GATE_THICKNESS
#define DETECT_GATE_CAMERA
#define VECT3_ASSIGN(_a, _x, _y, _z)
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.
Image helper functions like resizing, color filter, converters...
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Simple matrix helper macros.
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.
Detects gates as used in the IROS drone races, i.e., square colored gates.
int x_corners[4]
Array of corner x coordinates.
int y_corners[4]
Array of corner y coordinates.
float quality
gate quality
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.