From 715b925d89a53a87e6f48cdc148af0186dd0f6fa Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Thu, 12 Mar 2015 14:32:05 +0100 Subject: [PATCH] [vision] Optic flow debugging and small fixes --- .../computer_vision/lib/vision/fast_rosten.c | 40 +++++++++---------- .../computer_vision/lib/vision/image.c | 2 +- .../opticflow/opticflow_calculator.c | 17 +++++--- 3 files changed, 32 insertions(+), 27 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 0ff567e62bc..fbdb16d1531 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c +++ b/sw/airborne/modules/computer_vision/lib/vision/fast_rosten.c @@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "fast_rosten.h" -static void fast_make_offsets(int32_t *pixel, uint16_t row_stride); +static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel_size); /** * Do a FAST9 corner detection @@ -57,12 +57,12 @@ struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *n pixel_size = 2; // Calculate the pixel offsets - fast_make_offsets(pixel, img->w*pixel_size); + fast_make_offsets(pixel, img->w, pixel_size); // Go trough all the pixels (minus the borders) for (y = 3; y < img->h - 3; y++) for (x = 3; x < img->w - 3; x++) { - const uint8_t *p = (uint8_t *)img->buf + y * img->w * pixel_size + x + pixel_size/2; + const uint8_t *p = ((uint8_t *)img->buf) + y * img->w * pixel_size + x * pixel_size + pixel_size/2; // Calculate the threshold values int16_t cb = *p + threshold; @@ -3618,22 +3618,22 @@ struct point_t *fast9_detect(struct image_t *img, uint8_t threshold, uint32_t *n * @param[out] *pixel The offset array of the different pixels * @param[in] row_stride The row stride in the image */ -static void fast_make_offsets(int32_t *pixel, uint16_t row_stride) +static void fast_make_offsets(int32_t *pixel, uint16_t row_stride, uint8_t pixel_size) { - pixel[0] = 0 + row_stride * 3; - pixel[1] = 1 + row_stride * 3; - pixel[2] = 2 + row_stride * 2; - pixel[3] = 3 + row_stride * 1; - pixel[4] = 3 + row_stride * 0; - pixel[5] = 3 + row_stride * -1; - pixel[6] = 2 + row_stride * -2; - pixel[7] = 1 + row_stride * -3; - pixel[8] = 0 + row_stride * -3; - pixel[9] = -1 + row_stride * -3; - pixel[10] = -2 + row_stride * -2; - pixel[11] = -3 + row_stride * -1; - pixel[12] = -3 + row_stride * 0; - pixel[13] = -3 + row_stride * 1; - pixel[14] = -2 + row_stride * 2; - pixel[15] = -1 + row_stride * 3; + pixel[0] = 0*pixel_size + row_stride * 3*pixel_size; + pixel[1] = 1*pixel_size + row_stride * 3*pixel_size; + pixel[2] = 2*pixel_size + row_stride * 2*pixel_size; + pixel[3] = 3*pixel_size + row_stride * 1*pixel_size; + pixel[4] = 3*pixel_size + row_stride * 0*pixel_size; + pixel[5] = 3*pixel_size + row_stride * -1*pixel_size; + pixel[6] = 2*pixel_size + row_stride * -2*pixel_size; + pixel[7] = 1*pixel_size + row_stride * -3*pixel_size; + pixel[8] = 0*pixel_size + row_stride * -3*pixel_size; + pixel[9] = -1*pixel_size + row_stride * -3*pixel_size; + pixel[10] = -2*pixel_size + row_stride * -2*pixel_size; + pixel[11] = -3*pixel_size + row_stride * -1*pixel_size; + pixel[12] = -3*pixel_size + row_stride * 0*pixel_size; + pixel[13] = -3*pixel_size + row_stride * 1*pixel_size; + 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/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c index 9c4b0a8e03f..e9b9b66c1a9 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.c +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -77,7 +77,7 @@ void image_copy(struct image_t *input, struct image_t *output) output->h = input->h; output->buf_size = input->buf_size; memcpy(&output->ts, &input->ts, sizeof(struct timeval)); - memcpy(input->buf, output->buf, input->buf_size); + memcpy(output->buf, input->buf, input->buf_size); } /** diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 1d9ccc7e360..65f3e88d5b1 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -98,9 +98,12 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_ result->fps = 1 / (timeval_diff(&opticflow->prev_timestamp, &img->ts) / 1000.); memcpy(&opticflow->prev_timestamp, &img->ts, sizeof(struct timeval)); - // Conver the first image to gray + // Convert image to grayscale + image_to_grayscale(img, &opticflow->img_gray); + + // Copy to previous image if not set if (!opticflow->got_first_img) { - image_to_grayscale(img, &opticflow->prev_img_gray); + image_copy(&opticflow->img_gray, &opticflow->prev_img_gray); opticflow->got_first_img = TRUE; } @@ -109,7 +112,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_ // ************************************************************************************* // FAST corner detection (TODO: non fixed threashold) - struct point_t *pnts_fast = fast9_detect(&opticflow->prev_img_gray, 5, &result->corner_cnt); + struct point_t *pnts_fast = fast9_detect(img, 20, &result->corner_cnt); /*// Copy the points and remove neighboring corners const float min_distance2 = 10 * 10; @@ -136,15 +139,17 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_ result->count = count_fil;*/ + image_to_grayscale(img, img); uint8_t *im = (uint8_t *)img->buf; for(int i = 0; i < result->corner_cnt; i++) { - uint16_t idx = 2*pnts_fast[i].y*opticflow->img_w + 2*pnts_fast[i].x; + uint32_t idx = 2*pnts_fast[i].y*opticflow->img_w + pnts_fast[i].x*2; im[idx] = 255; - idx = idx+1 % (opticflow->img_w*opticflow->img_h*2); + /*idx = idx+1 % (opticflow->img_w*opticflow->img_h*2); im[idx] = 255; idx = idx+1 % (opticflow->img_w*opticflow->img_h*2); - im[idx] = 255; + im[idx] = 255;*/ } + free(pnts_fast); // ************************************************************************************* // Corner Tracking