Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
detect_gate.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2018, Guido de Croon
3 *
4 * @file modules/computer_vision/undistort_image.c
5 */
6
7// Own header
8#include <stdio.h>
9#include "detect_gate.h"
11
12// For solving the Persepctive n Point problem (PnP):
14#include "math/pprz_algebra.h"
17#include "modules/core/abi.h"
19
21
23
24
25//#define DEBUG_GATE
26
27#ifndef DETECT_GATE_JUST_FILTER
28#define DETECT_GATE_JUST_FILTER 0
29#endif
31
32#ifndef DETECT_GATE_FPS
33#define DETECT_GATE_FPS 0
34#endif
36
37#ifndef DETECT_GATE_CAMERA
38#define DETECT_GATE_CAMERA "front_camera"
39#endif
41
42#ifndef DETECT_GATE_N_SAMPLES
43#define DETECT_GATE_N_SAMPLES 2000
44#endif
46
47#ifndef DETECT_GATE_MIN_N_SIDES
48#define DETECT_GATE_MIN_N_SIDES 3
49#endif
51
52#ifndef DETECT_GATE_MIN_PIX_SIZE
53#define DETECT_GATE_MIN_PIX_SIZE 30
54#endif
56
57#ifndef DETECT_GATE_MIN_GATE_QUALITY
58#define DETECT_GATE_MIN_GATE_QUALITY 0.15
59#endif
61
62#ifndef DETECT_GATE_GATE_THICKNESS
63#define DETECT_GATE_GATE_THICKNESS 0.0f
64#endif
66
67#ifndef DETECT_GATE_EXCLUDE_PIXELS_TOP
68#define DETECT_GATE_EXCLUDE_PIXELS_TOP 0
69#endif
71
72#ifndef DETECT_GATE_EXCLUDE_PIXELS_BOTTOM
73#define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM 0
74#endif
76
77#ifndef DETECT_GATE_SIMPLIFIED_PNP
78#define DETECT_GATE_SIMPLIFIED_PNP 0
79#endif
81
82
83// settings:
98
99// External variables that have the results:
102// container for all detected gates:
104
105// Structure of the gate:
107float gate_size_m = 1.4; //size of gate edges in meters
108float gate_center_height = 0.0; //height of gate in meters ned wrt ground
109int n_corners = 3;
110
111// camera to body:
113
114// Shared data between thread and main
119
121
122// Module variables
125 int cnt;
126 float x;
127 float y;
128 float z;
129} detectgate_vision_position = {false, 0, 0.0f, 0.0f, 0.0f};
130
131
132
133// Function
134static struct image_t *detect_gate_func(struct image_t *img, uint8_t camera_id __attribute__((unused)))
135{
136 // detect the gate and draw it in the image:
137 if (just_filtering) {
138 // just color filter the image, so that the user can tune the thresholds:
140 } else {
141 // perform snake gate detection:
142 int n_gates;
145
146#if !CAMERA_ROTATED_90DEG_RIGHT
147 int temp[4];
148#endif
149
150#ifdef DEBUG_GATE
151 printf("\n**** START DEBUG DETECT GATE ****\n");
152 if (n_gates > 1) {
153 for (int i = 0; i < n_gates; i++) {
154 //printf("Gate %d out of %d\n", i, n_gates-1);
155 if (gates_c[i].quality > min_gate_quality * 2 && gates_c[i].n_sides >= 3) {
156
157#if !CAMERA_ROTATED_90DEG_RIGHT
158 // swap x and y coordinates:
159 memcpy(temp, gates_c[i].x_corners, sizeof(gates_c[i].x_corners));
160 memcpy(gates_c[i].x_corners, gates_c[i].y_corners, sizeof(gates_c[i].x_corners));
161 memcpy(gates_c[i].y_corners, temp, sizeof(gates_c[i].y_corners));
162#endif
163
165 n_corners,
166 DETECT_GATE_CAMERA.camera_intrinsics, cam_body);
167 // debugging the drone position:
168 printf("Position drone - gate %d, quality = %f: (%f, %f, %f)\n", i, gates_c[i].quality, drone_position.x,
170 }
171 }
172 }
173#endif
174
175
176#ifdef DEBUG_GATE
177 printf("ratio = %f\n", ratio);
178#endif
180
181#if !CAMERA_ROTATED_90DEG_RIGHT
182 // swap x and y coordinates:
186#endif
187
188#ifdef DEBUG_GATE
189 // debugging snake gate:
190 printf("Detected gate: ");
191 for (int i = 0; i < 4; i++) {
192 printf("(%d,%d) ", best_gate.x_corners[i], best_gate.y_corners[i]);
193 }
194 printf("\n");
195#endif
196
198
199 if(simple_position) {
200 float sz1_best, sz2_best;
203 float size = (sz1_best > sz2_best) ? sz1_best : sz2_best;
204
205 //float width, height;
206#if !CAMERA_ROTATED_90DEG_RIGHT
207 //width = (float) img->w;
208 //height = (float) img->h;
209 float pix_x = (best_gate.x_corners[2] + best_gate.x_corners[0]) / 2.0f;
210 float pix_y = (best_gate.y_corners[1] + best_gate.y_corners[0]) / 2.0f;
211 float angle_x = (pix_x-DETECT_GATE_CAMERA.camera_intrinsics.center_x) / DETECT_GATE_CAMERA.camera_intrinsics.focal_x;
212 float angle_y = (pix_y-DETECT_GATE_CAMERA.camera_intrinsics.center_y) / DETECT_GATE_CAMERA.camera_intrinsics.focal_y;
213 float dist = gate_size_m * (DETECT_GATE_CAMERA.camera_intrinsics.focal_x / size);
214 drone_position.x = -dist;
215 drone_position.y = -angle_y*dist;
216 drone_position.z = angle_x*dist;
217#else
218 //width = (float) img->h;
219 //height = (float) img->w;
220 float pix_y = (best_gate.x_corners[1] + best_gate.x_corners[0]) / 2.0f;
221 float pix_x = (best_gate.y_corners[2] + best_gate.y_corners[1]) / 2.0f;
222 printf("Not simulating, pix_x = %f, pix_y = %f\n", pix_x, pix_y);
223 float angle_x = (pix_x-DETECT_GATE_CAMERA.camera_intrinsics.center_y) / DETECT_GATE_CAMERA.camera_intrinsics.focal_y;
224 float angle_y = (pix_y-DETECT_GATE_CAMERA.camera_intrinsics.center_x) / DETECT_GATE_CAMERA.camera_intrinsics.focal_x;
225 float dist = gate_size_m * (DETECT_GATE_CAMERA.camera_intrinsics.focal_x / size);
226 drone_position.x = -dist;
227 drone_position.y = -angle_x*dist;
228 drone_position.z = -angle_y*dist;
229#endif
230
231#ifdef DEBUG_GATE
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);
234 printf("size = %f, focal = %f, %f, center = %f, %f\n", size, DETECT_GATE_CAMERA.camera_intrinsics.focal_x, DETECT_GATE_CAMERA.camera_intrinsics.focal_y,
235 DETECT_GATE_CAMERA.camera_intrinsics.center_x, DETECT_GATE_CAMERA.camera_intrinsics.center_y);
236#endif
237 }
238 else {
239 // TODO: try out RANSAC with all combinations of 3 corners out of 4 corners.
241 n_corners, DETECT_GATE_CAMERA.camera_intrinsics, cam_body);
242 }
243
244#ifdef DEBUG_GATE
245 // debugging the drone position:
246 printf("Position drone: (%f, %f, %f)\n", drone_position.x, drone_position.y, drone_position.z);
247 printf("**** END DEBUG DETECT GATE ****\n");
248#endif
249
250 // send from thread to module - only when there is a best gate:
255 //printf("new measurement!!\n");
258 }
259
260 }
261 return img;
262}
263
286
287
302
303
304
306{
307 // settings:
322
323 // World coordinates: X positive towards the gate, Z positive down, Y positive right:
324#if !CAMERA_ROTATED_90DEG_RIGHT
325 // Top-left, CW:
327 0.0f, -(gate_size_m / 2), gate_center_height - (gate_size_m / 2));
329 0.0f, (gate_size_m / 2), gate_center_height - (gate_size_m / 2));
331 0.0f, (gate_size_m / 2), gate_center_height + (gate_size_m / 2));
333 0.0f, -(gate_size_m / 2), gate_center_height + (gate_size_m / 2));
334
335#else
336 // Bottom-right, CCW:
338 0.0f, (gate_size_m / 2), gate_center_height + (gate_size_m / 2));
340 0.0f, (gate_size_m / 2), gate_center_height - (gate_size_m / 2));
342 0.0f, -(gate_size_m / 2), gate_center_height - (gate_size_m / 2));
344 0.0f, -(gate_size_m / 2), gate_center_height + (gate_size_m / 2));
345#endif
346 cam_body.phi = 0;
347 cam_body.theta = 0;
348 cam_body.psi = 0;
349
350 // Shared variables to copy data from thread to module
353 detect_gate_x = 0;
354 detect_gate_y = 0;
355 detect_gate_z = 0;
356
358
360}
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.
Definition PnP_AHRS.c:54
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)
Definition cv.c:46
int n_samples
Definition detect_gate.c:85
#define DETECT_GATE_MIN_PIX_SIZE
Definition detect_gate.c:53
static void send_detect_gate_visual_position(struct transport_tx *trans, struct link_device *dev)
float gate_center_height
uint8_t color_Um
Definition detect_gate.c:92
#define DETECT_GATE_MIN_GATE_QUALITY
Definition detect_gate.c:58
void detect_gate_event(void)
float gate_thickness
Definition detect_gate.c:89
#define DETECT_GATE_JUST_FILTER
Definition detect_gate.c:28
uint8_t color_Vm
Definition detect_gate.c:94
#define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM
Definition detect_gate.c:73
struct FloatVect3 world_corners[4]
#define DETECT_GATE_N_SAMPLES
Definition detect_gate.c:43
#define DETECT_GATE_FPS
Default FPS (zero means run at camera fps)
Definition detect_gate.c:33
uint8_t color_VM
Definition detect_gate.c:95
struct FloatEulers cam_body
#define DETECT_GATE_EXCLUDE_PIXELS_TOP
Definition detect_gate.c:68
#define DETECT_GATE_MIN_N_SIDES
Definition detect_gate.c:48
int min_px_size
Definition detect_gate.c:87
static pthread_mutex_t gate_detect_mutex
Mutex lock fo thread safety.
uint8_t color_YM
Definition detect_gate.c:91
volatile float detect_gate_y
struct gate_img best_gate
volatile float detect_gate_z
struct gate_img gates_c[MAX_GATES]
int just_filtering
Definition detect_gate.c:84
volatile int detect_gate_has_new_data
struct FloatVect3 drone_position
struct vision_relative_position_struct detectgate_vision_position
int exclude_top
Definition detect_gate.c:96
volatile float detect_gate_x
int exclude_bottom
Definition detect_gate.c:97
static struct image_t * detect_gate_func(struct image_t *img, uint8_t camera_id)
float min_gate_quality
Definition detect_gate.c:88
uint8_t color_UM
Definition detect_gate.c:93
#define DETECT_GATE_SIMPLIFIED_PNP
Definition detect_gate.c:78
int min_n_sides
Definition detect_gate.c:86
float gate_size_m
uint8_t color_Ym
Definition detect_gate.c:90
void detect_gate_init(void)
#define DETECT_GATE_GATE_THICKNESS
Definition detect_gate.c:63
int n_corners
#define DETECT_GATE_CAMERA
Definition detect_gate.c:38
float phi
in radians
float theta
in radians
float psi
in radians
euler angles
#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.
Definition image.c:173
Image helper functions like resizing, color filter, converters...
uint16_t foo
Definition main_demo5.c:58
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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.
#define MAX_GATES
float quality
gate quality
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.