From c5e432cb092fdcf05484158e41a045b385a8c0a7 Mon Sep 17 00:00:00 2001 From: Manos Kyriakakis Date: Thu, 1 Jun 2017 13:44:16 +0200 Subject: [PATCH] Added a Region of Interest feature to the fast9 corner detector --- .../computer_vision/lib/vision/fast_rosten.c | 28 +++++++++++++++---- .../computer_vision/lib/vision/fast_rosten.h | 2 +- .../opticflow/opticflow_calculator.c | 4 ++- 3 files changed, 26 insertions(+), 8 deletions(-) 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 7f1a2fbeb5..ce7cad836b 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c +++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c @@ -47,13 +47,14 @@ static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel * @param[in] *num_corners reference to the amount of corners found, set by this function * @param[in] *ret_corners_length the length of the array *ret_corners. * @param[in] *ret_corners array which contains the corners that were detected. + * @param[in] *roi array of format [x0 y0 x1 y1] describing the region of interest in the image where the corners will be detected. If null, the whole image is used. */ -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) { - uint32_t corner_cnt = 0; +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) { + uint32_t corner_cnt = 0; int pixel[16]; int16_t i; - uint16_t x, y, x_min, x_max, y_min; + uint16_t x, y, x_min, x_max, y_min, x_start, x_end, y_start, y_end; uint8_t need_skip; // Set the pixel size uint8_t pixel_size = 1; @@ -61,15 +62,29 @@ void fast9_detect(struct image_t *img, uint8_t threshold, uint16_t min_dist, uin pixel_size = 2; } + if (!roi){ + x_start = 3 + x_padding; + y_start = 3 + y_padding; + x_end =img->w - 3 - x_padding; + y_end =img->h - 3 - y_padding; + } + else{ + x_start = roi[0] > 0 ? roi[0] : 3 + x_padding; + y_start = roi[1] > 0 ? roi[1] : 3 + y_padding; + x_end = roi[2] < (img->w - 3 - x_padding) ? roi[2] : img->w - 3 - x_padding; + y_end = roi[3] < (img->h - 3 - y_padding) ? roi[3] : img->h - 3 - y_padding; + + } + // Calculate the pixel offsets fast_make_offsets(pixel, img->w, pixel_size); - // Go trough all the pixels (minus the borders) - for (y = 3 + y_padding; y < img->h - 3 - y_padding; y++) { + // 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; - for (x = 3 + x_padding; x < img->w - 3 - x_padding; x++) { + for (x = x_start; x < x_end; x++) { // First check if we aren't in range vertical (TODO: fix less intensive way) if (min_dist > 0) { @@ -3678,3 +3693,4 @@ static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel pixel[14] = -2 * pixel_size + row_stride * 2 * pixel_size; pixel[15] = -1 * pixel_size + row_stride * 3 * pixel_size; } + 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 713ba3275a..60e7ceef25 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h +++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.h @@ -37,6 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "std.h" #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); +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); #endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 1303422aa8..96abda1185 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -279,10 +279,12 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta // 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); + opticflow->fast9_ret_corners, + 0); // Adaptive threshold if (opticflow->fast9_adaptive) {