From 3ddd5d737b4fbbaf2f1e3a81e792e1c80648b39b Mon Sep 17 00:00:00 2001 From: Manos Kyriakakis Date: Fri, 16 Jun 2017 04:49:46 +0200 Subject: [PATCH] Changes to the Lucas-Kanade optical flow - Corners are now detected in the previous frame(instead of the new one) and tracked to the new one - A feature management method is available where corners are kept in memory for as long as they are tracked - Corners can be weighted against the number of times they have been tracked - Fast9 detection will be performed again after the number of tracked corners drops below a threshold is met and only in the regions of the image that have the less corners (optional) --- conf/modules/cv_opticflow.xml | 6 + .../computer_vision/lib/vision/fast_rosten.c | 23 ++- .../computer_vision/lib/vision/image.h | 5 + .../computer_vision/lib/vision/lucas_kanade.c | 9 +- .../opticflow/opticflow_calculator.c | 153 ++++++++++++++---- .../opticflow/opticflow_calculator.h | 3 + .../computer_vision/opticflow_module.c | 2 +- 7 files changed, 166 insertions(+), 35 deletions(-) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index b3b7e07c22..8b61de897d 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -36,6 +36,7 @@ + @@ -47,6 +48,8 @@ + + @@ -64,6 +67,7 @@ + @@ -74,6 +78,8 @@ + + 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 e37ca94e34..6eb4065d6b 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c +++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c @@ -52,7 +52,7 @@ static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel 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) { - uint16_t corner_cnt = 0; + uint16_t corner_cnt = *num_corners; int pixel[16]; int16_t i; uint16_t x, y, x_min, x_max, y_min, x_start, x_end, y_start, y_end; @@ -63,6 +63,14 @@ 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 (!roi) { x_start = 3 + x_padding; y_start = 3 + y_padding; @@ -82,7 +90,9 @@ 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) @@ -103,6 +113,15 @@ 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; diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h index 52a95bf0cd..1a3e830caa 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.h +++ b/sw/airborne/modules/computer_vision/lib/vision/image.h @@ -29,6 +29,7 @@ #include "std.h" #include +#include /* The different type of images we currently support */ enum image_type { @@ -44,6 +45,7 @@ struct image_t { uint16_t w; ///< Image width uint16_t h; ///< Image height struct timeval ts; ///< The timestamp of creation + struct FloatEulers *eulerAngles; ///< Pointer to the Euler Angles uint32_t pprz_ts; ///< The timestamp in us since system startup uint8_t buf_idx; ///< Buffer index for V4L2 freeing @@ -55,6 +57,9 @@ struct image_t { struct point_t { uint32_t x; ///< The x coordinate of the point uint32_t y; ///< The y coordinate of the point + uint16_t count; ///< Number of times the point has been tracked successfully + uint16_t x_sub; ///< The x subpixel coordinate of the point + uint16_t y_sub; ///< The y subpixel coordinate of the point }; /* Vector structure for point differences */ diff --git a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c index 25659cb0a1..02d5b5b77f 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c +++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c @@ -76,8 +76,7 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str { // if no pyramids, use the old code: - if(pyramid_level == 0) - { + if (pyramid_level == 0) { // use the old code in this case: return opticFlowLK_flat(new_img, old_img, points, points_cnt, half_window_size, subpixel_factor, max_iterations, step_threshold, max_points); } @@ -87,6 +86,7 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str // Determine patch sizes and initialize neighborhoods uint16_t patch_size = 2 * half_window_size + 1; + // TODO: Feature management shows that this threshold rejects corners maybe too often, maybe another formula could be chosen uint32_t error_threshold = (25 * 25) * (patch_size * patch_size); uint16_t padded_patch_size = patch_size + 2; uint8_t border_size = padded_patch_size / 2 + 2; // amount of padding added to images @@ -251,7 +251,8 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str * @return The vectors from the original *points in subpixels */ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, - uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points) { + uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points) +{ // A straightforward one-level implementation of Lucas-Kanade. // For all points: // (1) determine the subpixel neighborhood in the old image @@ -271,7 +272,7 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img // determine patch sizes and initialize neighborhoods uint16_t patch_size = 2 * half_window_size; - uint32_t error_threshold = (25 * 25) *(patch_size *patch_size); + uint32_t error_threshold = (25 * 25) * (patch_size * patch_size); uint16_t padded_patch_size = patch_size + 2; // Create the window images diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index aafb78baf3..9cc5c75ed1 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -118,7 +118,7 @@ PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS) PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC) #ifndef OPTICFLOW_PYRAMID_LEVEL -#define OPTICFLOW_PYRAMID_LEVEL 0 +#define OPTICFLOW_PYRAMID_LEVEL 2 #endif PRINT_CONFIG_VAR(OPTICFLOW_PYRAMID_LEVEL) @@ -185,6 +185,21 @@ PRINT_CONFIG_VAR(OPTICFLOW_KALMAN_FILTER) #endif PRINT_CONFIG_VAR(OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE) +#ifndef OPTICFLOW_FEATURE_MANAGEMENT +#define OPTICFLOW_FEATURE_MANAGEMENT 1 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_FEATURE_MANAGEMENT) + +#ifndef OPTICFLOW_FAST9_REGION_DETECT +#define OPTICFLOW_FAST9_REGION_DETECT 1 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_REGION_DETECT) + +#ifndef OPTICFLOW_FAST9_NUM_REGIONS +#define OPTICFLOW_FAST9_NUM_REGIONS 9 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_NUM_REGIONS) + //Include median filter #include "filters/median_filter.h" struct MedianFilterInt vel_x_filt, vel_y_filt; @@ -193,6 +208,7 @@ struct MedianFilterInt vel_x_filt, vel_y_filt; /* Functions only used here */ static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime); static int cmp_flow(const void *a, const void *b); +static int cmp_array(const void *a, const void *b); @@ -219,6 +235,9 @@ void opticflow_calc_init(struct opticflow_t *opticflow) opticflow->median_filter = OPTICFLOW_MEDIAN_FILTER; opticflow->kalman_filter = OPTICFLOW_KALMAN_FILTER; opticflow->kalman_filter_process_noise = OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE; + opticflow->feature_management = OPTICFLOW_FEATURE_MANAGEMENT; + opticflow->fast9_region_detect = OPTICFLOW_FAST9_REGION_DETECT; + opticflow->fast9_num_regions = OPTICFLOW_FAST9_NUM_REGIONS; opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE; opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD; @@ -238,17 +257,17 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta struct opticflow_result_t *result) { if (opticflow->just_switched_method) { - // Create the image buffers - image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE); - image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE); + // Create the image buffers + image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE); + image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE); - // Set the previous values - opticflow->got_first_img = false; - FLOAT_RATES_ZERO(opticflow->prev_rates); + // Set the previous values + opticflow->got_first_img = false; + FLOAT_RATES_ZERO(opticflow->prev_rates); - // Init median filters with zeros - init_median_filter(&vel_x_filt); - init_median_filter(&vel_y_filt); + // Init median filters with zeros + init_median_filter(&vel_x_filt); + init_median_filter(&vel_y_filt); } // variables for size_divergence: @@ -276,24 +295,74 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta // Corner detection // ************************************************************************************* - // 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 - // last parameter (for ROI detection) set to NULL because feature management is not implemented yet. - fast9_detect(img, 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 feature_management is selected and tracked corners drop below a threshold, redetect + if ((opticflow->feature_management) && (result->corner_cnt < opticflow->max_track_corners / 2)) { + // no need for "per region" re-detection when there are no previous corners + if ((!opticflow->fast9_region_detect) || (result->corner_cnt == 0)) { + 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 { + // allocating memory and initializing the 2d array that holds the number of corners per region and its index (for the sorting) + uint16_t **region_count = malloc(opticflow->fast9_num_regions * sizeof(uint16_t *)); + for (uint16_t i = 0; i < opticflow->fast9_num_regions ; i++) { + region_count[i] = malloc(sizeof(uint16_t) * 2); + region_count[i][0] = 0; + region_count[i][1] = i; + } + for (uint16_t i = 0; i < result->corner_cnt; i++) { + region_count[(opticflow->fast9_ret_corners[i].x / (img->w / (uint8_t)sqrt(opticflow->fast9_num_regions)) + + opticflow->fast9_ret_corners[i].y / (img->h / (uint8_t)sqrt(opticflow->fast9_num_regions)) * (uint8_t)sqrt(opticflow->fast9_num_regions))][0]++; + } - // Adaptive threshold - if (opticflow->fast9_adaptive) { - // 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--; - } else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) { - opticflow->fast9_threshold++; + //sorting region_count array according to first column (number of corners). + qsort(region_count, opticflow->fast9_num_regions, sizeof(region_count[0]), cmp_array); + + // Detecting corners from the region with the less to the one with the most, until a desired total is reached. + for (uint16_t i = 0; i < opticflow->fast9_num_regions && result->corner_cnt < 2 * opticflow->max_track_corners ; i++) { + + // Find the boundaries of the region of interest + uint16_t *roi = malloc(4 * sizeof(uint16_t)); + roi[0] = (region_count[i][1] % (uint8_t)sqrt(opticflow->fast9_num_regions)) * (img->w / (uint8_t)sqrt(opticflow->fast9_num_regions)); + roi[1] = (region_count[i][1] / (uint8_t)sqrt(opticflow->fast9_num_regions)) * (img->h / (uint8_t)sqrt(opticflow->fast9_num_regions)); + roi[2] = roi[0] + (img->w / (uint8_t)sqrt(opticflow->fast9_num_regions)); + roi[3] = roi[1] + (img->h / (uint8_t)sqrt(opticflow->fast9_num_regions)); + + 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, + roi); + free(roi); + } + for (uint16_t i = 0; i < opticflow->fast9_num_regions; i++) { + free(region_count[i]); + } + free(region_count); + } + } 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); + + // Adaptive threshold + if (opticflow->fast9_adaptive) { + // 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--; + } else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2 && opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) { + opticflow->fast9_threshold++; + } } } @@ -303,6 +372,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta // Check if we found some corners to track if (result->corner_cnt < 1) { + // Clear the result otherwise the previous values will be returned for this frame too + memset(result, 0, sizeof(struct opticflow_result_t)); image_copy(&opticflow->img_gray, &opticflow->prev_img_gray); return; } @@ -319,7 +390,6 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta opticflow->threshold_vec, opticflow->max_track_corners, opticflow->pyramid_level); #if OPTICFLOW_SHOW_FLOW - printf("show: n tracked = %d\n", result->tracked_cnt); image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor); #endif @@ -423,6 +493,18 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta // ************************************************************************************* // Next Loop Preparation // ************************************************************************************* + + if (opticflow->feature_management) { + 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_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; + } + } free(vectors); image_switch(&opticflow->img_gray, &opticflow->prev_img_gray); } @@ -614,6 +696,8 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_ if (switch_counter != opticflow->method) { opticflow->just_switched_method = true; switch_counter = opticflow->method; + // Clear the static result + memset(result, 0, sizeof(struct opticflow_result_t)); } else { opticflow->just_switched_method = false; } @@ -762,4 +846,17 @@ static int cmp_flow(const void *a, const void *b) b_p->flow_y); } +/** + * Compare the rows of an integer (uint16_t) 2D array based on the first column. + * Used for sorting. + * @param[in] *a The first row (should be *uint16_t) + * @param[in] *b The second flow vector (should be *uint16_t) + * @return Negative if a[0] < b[0],0 if a[0] == b[0] and positive if a[0] > b[0] + */ +static int cmp_array(const void *a, const void *b) +{ + const uint16_t *pa = *(const uint16_t **)a; + const uint16_t *pb = *(const uint16_t **)b; + return pa[0] - pb[0]; +} diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index 1fa444c642..c9b353ad57 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -70,6 +70,9 @@ struct opticflow_t { uint16_t fast9_rsize; ///< Amount of corners allocated struct point_t *fast9_ret_corners; ///< Corners + 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 }; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 4bb6b6c90b..90a4e46c72 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -183,7 +183,7 @@ struct image_t *opticflow_module_calc(struct image_t *img) temp_state.rates = pose.rates; // Do the optical flow calculation - struct opticflow_result_t temp_result = {}; // new initialization + static struct opticflow_result_t temp_result = {}; // static so that the number of corners is kept between frames opticflow_calc_frame(&opticflow, &temp_state, img, &temp_result); // Copy the result if finished