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)
{
static int last_frame_detection = 0;
static int repeat_gate = 0;
static struct gate_img previous_best_gate = {0};
static struct gate_img last_gate;
bool check_initial_square = true;
float iou_threshold = 0.7; // when bigger than this, gates are assumed to represent the same gate
(*n_gates) = 0;
// For a new image, set the total number of samples to 0:
// This number is augmented when checking the color of a pixel.
n_total_samples = 0;
color_Y_min = color_Ym;
color_Y_max = color_YM;
color_U_min = color_Um;
color_U_max = color_UM;
color_V_min = color_Vm;
color_V_max = color_VM;
min_pixel_size = min_px_size;
int x, y;
best_quality = 0;
best_gate->quality = 0;
(*n_gates) = 0;
// variables for snake gate detection:
int y_low = 0;
int x_l = 0;
int y_high = 0;
int x_h = 0;
int x_low1 = 0;
int y_l1 = 0;
int x_high1 = 0;
int y_h1 = 0;
int x_low2 = 0;
int y_l2 = 0;
int x_high2 = 0;
int y_h2 = 0;
int sz = 0;
int szx1 = 0;
int szx2 = 0;
//for (int i = 0; i < n_samples; i++) {
while (n_total_samples < n_samples) {
// TODO: would it work better to scan different lines in the image?
// get a random coordinate:
x = rand() % img->h;
y = rand() % img->w;
// check if it has the right color
if (check_color_snake_gate_detection(img, x, y)) {
// fill histogram (TODO: for a next pull request, in which we add the close-by histogram-detection)
// histogram[x]++;
// snake up and down:
snake_up_and_down(img, x, y, &x_l, &y_low, &x_h, &y_high);
// This assumes the gate to be square:
sz = y_high - y_low;
y_low = y_low + (sz * gate_thickness);
y_high = y_high - (sz * gate_thickness);
y = (y_high + y_low) / 2;
// if the found part of the gate is large enough
if (sz > min_pixel_size) {
// snake left and right, both for the top and bottom part of the gate:
snake_left_and_right(img, x_l, y_low, &x_low1, &y_l1, &x_high1, &y_h1);
snake_left_and_right(img, x_h, y_high, &x_low2, &y_l2, &x_high2, &y_h2);
x_low1 = x_low1 + (sz * gate_thickness);
x_high1 = x_high1 - (sz * gate_thickness);
x_low2 = x_low2 + (sz * gate_thickness);
x_high2 = x_high2 - (sz * gate_thickness);
// sizes of the left-right stretches: in y pixel coordinates
szx1 = (x_high1 - x_low1);
szx2 = (x_high2 - x_low2);
// set the size according to the biggest detection:
if (szx1 > szx2) {
// determine the center x based on the bottom part:
x = (x_high1 + x_low1) / 2;
// set the size to the largest line found:
sz = (sz > szx1) ? sz : szx1;
} else {
// determine the center x based on the top part:
x = (x_high2 + x_low2) / 2;
// set the size to the largest line found:
sz = (sz > szx2) ? sz : szx2;
}
if (sz > min_pixel_size) {
// create the gate:
gates_c[(*n_gates)].x = x;
gates_c[(*n_gates)].y = y;
// store the half gate size:
gates_c[(*n_gates)].sz = sz / 2;
if (check_initial_square) {
// check the gate quality:
check_gate_initial(img, gates_c[(*n_gates)], &gates_c[(*n_gates)].quality, &gates_c[(*n_gates)].n_sides);
} else {
// The first two corners have a high y:
gates_c[(*n_gates)].x_corners[0] = x_low2;
gates_c[(*n_gates)].y_corners[0] = y_l2;
gates_c[(*n_gates)].x_corners[1] = x_high2;
gates_c[(*n_gates)].y_corners[1] = y_h2;
// The third and fourth corner have a low y:
gates_c[(*n_gates)].x_corners[2] = x_high1;
gates_c[(*n_gates)].y_corners[2] = y_h1;
gates_c[(*n_gates)].x_corners[3] = x_low1;
gates_c[(*n_gates)].y_corners[3] = y_l1;
// check the polygon:
check_gate_outline(img, gates_c[(*n_gates)], &gates_c[(*n_gates)].quality, &gates_c[(*n_gates)].n_sides);
}
if (gates_c[(*n_gates)].quality > best_quality) {
best_quality = gates_c[(*n_gates)].quality;
}
// set the corners to make a square gate for now:
set_gate_points(&gates_c[(*n_gates)]);
// TODO: for multiple gates we need to use intersection of union and fitness here.
bool add_gate = true;
float iou;
for (int g = 0; g < (*n_gates); g++) {
iou = intersection_over_union(gates_c[g].x_corners, gates_c[g].y_corners, gates_c[(*n_gates)].x_corners,
gates_c[(*n_gates)].y_corners);
if (iou > iou_threshold) {
// we are looking at an existing gate:
add_gate = false;
if (gates_c[g].quality > gates_c[(*n_gates)].quality) {
// throw the current gate away:
break;
} else {
// throw the old gate away:
// TODO: consider making a function for doing this "deep" copy
gates_c[g].x = gates_c[(*n_gates)].x;
gates_c[g].y = gates_c[(*n_gates)].y;
gates_c[g].sz = gates_c[(*n_gates)].sz;
gates_c[g].quality = gates_c[(*n_gates)].quality;
memcpy(gates_c[g].x_corners, gates_c[(*n_gates)].x_corners, sizeof(int) * 4);
memcpy(gates_c[g].y_corners, gates_c[(*n_gates)].y_corners, sizeof(int) * 4);
}
}
}
if (add_gate) {
(*n_gates)++;
}
}
if ((*n_gates) >= MAX_GATES) {
break;
}
}
}
}
#ifdef DEBUG_SNAKE_GATE
// draw all candidates:
printf("(*n_gates):%d\n", (*n_gates));
for (int i = 0; i < (*n_gates); i++) {
//draw_gate_color_square(img, gates_c[i], white_color);
draw_gate_color_polygon(img, gates_c[i], white_color);
}
#endif
//init best gate
best_gate->quality = 0;
best_gate->n_sides = 0;
repeat_gate = 0;
// do an additional fit to improve the gate detection:
if ((best_quality > min_gate_quality && (*n_gates) > 0) || last_frame_detection) {
// go over all remaining gates:
for (int gate_nr = 0; gate_nr < (*n_gates); gate_nr++) {
// get gate information:
gate_refine_corners(img, gates_c[gate_nr].x_corners, gates_c[gate_nr].y_corners, gates_c[gate_nr].sz);
// also get the color fitness
check_gate_outline(img, gates_c[gate_nr], &gates_c[gate_nr].quality, &gates_c[gate_nr].n_sides);
// If the gate is good enough:
if (gates_c[gate_nr].n_sides >= min_n_sides && gates_c[gate_nr].quality > best_gate->quality) {
// store the information in the gate:
best_gate->x = gates_c[gate_nr].x;
best_gate->y = gates_c[gate_nr].y;
best_gate->sz = gates_c[gate_nr].sz;
best_gate->sz_left = gates_c[gate_nr].sz_left;
best_gate->sz_right = gates_c[gate_nr].sz_right;
best_gate->quality = gates_c[gate_nr].quality;
best_gate->n_sides = gates_c[gate_nr].n_sides;
memcpy(best_gate->x_corners, gates_c[gate_nr].x_corners, sizeof(best_gate->x_corners));
memcpy(best_gate->y_corners, gates_c[gate_nr].y_corners, sizeof(best_gate->y_corners));
}
}
// if the best gate is not good enough, but we did have a detection in the previous image:
if ((best_gate->quality == 0 && best_gate->n_sides == 0) && last_frame_detection == 1) {
// TODO: is it really important to do this sorting here to get the maximum size? Is the sz property not accurate enough?
// Or can we not assume the standard arrangement of the corners?
int x_values[4];
int y_values[4];
memcpy(x_values, last_gate.x_corners, sizeof(x_values));
memcpy(y_values, last_gate.y_corners, sizeof(y_values));
//sort small to large
qsort(x_values, 4, sizeof(int), cmpfunc);
qsort(y_values, 4, sizeof(int), cmpfunc);
//check x size, maybe use y also later?
int radius_p = x_values[3] - x_values[0];
// TODO: is 2*radius_p not huge?
gate_refine_corners(img, last_gate.x_corners, last_gate.y_corners, 2 * radius_p);
// also get the color fitness
check_gate_outline(img, last_gate, &last_gate.quality, &last_gate.n_sides);
// if the refined detection is good enough:
if (last_gate.n_sides >= min_n_sides && last_gate.quality > best_gate->quality) {
repeat_gate = 1;
best_gate->quality = last_gate.quality;
best_gate->n_sides = last_gate.n_sides;
memcpy(best_gate->x_corners, last_gate.x_corners, sizeof(best_gate->x_corners));
memcpy(best_gate->y_corners, last_gate.y_corners, sizeof(best_gate->y_corners));
}
}
#ifdef DEBUG_SNAKE_GATE
// draw the best gate:
draw_gate(img, (*best_gate));
#endif
}
// prepare for the next time:
previous_best_gate.x = best_gate->x;
previous_best_gate.y = best_gate->y;
previous_best_gate.sz = best_gate->sz;
previous_best_gate.sz_left = best_gate->sz_left;
previous_best_gate.sz_right = best_gate->sz_right;
previous_best_gate.quality = best_gate->quality;
previous_best_gate.n_sides = best_gate->n_sides;
memcpy(previous_best_gate.x_corners, best_gate->x_corners, sizeof(best_gate->x_corners));
memcpy(previous_best_gate.y_corners, best_gate->y_corners, sizeof(best_gate->y_corners));
//color filtered version of image for overlay and debugging
if (FILTER_IMAGE) { //filter) {
image_yuv422_colorfilt(img, img, color_Y_min, color_Y_max, color_U_min, color_U_max, color_V_min, color_V_max);
}
if (best_gate->quality > (min_gate_quality * 2) && best_gate->n_sides >= min_n_sides) {
// successful detection
last_frame_detection = 1;
//draw_gate_color(img, best_gate, blue_color);
if (DRAW_GATE) {
for (int gate_nr = 0; gate_nr < (*n_gates); gate_nr++) {
if (gates_c[gate_nr].n_sides >= min_n_sides && gates_c[gate_nr].quality > 2 * min_gate_quality) {
// draw the best gate:
// draw_gate(img, gates_c[gate_nr]);
draw_gate_color_polygon(img, gates_c[gate_nr], green_color);
}
}
int size_crosshair = 10;
if (repeat_gate == 0) {
draw_gate_color_polygon(img, (*best_gate), blue_color);
} else if (repeat_gate == 1) {
draw_gate_color_polygon(img, (*best_gate), green_color);
for (int i = 0; i < 3; i++) {
struct point_t loc = { .x = last_gate.x_corners[i], .y = last_gate.y_corners[i] };
image_draw_crosshair(img, &loc, blue_color, size_crosshair);
}
}
}
//save for next iteration
memcpy(last_gate.x_corners, best_gate->x_corners, sizeof(best_gate->x_corners));
memcpy(last_gate.y_corners, best_gate->y_corners, sizeof(best_gate->y_corners));
//previous best snake gate
last_gate.x = best_gate->x;
last_gate.y = best_gate->y;
last_gate.sz = best_gate->sz;
//SIGNAL NEW DETECTION AVAILABLE
return SUCCESS_DETECT;
} else {
// Function
static struct image_t *detect_gate_func(struct image_t *img)
{
// detect the gate and draw it in the image:
if (just_filtering) {
// just color filter the image, so that the user can tune the thresholds:
image_yuv422_colorfilt(img, img, color_Ym, color_YM, color_Um, color_UM, color_Vm, color_VM);
} else {
// perform snake gate detection:
int n_gates;
snake_gate_detection(img, n_samples, min_px_size, min_gate_quality, gate_thickness, min_n_sides, color_Ym, color_YM,
color_Um, color_UM, color_Vm, color_VM, &best_gate, gates_c, &n_gates, exclude_top, exclude_bottom);
#if !CAMERA_ROTATED_90DEG_RIGHT
int temp[4];
#endif
#ifdef DEBUG_GATE
printf("\n**** START DEBUG DETECT GATE ****\n");
if (n_gates > 1) {
for (int i = 0; i < n_gates; i++) {
//printf("Gate %d out of %d\n", i, n_gates-1);
if (gates_c[i].quality > min_gate_quality * 2 && gates_c[i].n_sides >= 3) {
#if !CAMERA_ROTATED_90DEG_RIGHT
// swap x and y coordinates:
memcpy(temp, gates_c[i].x_corners, sizeof(gates_c[i].x_corners));
memcpy(gates_c[i].x_corners, gates_c[i].y_corners, sizeof(gates_c[i].x_corners));
memcpy(gates_c[i].y_corners, temp, sizeof(gates_c[i].y_corners));
#endif
drone_position = get_world_position_from_image_points(gates_c[i].x_corners, gates_c[i].y_corners, world_corners,
n_corners,
DETECT_GATE_CAMERA.camera_intrinsics, cam_body);
// debugging the drone position:
printf("Position drone - gate %d, quality = %f: (%f, %f, %f)\n", i, gates_c[i].quality, drone_position.x,
drone_position.y, drone_position.z);
}
}
}
#endif
#ifdef DEBUG_GATE
printf("ratio = %f\n", ratio);
#endif
if (best_gate.quality > min_gate_quality * 2) {
#if !CAMERA_ROTATED_90DEG_RIGHT
// swap x and y coordinates:
memcpy(temp, best_gate.x_corners, sizeof(best_gate.x_corners));
memcpy(best_gate.x_corners, best_gate.y_corners, sizeof(best_gate.x_corners));
memcpy(best_gate.y_corners, temp, sizeof(best_gate.y_corners));
#endif
#ifdef DEBUG_GATE
// debugging snake gate:
printf("Detected gate: ");
for (int i = 0; i < 4; i++) {
printf("(%d,%d) ", best_gate.x_corners[i], best_gate.y_corners[i]);
}
printf("\n");
#endif
static bool simple_position = DETECT_GATE_SIMPLIFIED_PNP;
if(simple_position) {
float sz1_best, sz2_best;
sz1_best = (float) (best_gate.x_corners[2] - best_gate.x_corners[0]);
sz2_best = (float) (best_gate.y_corners[1] - best_gate.y_corners[0]);
float size = (sz1_best > sz2_best) ? sz1_best : sz2_best;
//float width, height;
#if !CAMERA_ROTATED_90DEG_RIGHT
//width = (float) img->w;
//height = (float) img->h;
float pix_x = (best_gate.x_corners[2] + best_gate.x_corners[0]) / 2.0f;
float pix_y = (best_gate.y_corners[1] + best_gate.y_corners[0]) / 2.0f;
float angle_x = (pix_x-DETECT_GATE_CAMERA.camera_intrinsics.center_x) / DETECT_GATE_CAMERA.camera_intrinsics.focal_x;
float angle_y = (pix_y-DETECT_GATE_CAMERA.camera_intrinsics.center_y) / DETECT_GATE_CAMERA.camera_intrinsics.focal_y;
float dist = gate_size_m * (DETECT_GATE_CAMERA.camera_intrinsics.focal_x / size);
drone_position.x = -dist;
drone_position.y = -angle_y*dist;
drone_position.z = angle_x*dist;
#else
//width = (float) img->h;
//height = (float) img->w;
float pix_y = (best_gate.x_corners[1] + best_gate.x_corners[0]) / 2.0f;
float pix_x = (best_gate.y_corners[2] + best_gate.y_corners[1]) / 2.0f;
printf("Not simulating, pix_x = %f, pix_y = %f\n", pix_x, pix_y);
float angle_x = (pix_x-DETECT_GATE_CAMERA.camera_intrinsics.center_y) / DETECT_GATE_CAMERA.camera_intrinsics.focal_y;
float angle_y = (pix_y-DETECT_GATE_CAMERA.camera_intrinsics.center_x) / DETECT_GATE_CAMERA.camera_intrinsics.focal_x;
float dist = gate_size_m * (DETECT_GATE_CAMERA.camera_intrinsics.focal_x / size);
drone_position.x = -dist;
drone_position.y = -angle_x*dist;
drone_position.z = -angle_y*dist;
#endif
#ifdef DEBUG_GATE
printf("angle_x = %f, angle_y = %f, dist = %f\n", angle_x, angle_y, dist);
printf("pix_x = %f, pix_y = %f\n", pix_x, pix_y);
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,
DETECT_GATE_CAMERA.camera_intrinsics.center_x, DETECT_GATE_CAMERA.camera_intrinsics.center_y);
#endif
}
else {
// TODO: try out RANSAC with all combinations of 3 corners out of 4 corners.
drone_position = get_world_position_from_image_points(best_gate.x_corners, best_gate.y_corners, world_corners,
n_corners, DETECT_GATE_CAMERA.camera_intrinsics, cam_body);
}
#ifdef DEBUG_GATE
// debugging the drone position:
printf("Position drone: (%f, %f, %f)\n", drone_position.x, drone_position.y, drone_position.z);
printf("**** END DEBUG DETECT GATE ****\n");
#endif
// send from thread to module - only when there is a best gate:
pthread_mutex_lock(&gate_detect_mutex);
detect_gate_x = drone_position.x;
detect_gate_y = drone_position.y;
detect_gate_z = drone_position.z;
//printf("new measurement!!\n");
detect_gate_has_new_data = true;
pthread_mutex_unlock(&gate_detect_mutex);
}
}
return img;
}
