Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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
30 PRINT_CONFIG_VAR(DETECT_GATE_JUST_FILTER)
31 
32 #ifndef DETECT_GATE_FPS
33 #define DETECT_GATE_FPS 0
34 #endif
35 PRINT_CONFIG_VAR(DETECT_GATE_FPS)
36 
37 #ifndef DETECT_GATE_CAMERA
38 #define DETECT_GATE_CAMERA "front_camera"
39 #endif
40 PRINT_CONFIG_VAR(DETECT_GATE_CAMERA)
41 
42 #ifndef DETECT_GATE_N_SAMPLES
43 #define DETECT_GATE_N_SAMPLES 2000
44 #endif
45 PRINT_CONFIG_VAR(DETECT_GATE_N_SAMPLES)
46 
47 #ifndef DETECT_GATE_MIN_N_SIDES
48 #define DETECT_GATE_MIN_N_SIDES 3
49 #endif
50 PRINT_CONFIG_VAR(DETECT_GATE_MIN_N_SIDES)
51 
52 #ifndef DETECT_GATE_MIN_PIX_SIZE
53 #define DETECT_GATE_MIN_PIX_SIZE 30
54 #endif
55 PRINT_CONFIG_VAR(DETECT_GATE_MIN_PIX_SIZE)
56 
57 #ifndef DETECT_GATE_MIN_GATE_QUALITY
58 #define DETECT_GATE_MIN_GATE_QUALITY 0.15
59 #endif
60 PRINT_CONFIG_VAR(DETECT_GATE_MIN_GATE_QUALITY)
61 
62 #ifndef DETECT_GATE_GATE_THICKNESS
63 #define DETECT_GATE_GATE_THICKNESS 0.0f
64 #endif
65 PRINT_CONFIG_VAR(DETECT_GATE_GATE_THICKNESS)
66 
67 #ifndef DETECT_GATE_EXCLUDE_PIXELS_TOP
68 #define DETECT_GATE_EXCLUDE_PIXELS_TOP 0
69 #endif
70 PRINT_CONFIG_VAR(DETECT_GATE_EXCLUDE_PIXELS_TOP)
71 
72 #ifndef DETECT_GATE_EXCLUDE_PIXELS_BOTTOM
73 #define DETECT_GATE_EXCLUDE_PIXELS_BOTTOM 0
74 #endif
75 PRINT_CONFIG_VAR(DETECT_GATE_EXCLUDE_PIXELS_BOTTOM)
76 
77 #ifndef DETECT_GATE_SIMPLIFIED_PNP
78 #define DETECT_GATE_SIMPLIFIED_PNP 0
79 #endif
80 PRINT_CONFIG_VAR(DETECT_GATE_SIMPLIFIED_PNP)
81 
82 
83 // settings:
98 
99 // External variables that have the results:
101 struct gate_img best_gate;
102 // container for all detected gates:
103 struct gate_img gates_c[MAX_GATES];
104 
105 // Structure of the gate:
106 struct FloatVect3 world_corners[4];
107 float gate_size_m = 1.4; //size of gate edges in meters
108 float gate_center_height = 0.0; //height of gate in meters ned wrt ground
109 int n_corners = 3;
110 
111 // camera to body:
112 struct FloatEulers cam_body;
113 
114 // Shared data between thread and main
116 volatile float detect_gate_x;
117 volatile float detect_gate_y;
118 volatile float detect_gate_z;
119 
120 static pthread_mutex_t gate_detect_mutex;
121 
122 // Module variables
124  int received;
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
134 static 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
179  if (best_gate.quality > min_gate_quality * 2) {
180 
181 #if !CAMERA_ROTATED_90DEG_RIGHT
182  // swap x and y coordinates:
183  memcpy(temp, best_gate.x_corners, sizeof(best_gate.x_corners));
185  memcpy(best_gate.y_corners, temp, sizeof(best_gate.y_corners));
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 
197  static bool simple_position = DETECT_GATE_SIMPLIFIED_PNP;
198 
199  if(simple_position) {
200  float sz1_best, sz2_best;
201  sz1_best = (float) (best_gate.x_corners[2] - best_gate.x_corners[0]);
202  sz2_best = (float) (best_gate.y_corners[1] - best_gate.y_corners[0]);
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:
251  pthread_mutex_lock(&gate_detect_mutex);
255  //printf("new measurement!!\n");
257  pthread_mutex_unlock(&gate_detect_mutex);
258  }
259 
260  }
261  return img;
262 }
263 
265 {
266  pthread_mutex_lock(&gate_detect_mutex);
268  detect_gate_has_new_data = false;
269 
275 
276  AbiSendMsgRELATIVE_LOCALIZATION(DETECT_GATE_ABI_ID, detectgate_vision_position.cnt,
280  0,
281  0,
282  0);
283  }
284  pthread_mutex_unlock(&gate_detect_mutex);
285 }
286 
287 
288 static void send_detect_gate_visual_position(struct transport_tx *trans, struct link_device *dev)
289 {
293  float foo = 0;
294  pprz_msg_send_VISION_POSITION_ESTIMATE(trans, dev, AC_ID,
295  &cnt,
299  &foo, &foo );
300  }
301 }
302 
303 
304 
306 {
307  // settings:
314  color_Ym = DETECT_GATE_Y_MIN;
315  color_YM = DETECT_GATE_Y_MAX;
316  color_Um = DETECT_GATE_U_MIN;
317  color_UM = DETECT_GATE_U_MAX;
318  color_Vm = DETECT_GATE_V_MIN;
319  color_VM = DETECT_GATE_V_MAX;
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
351  pthread_mutex_init(&gate_detect_mutex, NULL);
352  detect_gate_has_new_data = false;
353  detect_gate_x = 0;
354  detect_gate_y = 0;
355  detect_gate_z = 0;
356 
358 
359  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VISION_POSITION_ESTIMATE, send_detect_gate_visual_position);
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)
Definition: detect_gate.c:288
float gate_center_height
Definition: detect_gate.c:108
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)
Definition: detect_gate.c:264
static struct image_t * detect_gate_func(struct image_t *img, uint8_t camera_id)
Definition: detect_gate.c:134
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]
Definition: detect_gate.c:106
#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
Definition: detect_gate.c:112
#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.
Definition: detect_gate.c:120
uint8_t color_YM
Definition: detect_gate.c:91
volatile float detect_gate_y
Definition: detect_gate.c:117
struct gate_img best_gate
Definition: detect_gate.c:101
volatile float detect_gate_z
Definition: detect_gate.c:118
struct gate_img gates_c[MAX_GATES]
Definition: detect_gate.c:103
int just_filtering
Definition: detect_gate.c:84
volatile int detect_gate_has_new_data
Definition: detect_gate.c:115
struct FloatVect3 drone_position
Definition: detect_gate.c:100
struct vision_relative_position_struct detectgate_vision_position
int exclude_top
Definition: detect_gate.c:96
volatile float detect_gate_x
Definition: detect_gate.c:116
int exclude_bottom
Definition: detect_gate.c:97
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
Definition: detect_gate.c:107
uint8_t color_Ym
Definition: detect_gate.c:90
void detect_gate_init(void)
Definition: detect_gate.c:305
#define DETECT_GATE_GATE_THICKNESS
Definition: detect_gate.c:63
int n_corners
Definition: detect_gate.c:109
#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)
Definition: pprz_algebra.h:125
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...
Definition: image.h:44
uint16_t foo
Definition: main_demo5.c:58
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.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98