From 044ba0dc1183765508c55d6a0c70ea4a73c9ddd8 Mon Sep 17 00:00:00 2001 From: guidoAI Date: Thu, 2 Nov 2017 13:23:35 +0100 Subject: [PATCH] Added settings etc. --- conf/modules/cv_opticflow.xml | 13 ++ conf/userconf/tudelft/conf.xml | 2 +- .../computer_vision/lib/vision/act_fast.c | 170 +++++++++--------- .../computer_vision/lib/vision/act_fast.h | 16 +- .../opticflow/opticflow_calculator.c | 90 ++++++---- .../opticflow/opticflow_calculator.h | 7 + 6 files changed, 170 insertions(+), 128 deletions(-) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 499bebab52..0d3da40530 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -50,6 +50,13 @@ + + + + + + + @@ -78,6 +85,12 @@ + + + + + + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index bcc451f4ab..9b16640cc6 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -494,6 +494,7 @@ settings="settings/rotorcraft_basic.xml" settings_modules="modules/video_rtp_stream.xml modules/optical_flow_landing.xml modules/cv_opticflow.xml modules/video_capture.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" + release="" /> - diff --git a/sw/airborne/modules/computer_vision/lib/vision/act_fast.c b/sw/airborne/modules/computer_vision/lib/vision/act_fast.c index a56ece6b55..8b52ff1027 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/act_fast.c +++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.c @@ -30,7 +30,7 @@ All rights reserved. #define MAX_AGENTS 1000 -struct agent agents[MAX_AGENTS]; +struct agent_t agents[MAX_AGENTS]; /** * Do an ACT-FAST corner detection. @@ -43,8 +43,11 @@ struct agent agents[MAX_AGENTS]; * @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 + * @param[in] gradient_method: 0 = simple {-1, 0, 1}, 1 = Sobel {-1,0,1,-2,0,2,-1,0,1} */ -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) { +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, int gradient_method) +{ /* * Procedure: @@ -52,11 +55,6 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners * 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; @@ -68,30 +66,28 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners // 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); + 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++) - { + 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}; + px = ((float)(rand() % 10000)) / 10000.0f; + py = ((float)(rand() % 10000)) / 10000.0f; + pnorm = sqrtf(px * px + py * py); + struct agent_t 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; + if (a == n_agents) { break; } } // only initialize a maximum of n_agents agents. - if(a == n_agents) break; + if (a == n_agents) { break; } } /* ******************************************************** @@ -102,75 +98,71 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners 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; + 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; } - } + } - // 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)++; - } - } + // 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 index b957cf3a86..2e5802714b 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/act_fast.h +++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h @@ -25,18 +25,18 @@ All rights reserved. #ifndef ACT_FAST_H #define ACT_FAST_H -struct agent -{ - float x; - float y; - int active; - float preferred_dir_x; - float preferred_dir_y; +struct agent_t { + 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); +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, int gradient_method); #endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 289ec0c19f..2d949ff969 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -195,6 +195,26 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_REGION_DETECT) #endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_NUM_REGIONS) +#ifndef OPTICFLOW_ACTFAST_LONG_STEP +#define OPTICFLOW_ACTFAST_LONG_STEP 10 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_LONG_STEP) + +#ifndef OPTICFLOW_ACTFAST_SHORT_STEP +#define OPTICFLOW_ACTFAST_SHORT_STEP 2 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_SHORT_STEP) + +#ifndef OPTICFLOW_ACTFAST_GRADIENT_METHOD +#define OPTICFLOW_ACTFAST_GRADIENT_METHOD 1 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_GRADIENT_METHOD) + +#ifndef OPTICFLOW_ACTFAST_MIN_GRADIENT +#define OPTICFLOW_ACTFAST_MIN_GRADIENT 10 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_MIN_GRADIENT) + // Defaults for ARdrone #ifndef OPTICFLOW_BODY_TO_CAM_PHI #define OPTICFLOW_BODY_TO_CAM_PHI 0 @@ -253,6 +273,12 @@ void opticflow_calc_init(struct opticflow_t *opticflow) opticflow->fast9_rsize = 512; opticflow->fast9_ret_corners = calloc(opticflow->fast9_rsize, sizeof(struct point_t)); + + opticflow->actfast_long_step = OPTICFLOW_ACTFAST_LONG_STEP; + opticflow->actfast_short_step = OPTICFLOW_ACTFAST_SHORT_STEP; + opticflow->actfast_min_gradient = OPTICFLOW_ACTFAST_MIN_GRADIENT; + opticflow->actfast_gradient_method = OPTICFLOW_ACTFAST_GRADIENT_METHOD; + struct FloatEulers euler = {OPTICFLOW_BODY_TO_CAM_PHI, OPTICFLOW_BODY_TO_CAM_THETA, OPTICFLOW_BODY_TO_CAM_PSI}; float_rmat_of_eulers(&body_to_cam, &euler); } @@ -316,7 +342,7 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, // needs to be set to 0 because result is now static result->corner_cnt = 0; - if(CORNER_METHOD == EXHAUSTIVE_FAST) { + 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 @@ -326,40 +352,35 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, &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); + } else if (CORNER_METHOD == ACT_FAST) { + // ACT-FAST corner detection: + act_fast(&opticflow->prev_img_gray, opticflow->fast9_threshold, &result->corner_cnt, + &opticflow->fast9_ret_corners, n_agents, n_time_steps, + opticflow->actfast_long_step, opticflow->actfast_short_step, opticflow->actfast_min_gradient, + opticflow->actfast_gradient_method); } // Adaptive threshold if (opticflow->fast9_adaptive) { - // This works well for exhaustive FAST, but drives the threshold to the minimum for ACT-FAST: + // 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) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2 // make detections easier: - if(opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { - opticflow->fast9_threshold--; + if (opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { + opticflow->fast9_threshold--; } - if(CORNER_METHOD == ACT_FAST) { - n_time_steps++; - n_agents++; + 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--; + if (CORNER_METHOD == ACT_FAST && n_time_steps > 5 && n_agents > 10) { + n_time_steps--; + n_agents--; } } } @@ -473,8 +494,10 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, // Velocity calculation // Right now this formula is under assumption that the flow only exist in the center axis of the camera. // TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane. - result->vel_cam.x = (float)result->flow_der_x * result->fps * agl_dist_value_filtered / (opticflow->subpixel_factor * OPTICFLOW_FX); - result->vel_cam.y = (float)result->flow_der_y * result->fps * agl_dist_value_filtered / (opticflow->subpixel_factor * OPTICFLOW_FY); + result->vel_cam.x = (float)result->flow_der_x * result->fps * agl_dist_value_filtered / + (opticflow->subpixel_factor * OPTICFLOW_FX); + result->vel_cam.y = (float)result->flow_der_y * result->fps * agl_dist_value_filtered / + (opticflow->subpixel_factor * OPTICFLOW_FY); result->vel_cam.z = result->divergence * result->fps * agl_dist_value_filtered; //Apply a median filter to the velocity if wanted @@ -494,8 +517,10 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, result->corner_cnt = result->tracked_cnt; //get the new positions of the corners and the "residual" subpixel positions for (uint16_t i = 0; i < result->tracked_cnt; i++) { - opticflow->fast9_ret_corners[i].x = (uint32_t)((vectors[i].pos.x + (float)vectors[i].flow_x) / opticflow->subpixel_factor); - opticflow->fast9_ret_corners[i].y = (uint32_t)((vectors[i].pos.y + (float)vectors[i].flow_y) / opticflow->subpixel_factor); + opticflow->fast9_ret_corners[i].x = (uint32_t)((vectors[i].pos.x + (float)vectors[i].flow_x) / + opticflow->subpixel_factor); + opticflow->fast9_ret_corners[i].y = (uint32_t)((vectors[i].pos.y + (float)vectors[i].flow_y) / + opticflow->subpixel_factor); opticflow->fast9_ret_corners[i].x_sub = (uint16_t)((vectors[i].pos.x + vectors[i].flow_x) % opticflow->subpixel_factor); opticflow->fast9_ret_corners[i].y_sub = (uint16_t)((vectors[i].pos.y + vectors[i].flow_y) % opticflow->subpixel_factor); opticflow->fast9_ret_corners[i].count = vectors[i].pos.count; @@ -519,8 +544,10 @@ static void manage_flow_features(struct image_t *img, struct opticflow_t *opticf while (c1 < (int16_t)result->corner_cnt - 1) { bool exists = false; for (int16_t i = c1 + 1; i < result->corner_cnt; i++) { - if (abs((int16_t)opticflow->fast9_ret_corners[c1].x - (int16_t)opticflow->fast9_ret_corners[i].x) < opticflow->fast9_min_distance / 2 - && abs((int16_t)opticflow->fast9_ret_corners[c1].y - (int16_t)opticflow->fast9_ret_corners[i].y) < opticflow->fast9_min_distance / 2) { + if (abs((int16_t)opticflow->fast9_ret_corners[c1].x - (int16_t)opticflow->fast9_ret_corners[i].x) < + opticflow->fast9_min_distance / 2 + && abs((int16_t)opticflow->fast9_ret_corners[c1].y - (int16_t)opticflow->fast9_ret_corners[i].y) < + opticflow->fast9_min_distance / 2) { // if too close, replace the corner with the last one in the list: opticflow->fast9_ret_corners[c1].x = opticflow->fast9_ret_corners[result->corner_cnt - 1].x; opticflow->fast9_ret_corners[c1].y = opticflow->fast9_ret_corners[result->corner_cnt - 1].y; @@ -587,7 +614,8 @@ static void manage_flow_features(struct image_t *img, struct opticflow_t *opticf bool exists = false; for (uint16_t k = 0; k < result->corner_cnt; k++) { if (abs((int16_t)new_corners[j].x - (int16_t)opticflow->fast9_ret_corners[k].x) < (int16_t)opticflow->fast9_min_distance - && abs((int16_t)new_corners[j].y - (int16_t)opticflow->fast9_ret_corners[k].y) < (int16_t)opticflow->fast9_min_distance) { + && abs((int16_t)new_corners[j].y - (int16_t)opticflow->fast9_ret_corners[k].y) < (int16_t) + opticflow->fast9_min_distance) { exists = true; break; } @@ -734,8 +762,10 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, result->flow_der_y = result->flow_y; result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w); result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w); - result->divergence = -1.0 * (float)edgeflow.div_x / RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of - result->div_size = result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF + result->divergence = -1.0 * (float)edgeflow.div_x / + RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of + result->div_size = + result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF result->surface_roughness = 0.0f; //......................Calculating VELOCITY ..................... // diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index 34569d7032..0bf54ba531 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -69,6 +69,13 @@ struct opticflow_t { bool feature_management; ///< Decides whether to keep track corners in memory for the next frame instead of re-detecting every time bool fast9_region_detect; ///< Decides whether to detect fast9 corners in specific regions of interest or the whole image (only for feature management) uint8_t fast9_num_regions; ///< The number of regions of interest the image is split into + + float actfast_long_step; ///< Step size to take when there is no texture + float actfast_short_step; ///< Step size to take when there is an edge to be followed + int actfast_min_gradient; ///< Threshold that decides when there is sufficient texture for edge following + int actfast_gradient_method; ///< Whether to use a simple or Sobel filter + + };