diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml new file mode 100644 index 0000000000..ffb9c1e212 --- /dev/null +++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml @@ -0,0 +1,261 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + + + +
+ + + +
+ + +
+ + + + + + +
+ + + +
+ + + + + + + + +
+ +
+ + + + +
+ + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + +
+ +
+ + +
+ +
+ + + +
+ +
+ + + + + + + +
+ +
+ + + + + +
+ diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index b313d4de82..499bebab52 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -112,6 +112,7 @@ + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index e64d761cbc..bcc451f4ab 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -484,6 +484,17 @@ settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="#ffffbf17bf17" /> + 10). + * + * The code here is part of the following publication: + * de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission. + * + */ + +#include "fast_rosten.h" +#include "act_fast.h" +#include "math.h" +#include "image.h" + + +#define MAX_AGENTS 1000 +struct agent agents[MAX_AGENTS]; + +/** + * Do an ACT-FAST corner detection. + * @param[in] *img The image to do the corner detection on + * @param[in] threshold The threshold which we use for FAST9 + * @param[in] *num_corners reference to the amount of corners found, set by this function + * @param[in] **ret_corners pointer to the array which contains the corners that were detected. + * @param[in] n_agents The number of agents that will scan the image for corners + * @param[in] n_time_steps The maximum number of time steps allowed for scanning + * @param[in] long_step When there is not enough texture, the agent will take a long step to a next point of this length in pixels + * @param[in] short_step When there is texture, the agent will follow the edge with this short step in pixels + * @param[in] min_gradient The minimum gradient, in order to determine when to take a long or short step +*/ +void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient) { + + /* + * Procedure: + * 1) initialize agent positions + * 2) loop over the agents, moving and checking for corners + */ + + // method to determine the gradient: + // 0 = simple (-1, 0, 1) + // 1 = Sobel + int gradient_method = 1; + + // ensure that n_agents is never bigger than MAX_AGENTS + n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS; + + int border = 4; + + // *********************************** + // 1) initialize the agents' positions + // *********************************** + + // grid sampling with a border: + int init_border = 10; + float GRID_ROWS = (int) ceil( sqrtf((float) n_agents) ); + float step_size_x = (img->w - 2*init_border) / (GRID_ROWS-1); + float step_size_y = (img->h - 2*init_border) / (GRID_ROWS-1); + + int a = 0; + float px,py,pnorm; + for(int c = 0; c < GRID_ROWS; c++) + { + for(int r = 0; r < GRID_ROWS; r++) + { + // px, py represent the preferred direction of the agent when there is no texture + // here we initialize it differently for each agent: + // TODO: don't we have a randf function in Paparazzi? + px = ((float) (rand() % 10000)) / 10000.0f; + py = ((float) (rand() % 10000)) / 10000.0f; + pnorm = sqrtf(px*px+py*py); + struct agent ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px/pnorm, py/pnorm}; + agents[a] = ag; + a++; + if(a == n_agents) break; + } + + // only initialize a maximum of n_agents agents. + if(a == n_agents) break; + } + + /* ******************************************************** + * 2) loop over the agents, moving and checking for corners + * ********************************************************/ + + // gradient + int dx, dy; + + // loop over all time steps: + for(int t = 0; t < n_time_steps; t++) { + // loop over the agents + for(a = 0; a < n_agents; a++) { + // only do something if the agent is active: + if(agents[a].active) { + // check if this position is a corner: + uint16_t x = (uint16_t) agents[a].x; + uint16_t y = (uint16_t) agents[a].y; + if(fast9_detect_pixel(img, fast_threshold, x, y)) { + // we arrived at a corner, yeah!!! + agents[a].active = 0; + break; + } + else { + // make a step: + struct point_t loc = {agents[a].x, agents[a].y}; + image_gradient_pixel(img, &loc, gradient_method, &dx, &dy); + int gradient = (abs(dx) + abs(dy)) / 2; + if(abs(gradient) >= min_gradient) { + // determine the angle and make a step in that direction: + float norm_factor = sqrtf((float) (dx*dx + dy*dy)); + agents[a].x += (dy / norm_factor) * short_step; + agents[a].y += (dx / norm_factor) * short_step; + } + else { + // make a step in the preferred direction: + agents[a].x += agents[a].preferred_dir_x * long_step; + agents[a].y += agents[a].preferred_dir_y * long_step; + } + } + + // let the agent move over the image in a toroid world: + if(agents[a].x > img->w - border) { + agents[a].x = border; + } + else if(agents[a].x < border) { + agents[a].x = img->w - border; + } + if(agents[a].y > img->h - border) { + agents[a].y = border; + } + else if(agents[a].y < border) { + agents[a].y = img->h - border; + } + } + } + } + + // Transform agents to corners: + (*num_corners) = 0; + for(a = 0; a < n_agents; a++) { + + // for active agents do a last check on the new position: + if(agents[a].active) { + // check if the last step brought the agent to a corner: + uint16_t x = (uint16_t) agents[a].x; + uint16_t y = (uint16_t) agents[a].y; + if(fast9_detect_pixel(img, fast_threshold, x, y)) { + // we arrived at a corner, yeah!!! + agents[a].active = 0; + } + } + + // if inactive, the agent is a corner: + if(!agents[a].active) { + (*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x; + (*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y; + (*num_corners)++; + } + } +} + diff --git a/sw/airborne/modules/computer_vision/lib/vision/act_fast.h b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h new file mode 100644 index 0000000000..b957cf3a86 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h @@ -0,0 +1,42 @@ +/* +Copyright (c) 2017, Guido de Croon, TU Delft +All rights reserved. +*/ + +/** + * @file modules/computer_vision/lib/vision/act_fast.h + * @brief Finds corners in an image by actively scanning the image. This method is inspired by the work in: + * de Croon, G.C.H.E., and Nolfi, S. (2013, May). Act-corner: Active corner finding for optic flow determination. In Robotics and Automation (ICRA), 2013 IEEE International Conference on (pp. 4679-4684). IEEE. + * + * The main idea of this particular implementation, called ACT-FAST, is that actively scanning the image allows to: + * 1. Skip uniform areas in the image. If these areas are contiguous, many non-corners will never be evaluated. + * 2. Follow edges. Typically, following an edge will bring an agent to a corner. + * These two simple rules lead to a significant lower number of corner evaluations, while still detecting a reasonable number of corners. + * Moreover, since the agents scanning the image start on a grid, corners will be quite well-distributed over the image. + * Each step of the agent starts by classifying the agent location as a corner or not with FAST. + * + * For bigger images (e.g., 640 x 640), the computational advantage of ACT-FAST over the normal, exhaustive application of FAST becomes significant (in the order of a factor > 10). + * + * The code here is part of the following publication: + * de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission. + * + */ + +#ifndef ACT_FAST_H +#define ACT_FAST_H + +struct agent +{ + float x; + float y; + int active; + float preferred_dir_x; + float preferred_dir_y; +}; + +#include "std.h" +#include "lib/vision/image.h" + +void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient); + +#endif diff --git a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c index 6eb4065d6b..464658a64e 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c +++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c @@ -63,13 +63,8 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin pixel_size = 2; } - // Padding less than min_dist could cause overflow on some comparisons below. - if (x_padding < min_dist) { - x_padding = min_dist; - } - if (y_padding < min_dist) { - y_padding = min_dist; - } + if(x_padding < min_dist) x_padding = min_dist; + if(y_padding < min_dist) y_padding = min_dist; if (!roi) { x_start = 3 + x_padding; @@ -90,9 +85,7 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin // Go trough all the pixels (minus the borders and inside the requested roi) for (y = y_start; y < y_end; y++) { - if (min_dist > 0) { - y_min = y - min_dist; - } + if (min_dist > 0) { y_min = y - min_dist; } for (x = x_start; x < x_end; x++) { // First check if we aren't in range vertical (TODO: fix less intensive way) @@ -113,15 +106,6 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin if ((*ret_corners)[i].y < y_min) { break; } - /* - // If detecting with already existing corners gives too much overlap uncomment this comparison instead of the one above. - // But, it will make the detection more time consuming - // TODO: maybe sort the corners before calling... - if(ret_corners[i].y < y_min || ret_corners[i].y > y_max){ - i--; - continue; - } - */ if (x_min < (*ret_corners)[i].x && (*ret_corners)[i].x < x_max) { need_skip = 1; @@ -3714,3 +3698,3565 @@ static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel pixel[15] = -1 * pixel_size + row_stride * 3 * pixel_size; } + +/** + * Do a FAST9 corner detection for a single pixel. Returns 0 when not a corner, and 1 when a corner. + * @param[in] *img The image to do the corner detection on + * @param[in] threshold The threshold which we use for FAST9 + * @param[in] x, the x-coordinate of the pixel + * @param[in] y, the y-coordinate of the pixel + */ +int fast9_detect_pixel(struct image_t *img, uint8_t threshold, uint16_t x, uint16_t y) { + + // Set the pixel size + uint8_t pixel_size = 1; + if (img->type == IMAGE_YUV422) { + pixel_size = 2; + } + + int pixel[16]; + // Calculate the pixel offsets: not efficient to do this every time: + fast_make_offsets(pixel, img->w, pixel_size); + + uint16_t border = 4; + if(x < border || x > img->w - border || y < border || y > img->h - border) { + return 0; + } + else { + // Calculate the threshold values + const uint8_t *p = ((uint8_t *)img->buf) + y * img->w * pixel_size + x * pixel_size + pixel_size / 2; + int16_t cb = *p + threshold; + int16_t c_b = *p - threshold; + + // Do the checks if it is a corner + if (p[pixel[0]] > cb) + if (p[pixel[1]] > cb) + if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[15]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[14]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[6]] < c_b) + {} + else if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[6]] < c_b) + {} + else if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[13]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + {} + else if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + {} + else if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + {} + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + {} + else if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + {} + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + {} + else if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[1]] < c_b) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + {} + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + {} + else if (p[pixel[11]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[0]] < c_b) + if (p[pixel[1]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + {} + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[1]] < c_b) + if (p[pixel[2]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + {} + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + {} + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + {} + else if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[11]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[6]] > cb) + {} + else if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] > cb) + if (p[pixel[15]] < c_b) + if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[6]] > cb) + {} + else if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + {} + else if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + {} + else if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[9]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + {} + else if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[8]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + {} + else if (p[pixel[11]] > cb) + {} + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + if (p[pixel[1]] > cb) + {} + else if (p[pixel[10]] > cb) + {} + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + if (p[pixel[1]] < c_b) + {} + else if (p[pixel[10]] < c_b) + {} + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + else { + return 0; + } + // if not returned yet, it is a corner: + return 1; + } +} diff --git a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h index c831bd14e1..70391152ad 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h +++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h @@ -38,5 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "lib/vision/image.h" void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uint16_t x_padding, uint16_t y_padding, uint16_t *num_corners, uint16_t *ret_corners_length, struct point_t **ret_corners, uint16_t *roi); +int fast9_detect_pixel(struct image_t *img, uint8_t threshold, uint16_t x, uint16_t y); + #endif diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c index a0d58a2847..4b5151994a 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.c +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -599,6 +599,77 @@ void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t point } } +/** + * Get the gradient at a pixel location + * @param[in,out] *img The image + * @param[in] loc The location at which to get the gradient + * @param[in] method 0 = {-1, 0, 1}, 1 = Sobel {-1, 0, 1; -2, 0, 2; -1, 0, 1} + * @param[in] dx The gradient in x-direction + * @param[in] dy The gradient in y-direction + */ +void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int* dy) { + // create the simple and sobel filter only once: + + int gradient_x, gradient_y, index; + gradient_x = 0; + gradient_y = 0; + + // get image buffer and take into account YUV vs. grayscale: + uint8_t *img_buf = (uint8_t *)img->buf; + uint8_t pixel_width = (img->type == IMAGE_YUV422) ? 2 : 1; + uint8_t add_ind = pixel_width - 1; + + // check if all pixels will fall in the image: + if(loc->x >= 1 && loc->x < img->w-1 && loc->y >= 1 && loc->y < img->h - 1) { + if(method == 0) { + + // ************* + // Simple method + // ************* + + // dx: + index = loc->y * img->w * pixel_width + (loc->x-1) * pixel_width; + gradient_x -= (int) img_buf[index+add_ind]; + index = loc->y * img->w * pixel_width + (loc->x+1) * pixel_width; + gradient_x += (int) img_buf[index+add_ind]; + // dy: + index = (loc->y-1) * img->w * pixel_width + loc->x * pixel_width; + gradient_y -= (int) img_buf[index+add_ind]; + index = (loc->y+1) * img->w * pixel_width + loc->x * pixel_width; + gradient_y += (int) img_buf[index+add_ind]; + } + else { + + // ***** + // Sobel + // ***** + static int Sobel[9] = {-1, 0, 1, -2, 0, 2, -1, 0, 1}; + static int total_sobel = 8; + + int filt_ind_y = 0; + int filt_ind_x; + for (int x = -1; x <= 1; x++) { + for (int y = -1; y <= 1; y++) { + index = (loc->y + y) * img->w * pixel_width + (loc->x+x) * pixel_width; + if(x!=0) { + filt_ind_x = (x+1)%3 + (y+1)*3; + gradient_x += Sobel[filt_ind_x] * (int) img_buf[index+add_ind]; + } + if(y!=0) { + gradient_y += Sobel[filt_ind_y] * (int) img_buf[index+add_ind]; + } + filt_ind_y++; + } + } + gradient_x /= total_sobel; + } + } + + // TODO: more efficient would be to use dx, dy directly: + (*dx) = gradient_x; + (*dy) = gradient_y; +} + /** * Draw a pink line on the image * @param[in,out] *img The image to show the line on diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h index bebe4e8a86..5528acf9a9 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.h +++ b/sw/airborne/modules/computer_vision/lib/vision/image.h @@ -105,5 +105,6 @@ void image_draw_line(struct image_t *img, struct point_t *from, struct point_t * void image_draw_line_color(struct image_t *img, struct point_t *from, struct point_t *to, uint8_t *color); void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size); void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size); +void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int* dy); #endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index b0d2634d39..289ec0c19f 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -39,6 +39,7 @@ #include "lib/vision/image.h" #include "lib/vision/lucas_kanade.h" #include "lib/vision/fast_rosten.h" +#include "lib/vision/act_fast.h" #include "lib/vision/edge_flow.h" #include "size_divergence.h" #include "linear_flow_fit.h" @@ -48,6 +49,13 @@ #define OPTICFLOW_SHOW_FLOW 0 #define OPTICFLOW_SHOW_CORNERS 0 +#define EXHAUSTIVE_FAST 0 +#define ACT_FAST 1 +uint16_t n_time_steps = 10; +uint16_t n_agents = 25; +// corner method: +#define CORNER_METHOD 0 + // What methods are run to determine divergence, lateral flow, etc. // SIZE_DIV looks at line sizes and only calculates divergence #define SIZE_DIV 1 @@ -307,23 +315,52 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, } else if (!opticflow->feature_management) { // needs to be set to 0 because result is now static result->corner_cnt = 0; - // FAST corner detection - // TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance - // to 0 (see defines), however a more permanent solution should be considered - fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance, - opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt, - &opticflow->fast9_rsize, - &opticflow->fast9_ret_corners, - NULL); + + if(CORNER_METHOD == EXHAUSTIVE_FAST) { + // FAST corner detection + // TODO: There is something wrong with fast9_detect destabilizing FPS. This problem is reduced with putting min_distance + // to 0 (see defines), however a more permanent solution should be considered + fast9_detect(&opticflow->prev_img_gray, opticflow->fast9_threshold, opticflow->fast9_min_distance, + opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt, + &opticflow->fast9_rsize, + &opticflow->fast9_ret_corners, + NULL); + + } + else if (CORNER_METHOD == ACT_FAST) { + // ACT-FAST corner detection: + // TODO: all relevant things should be settings: + float long_step = 10; // 5 + float short_step = 2; // 2 + int min_gradient = 10; // 10 + printf("opticflow->fast9_threshold = %d, n_agents = %d, n_time_steps = %d\n", opticflow->fast9_threshold, n_agents, n_time_steps); + act_fast(&opticflow->prev_img_gray, opticflow->fast9_threshold, &result->corner_cnt, + &opticflow->fast9_ret_corners, n_agents, n_time_steps, + long_step, short_step, min_gradient); + } // Adaptive threshold if (opticflow->fast9_adaptive) { + + // This works well for exhaustive FAST, but drives the threshold to the minimum for ACT-FAST: // Decrease and increase the threshold based on previous values - if (result->corner_cnt < 40 - && opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2 - opticflow->fast9_threshold--; + if (result->corner_cnt < 40) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2 + // make detections easier: + if(opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { + opticflow->fast9_threshold--; + } + + if(CORNER_METHOD == ACT_FAST) { + n_time_steps++; + n_agents++; + } + } else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) { opticflow->fast9_threshold++; + if(CORNER_METHOD == ACT_FAST && n_time_steps > 5 && n_agents > 10) { + n_time_steps--; + n_agents--; + } } } } diff --git a/sw/ext/opencv_bebop b/sw/ext/opencv_bebop index 4e3904d2f4..517e0f9013 160000 --- a/sw/ext/opencv_bebop +++ b/sw/ext/opencv_bebop @@ -1 +1 @@ -Subproject commit 4e3904d2f4d72d98d16abd0f94c0af50233321cc +Subproject commit 517e0f90131c864c1e2d40495da17e886301a516 diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 625ed366b4..9525de7663 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 625ed366b45c1dbc3de1de3702f1cba661aff06e +Subproject commit 9525de76635aac9b6dc058cc3940fa8fff17cd53