diff --git a/src/omv/img/hough.c b/src/omv/img/hough.c index 1b49fcf4c..0a78878de 100644 --- a/src/omv/img/hough.c +++ b/src/omv/img/hough.c @@ -5,77 +5,82 @@ #include "imlib.h" -const int hough_divide = 2; // divides theta and rho accumulators - -void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, int y_stride, float threshold, int theta_margin, int rho_margin) +void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride, + uint32_t threshold, unsigned int theta_margin, unsigned int rho_margin) { - const int r_diag_len = fast_roundf(fast_sqrtf((roi->w * roi->w) + (roi->h * roi->h))); - const int r_diag_len_div = (r_diag_len + hough_divide - 1) / hough_divide; - const int theta_size = 1 + ((180 + hough_divide - 1) / hough_divide) + 1; // left & right padding - const int r_size = (r_diag_len_div * 2) + 1; // -r_diag_len to r_diag_len + int r_diag_len, r_diag_len_div, theta_size, r_size, hough_divide = 1; // divides theta and rho accumulators + + for (;;) { // shrink to fit... + r_diag_len = fast_roundf(fast_sqrtf((roi->w * roi->w) + (roi->h * roi->h))); + r_diag_len_div = (r_diag_len + hough_divide - 1) / hough_divide; + theta_size = 1 + ((180 + hough_divide - 1) / hough_divide) + 1; // left & right padding + r_size = (r_diag_len_div * 2) + 1; // -r_diag_len to +r_diag_len + if ((sizeof(uint32_t) * theta_size * r_size) <= fb_avail()) break; + hough_divide = hough_divide << 1; // powers of 2... + if (hough_divide > 4) fb_alloc_fail(); // support 1, 2, 4 + } + uint32_t *acc = fb_alloc0(sizeof(uint32_t) * theta_size * r_size); - uint32_t acc_max = 0; switch (ptr->bpp) { case IMAGE_BPP_BINARY: { for (int y = roi->y + 1, yy = roi->y + roi->h - 1; y < yy; y += y_stride) { uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(ptr, y); for (int x = roi->x + (y % x_stride) + 1, xx = roi->x + roi->w - 1; x < xx; x += x_stride) { - int pixel; // Sobel Algorithm Below... w/ Scharr... + int pixel; // Sobel Algorithm Below int x_acc = 0; int y_acc = 0; - row_ptr -= roi->w; + row_ptr -= ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x - 1)); - x_acc += pixel * -3; // x[0,0] -> pixel * -3 - y_acc += pixel * -3; // y[0,0] -> pixel * -3 + x_acc += pixel * +1; // x[0,0] -> pixel * +1 + y_acc += pixel * +1; // y[0,0] -> pixel * +1 pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); - // x[0,1] -> pixel * 0 - y_acc += pixel * -10; // y[0,1] -> pixel * -10 + // x[0,1] -> pixel * 0 + y_acc += pixel * +2; // y[0,1] -> pixel * +2 pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x + 1)); - x_acc += pixel * +3; // x[0,2] -> pixel * +3 - y_acc += pixel * -3; // y[0,2] -> pixel * -3 + x_acc += pixel * -1; // x[0,2] -> pixel * -1 + y_acc += pixel * +1; // y[0,2] -> pixel * +1 - row_ptr += roi->w; + row_ptr += ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x - 1)); - x_acc += pixel * -10; // x[1,0] -> pixel * -10 - // y[1,0] -> pixel * 0 + x_acc += pixel * +2; // x[1,0] -> pixel * +2 + // y[1,0] -> pixel * 0 // pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); // x[1,1] -> pixel * 0 // y[1,1] -> pixel * 0 pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x + 1)); - x_acc += pixel * +10; // x[1,2] -> pixel * +10 - // y[1,2] -> pixel * 0 + x_acc += pixel * -2; // x[1,2] -> pixel * -2 + // y[1,2] -> pixel * 0 - row_ptr += roi->w; + row_ptr += ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x - 1)); - x_acc += pixel * -3; // x[2,0] -> pixel * -3 - y_acc += pixel * +3; // y[2,0] -> pixel * +3 + x_acc += pixel * +1; // x[2,0] -> pixel * +1 + y_acc += pixel * -1; // y[2,0] -> pixel * -1 pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); - // x[2,1] -> pixel * 0 - y_acc += pixel * +10; // y[2,1] -> pixel * +10 + // x[2,1] -> pixel * 0 + y_acc += pixel * -2; // y[2,1] -> pixel * -2 pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x + 1)); - x_acc += pixel * +3; // x[2,2] -> pixel * +3 - y_acc += pixel * +3; // y[2,2] -> pixel * +3 + x_acc += pixel * -1; // x[2,2] -> pixel * -1 + y_acc += pixel * -1; // y[2,2] -> pixel * -1 - row_ptr -= roi->w; + row_ptr -= ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); int theta = fast_roundf(fast_atan2f(y_acc, x_acc) * 57.295780) % 180; // * (180 / PI) if (theta < 0) theta += 180; int rho = (fast_roundf(((x - roi->x) * cos_table[theta]) + ((y - roi->y) * sin_table[theta])) / hough_divide) + r_diag_len_div; int acc_index = (rho * theta_size) + ((theta / hough_divide) + 1); // add offset - int acc_value = acc[acc_index] + fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc)); - if (acc_value > acc_max) acc_max = acc_value; + int acc_value = acc[acc_index] + fast_roundf(fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc))); acc[acc_index] = acc_value; } } @@ -85,61 +90,60 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, for (int y = roi->y + 1, yy = roi->y + roi->h - 1; y < yy; y += y_stride) { uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(ptr, y); for (int x = roi->x + (y % x_stride) + 1, xx = roi->x + roi->w - 1; x < xx; x += x_stride) { - int pixel; // Sobel Algorithm Below... w/ Scharr... + int pixel; // Sobel Algorithm Below int x_acc = 0; int y_acc = 0; - row_ptr -= roi->w; + row_ptr -= ptr->w; pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x - 1); - x_acc += pixel * -3; // x[0,0] -> pixel * -3 - y_acc += pixel * -3; // y[0,0] -> pixel * -3 + x_acc += pixel * +1; // x[0,0] -> pixel * +1 + y_acc += pixel * +1; // y[0,0] -> pixel * +1 pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x); - // x[0,1] -> pixel * 0 - y_acc += pixel * -10; // y[0,1] -> pixel * -10 + // x[0,1] -> pixel * 0 + y_acc += pixel * +2; // y[0,1] -> pixel * +2 pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x + 1); - x_acc += pixel * +3; // x[0,2] -> pixel * +3 - y_acc += pixel * -3; // y[0,2] -> pixel * -3 + x_acc += pixel * -1; // x[0,2] -> pixel * -1 + y_acc += pixel * +1; // y[0,2] -> pixel * +1 - row_ptr += roi->w; + row_ptr += ptr->w; pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x - 1); - x_acc += pixel * -10; // x[1,0] -> pixel * -10 - // y[1,0] -> pixel * 0 + x_acc += pixel * +2; // x[1,0] -> pixel * +2 + // y[1,0] -> pixel * 0 // pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x); // x[1,1] -> pixel * 0 // y[1,1] -> pixel * 0 pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x + 1); - x_acc += pixel * +10; // x[1,2] -> pixel * +10 - // y[1,2] -> pixel * 0 + x_acc += pixel * -2; // x[1,2] -> pixel * -2 + // y[1,2] -> pixel * 0 - row_ptr += roi->w; + row_ptr += ptr->w; pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x - 1); - x_acc += pixel * -3; // x[2,0] -> pixel * -3 - y_acc += pixel * +3; // y[2,0] -> pixel * +3 + x_acc += pixel * +1; // x[2,0] -> pixel * +1 + y_acc += pixel * -1; // y[2,0] -> pixel * -1 pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x); - // x[2,1] -> pixel * 0 - y_acc += pixel * +10; // y[2,1] -> pixel * +10 + // x[2,1] -> pixel * 0 + y_acc += pixel * -2; // y[2,1] -> pixel * -2 pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x + 1); - x_acc += pixel * +3; // x[2,2] -> pixel * +3 - y_acc += pixel * +3; // y[2,2] -> pixel * +3 + x_acc += pixel * -1; // x[2,2] -> pixel * -1 + y_acc += pixel * -1; // y[2,2] -> pixel * -1 - row_ptr -= roi->w; + row_ptr -= ptr->w; int theta = fast_roundf(fast_atan2f(y_acc, x_acc) * 57.295780) % 180; // * (180 / PI) if (theta < 0) theta += 180; int rho = (fast_roundf(((x - roi->x) * cos_table[theta]) + ((y - roi->y) * sin_table[theta])) / hough_divide) + r_diag_len_div; int acc_index = (rho * theta_size) + ((theta / hough_divide) + 1); // add offset - int acc_value = acc[acc_index] + fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc)); - if (acc_value > acc_max) acc_max = acc_value; + int acc_value = acc[acc_index] + fast_roundf(fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc))); acc[acc_index] = acc_value; } } @@ -149,61 +153,60 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, for (int y = roi->y + 1, yy = roi->y + roi->h - 1; y < yy; y += y_stride) { uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(ptr, y); for (int x = roi->x + (y % x_stride) + 1, xx = roi->x + roi->w - 1; x < xx; x += x_stride) { - int pixel; // Sobel Algorithm Below... w/ Scharr... + int pixel; // Sobel Algorithm Below int x_acc = 0; int y_acc = 0; - row_ptr -= roi->w; + row_ptr -= ptr->w; pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x - 1)); - x_acc += pixel * -3; // x[0,0] -> pixel * -3 - y_acc += pixel * -3; // y[0,0] -> pixel * -3 + x_acc += pixel * +1; // x[0,0] -> pixel * +1 + y_acc += pixel * +1; // y[0,0] -> pixel * +1 pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); - // x[0,1] -> pixel * 0 - y_acc += pixel * -10; // y[0,1] -> pixel * -10 + // x[0,1] -> pixel * 0 + y_acc += pixel * +2; // y[0,1] -> pixel * +2 pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x + 1)); - x_acc += pixel * +3; // x[0,2] -> pixel * +3 - y_acc += pixel * -3; // y[0,2] -> pixel * -3 + x_acc += pixel * -1; // x[0,2] -> pixel * -1 + y_acc += pixel * +1; // y[0,2] -> pixel * +1 - row_ptr += roi->w; + row_ptr += ptr->w; pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x - 1)); - x_acc += pixel * -10; // x[1,0] -> pixel * -10 - // y[1,0] -> pixel * 0 + x_acc += pixel * +2; // x[1,0] -> pixel * +2 + // y[1,0] -> pixel * 0 // pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); // x[1,1] -> pixel * 0 // y[1,1] -> pixel * 0 pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x + 1)); - x_acc += pixel * +10; // x[1,2] -> pixel * +10 - // y[1,2] -> pixel * 0 + x_acc += pixel * -2; // x[1,2] -> pixel * -2 + // y[1,2] -> pixel * 0 - row_ptr += roi->w; + row_ptr += ptr->w; pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x - 1)); - x_acc += pixel * -3; // x[2,0] -> pixel * -3 - y_acc += pixel * +3; // y[2,0] -> pixel * +3 + x_acc += pixel * +1; // x[2,0] -> pixel * +1 + y_acc += pixel * -1; // y[2,0] -> pixel * -1 pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); - // x[2,1] -> pixel * 0 - y_acc += pixel * +10; // y[2,1] -> pixel * +10 + // x[2,1] -> pixel * 0 + y_acc += pixel * -2; // y[2,1] -> pixel * -2 pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x + 1)); - x_acc += pixel * +3; // x[2,2] -> pixel * +3 - y_acc += pixel * +3; // y[2,2] -> pixel * +3 + x_acc += pixel * -1; // x[2,2] -> pixel * -1 + y_acc += pixel * -1; // y[2,2] -> pixel * -1 - row_ptr -= roi->w; + row_ptr -= ptr->w; int theta = fast_roundf(fast_atan2f(y_acc, x_acc) * 57.295780) % 180; // * (180 / PI) if (theta < 0) theta += 180; int rho = (fast_roundf(((x - roi->x) * cos_table[theta]) + ((y - roi->y) * sin_table[theta])) / hough_divide) + r_diag_len_div; int acc_index = (rho * theta_size) + ((theta / hough_divide) + 1); // add offset - int acc_value = acc[acc_index] + fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc)); - if (acc_value > acc_max) acc_max = acc_value; + int acc_value = acc[acc_index] + fast_roundf(fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc))); acc[acc_index] = acc_value; } } @@ -216,39 +219,34 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, list_init(out, sizeof(find_lines_list_lnk_data_t)); - if (acc_max) { - uint32_t acc_threshold = fast_roundf(acc_max * threshold); + for (int y = 1, yy = r_size - 1; y < yy; y++) { + uint32_t *row_ptr = acc + (theta_size * y); - for (int y = 1, yy = r_size - 1; y < yy; y++) { - uint32_t *row_ptr = acc + (theta_size * y); + for (int x = 1, xx = theta_size - 1; x < xx; x++) { + if ((row_ptr[x] >= threshold) + && (row_ptr[x] >= row_ptr[x-theta_size-1]) + && (row_ptr[x] >= row_ptr[x-theta_size]) + && (row_ptr[x] >= row_ptr[x-theta_size+1]) + && (row_ptr[x] >= row_ptr[x-1]) + && (row_ptr[x] >= row_ptr[x+1]) + && (row_ptr[x] >= row_ptr[x+theta_size-1]) + && (row_ptr[x] >= row_ptr[x+theta_size]) + && (row_ptr[x] >= row_ptr[x+theta_size+1])) { - for (int x = 1, xx = theta_size - 1; x < xx; x++) { - if ((row_ptr[x] >= acc_threshold) - && (row_ptr[x] > row_ptr[x-theta_size+1]) - && (row_ptr[x] > row_ptr[x-theta_size]) - && (row_ptr[x] > row_ptr[x-theta_size+1]) - && (row_ptr[x] > row_ptr[x-1]) - && (row_ptr[x] > row_ptr[x+1]) - && (row_ptr[x] > row_ptr[x+theta_size-1]) - && (row_ptr[x] > row_ptr[x+theta_size]) - && (row_ptr[x] > row_ptr[x+theta_size+1])) { + find_lines_list_lnk_data_t lnk_line; + memset(&lnk_line, 0, sizeof(find_lines_list_lnk_data_t)); - find_lines_list_lnk_data_t lnk_line; - memset(&lnk_line, 0, sizeof(find_lines_list_lnk_data_t)); + lnk_line.magnitude = row_ptr[x]; + lnk_line.theta = (x - 1) * hough_divide; // remove offset + lnk_line.rho = (y - r_diag_len_div) * hough_divide; - lnk_line.magnitude = ((float) row_ptr[x]) / ((float) acc_max); - lnk_line.theta = (x - 1) * hough_divide; // remove offset - lnk_line.rho = (y - r_diag_len_div) * hough_divide; - - list_push_back(out, &lnk_line); - } + list_push_back(out, &lnk_line); } } } fb_free(); // acc - float magnitude_max = 0; for (;;) { // Merge overlapping. bool merge_occured = false; @@ -285,7 +283,7 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, bool rho_merge = abs(rho_0_temp - rho_1_temp) < rho_margin; if (theta_merge && rho_merge) { - float magnitude = lnk_line.magnitude + tmp_line.magnitude; + uint32_t magnitude = lnk_line.magnitude + tmp_line.magnitude; float sin_mean = ((sin_table[theta_0_temp] * lnk_line.magnitude) + (sin_table[theta_1_temp] * tmp_line.magnitude)) / magnitude; float cos_mean = ((cos_table[theta_0_temp] * lnk_line.magnitude) + (cos_table[theta_1_temp] * tmp_line.magnitude)) / magnitude; @@ -305,7 +303,6 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, } } - if (lnk_line.magnitude > magnitude_max) magnitude_max = lnk_line.magnitude; list_push_back(&out_temp, &lnk_line); } @@ -316,9 +313,9 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, } } - for (list_lnk_t *i = iterator_start_from_head(out); i; i = iterator_next(i)) { + for (size_t i = 0, j = list_size(out); i < j; i++) { find_lines_list_lnk_data_t lnk_line; - iterator_get(out, i, &lnk_line); + list_pop_front(out, &lnk_line); if ((45 <= lnk_line.theta) && (lnk_line.theta < 135)) { // y = (r - x cos(t)) / sin(t) @@ -334,12 +331,523 @@ void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, lnk_line.line.x2 = fast_roundf((lnk_line.rho - (lnk_line.line.y2 * sin_table[lnk_line.theta])) / cos_table[lnk_line.theta]); } - lnk_line.line.x1 += roi->x; - lnk_line.line.y1 += roi->y; - lnk_line.line.x2 += roi->x; - lnk_line.line.y2 += roi->y; - lnk_line.magnitude /= magnitude_max; + if(lb_clip_line(&lnk_line.line, 0, 0, roi->w, roi->h)) { + lnk_line.line.x1 += roi->x; + lnk_line.line.y1 += roi->y; + lnk_line.line.x2 += roi->x; + lnk_line.line.y2 += roi->y; - iterator_set(out, i, &lnk_line); + list_push_back(out, &lnk_line); + } } } + +static void pixel_magnitude(image_t *ptr, int x, int y, int *theta, uint32_t *mag) +{ + switch (ptr->bpp) { + case IMAGE_BPP_BINARY: { + uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(ptr, y); + int pixel; // Sobel Algorithm Below + int x_acc = 0; + int y_acc = 0; + + if (y != 0) row_ptr -= ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0))); + x_acc += pixel * +1; // x[0,0] -> pixel * +1 + y_acc += pixel * +1; // y[0,0] -> pixel * +1 + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); + // x[0,1] -> pixel * 0 + y_acc += pixel * +2; // y[0,1] -> pixel * +2 + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1))); + x_acc += pixel * -1; // x[0,2] -> pixel * -1 + y_acc += pixel * +1; // y[0,2] -> pixel * +1 + + if (y != 0) row_ptr += ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0))); + x_acc += pixel * +2; // x[1,0] -> pixel * +2 + // y[1,0] -> pixel * 0 + + // pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); + // x[1,1] -> pixel * 0 + // y[1,1] -> pixel * 0 + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1))); + x_acc += pixel * -2; // x[1,2] -> pixel * -2 + // y[1,2] -> pixel * 0 + + if (y != (ptr->h - 1)) row_ptr += ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0))); + x_acc += pixel * +1; // x[2,0] -> pixel * +1 + y_acc += pixel * -1; // y[2,0] -> pixel * -1 + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); + // x[2,1] -> pixel * 0 + y_acc += pixel * -2; // y[2,1] -> pixel * -2 + + pixel = COLOR_BINARY_TO_GRAYSCALE(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1))); + x_acc += pixel * -1; // x[2,2] -> pixel * -1 + y_acc += pixel * -1; // y[2,2] -> pixel * -1 + + if (y != (ptr->h - 1)) row_ptr -= ((ptr->w + UINT32_T_MASK) >> UINT32_T_SHIFT); + + *theta = fast_roundf(fast_atan2f(y_acc, x_acc) * 57.295780) % 180; // * (180 / PI) + if (*theta < 0) *theta += 180; + *mag = fast_roundf(fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc))); + break; + } + case IMAGE_BPP_GRAYSCALE: { + uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(ptr, y); + int pixel; // Sobel Algorithm Below... w/ Scharr... + int x_acc = 0; + int y_acc = 0; + + if (y != 0) row_ptr -= ptr->w; + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0)); + x_acc += pixel * +1; // x[0,0] -> pixel * +1 + y_acc += pixel * +1; // y[0,0] -> pixel * +1 + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x); + // x[0,1] -> pixel * 0 + y_acc += pixel * +2; // y[0,1] -> pixel * +2 + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1)); + x_acc += pixel * -1; // x[0,2] -> pixel * -1 + y_acc += pixel * +1; // y[0,2] -> pixel * +1 + + if (y != 0) row_ptr += ptr->w; + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0)); + x_acc += pixel * +2; // x[1,0] -> pixel * +2 + // y[1,0] -> pixel * 0 + + // pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x)); + // x[1,1] -> pixel * 0 + // y[1,1] -> pixel * 0 + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1)); + x_acc += pixel * -2; // x[1,2] -> pixel * -2 + // y[1,2] -> pixel * 0 + + if (y != (ptr->h - 1)) row_ptr += ptr->w; + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0)); + x_acc += pixel * +1; // x[2,0] -> pixel * +1 + y_acc += pixel * -1; // y[2,0] -> pixel * -1 + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x); + // x[2,1] -> pixel * 0 + y_acc += pixel * -2; // y[2,1] -> pixel * -2 + + pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1)); + x_acc += pixel * -1; // x[2,2] -> pixel * -1 + y_acc += pixel * -1; // y[2,2] -> pixel * -1 + + if (y != (ptr->h - 1)) row_ptr -= ptr->w; + + *theta = fast_roundf(fast_atan2f(y_acc, x_acc) * 57.295780) % 180; // * (180 / PI) + if (*theta < 0) *theta += 180; + *mag = fast_roundf(fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc))); + break; + } + case IMAGE_BPP_RGB565: { + uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(ptr, y); + int pixel; // Sobel Algorithm Below... w/ Scharr... + int x_acc = 0; + int y_acc = 0; + + if (y != 0) row_ptr -= ptr->w; + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0))); + x_acc += pixel * +1; // x[0,0] -> pixel * +1 + y_acc += pixel * +1; // y[0,0] -> pixel * +1 + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); + // x[0,1] -> pixel * 0 + y_acc += pixel * +2; // y[0,1] -> pixel * +2 + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1))); + x_acc += pixel * -1; // x[0,2] -> pixel * -1 + y_acc += pixel * +1; // y[0,2] -> pixel * +1 + + if (y != 0) row_ptr += ptr->w; + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0))); + x_acc += pixel * +2; // x[1,0] -> pixel * +2 + // y[1,0] -> pixel * 0 + + // pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); + // x[1,1] -> pixel * 0 + // y[1,1] -> pixel * 0 + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1))); + x_acc += pixel * -2; // x[1,2] -> pixel * -2 + // y[1,2] -> pixel * 0 + + if (y != (ptr->h - 1)) row_ptr += ptr->w; + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, IM_MAX(x - 1, 0))); + x_acc += pixel * +1; // x[2,0] -> pixel * +1 + y_acc += pixel * -1; // y[2,0] -> pixel * -1 + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); + // x[2,1] -> pixel * 0 + y_acc += pixel * -2; // y[2,1] -> pixel * -2 + + pixel = COLOR_RGB565_TO_GRAYSCALE(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, IM_MIN(x + 1, ptr->w - 1))); + x_acc += pixel * -1; // x[2,2] -> pixel * -1 + y_acc += pixel * -1; // y[2,2] -> pixel * -1 + + if (y != (ptr->h - 1)) row_ptr -= ptr->w; + + *theta = fast_roundf(fast_atan2f(y_acc, x_acc) * 57.295780) % 180; // * (180 / PI) + if (*theta < 0) *theta += 180; + *mag = fast_roundf(fast_sqrtf((x_acc * x_acc) + (y_acc * y_acc))); + break; + } + default: { + break; + } + } +} + +// http://www.brackeen.com/vga/source/djgpp20/lines.c.html +// http://www.brackeen.com/vga/source/bc31/lines.c.html +static size_t trace_line(image_t *ptr, line_t *l, int *theta_buffer, uint32_t *mag_buffer, point_t *point_buffer) +{ + int dx = l->x2 - l->x1; // the horizontal distance of the line + int dy = l->y2 - l->y1; // the vertical distance of the line + int dxabs = abs(dx); + int dyabs = abs(dy); + int sdx = (dx < 0) ? -1 :((dx > 0) ? 1 : 0); + int sdy = (dy < 0) ? -1 :((dy > 0) ? 1 : 0); + int x = dyabs >> 1; // correct + int y = dxabs >> 1; // correct + int px = l->x1; + int py = l->y1; + + size_t index = 0; + + pixel_magnitude(ptr, px, py, theta_buffer + index, mag_buffer + index); + point_buffer[index++] = (point_t) {.x = px, .y = py}; + + if (dxabs >= dyabs) { // the line is more horizontal than vertical + for(int i = 0; i < dxabs; i++) { + y += dyabs; + + if (y >= dxabs) { + y -= dxabs; + py += sdy; + } + + px += sdx; + + pixel_magnitude(ptr, px, py, theta_buffer + index, mag_buffer + index); + point_buffer[index++] = (point_t) {.x = px, .y = py}; + } + } else { // the line is more vertical than horizontal + for(int i = 0; i < dyabs; i++) { + x += dxabs; + + if (x >= dyabs) { + x -= dyabs; + px += sdx; + } + + py += sdy; + + pixel_magnitude(ptr, px, py, theta_buffer + index, mag_buffer + index); + point_buffer[index++] = (point_t) {.x = px, .y = py}; + } + } + + return index; +} + +// http://www.brackeen.com/vga/source/djgpp20/lines.c.html +// http://www.brackeen.com/vga/source/bc31/lines.c.html +static bool merge_line(line_t *big, line_t *small, unsigned int threshold) +{ + int dx = big->x2 - big->x1; // the horizontal distance of the line + int dy = big->y2 - big->y1; // the vertical distance of the line + int dxabs = abs(dx); + int dyabs = abs(dy); + int sdx = (dx < 0) ? -1 :((dx > 0) ? 1 : 0); + int sdy = (dy < 0) ? -1 :((dy > 0) ? 1 : 0); + int x = dyabs >> 1; // correct + int y = dxabs >> 1; // correct + int px = big->x1; + int py = big->y1; + + int x_diff_0 = (px - small->x1); + int y_diff_0 = (py - small->y1); + if (fast_roundf(fast_sqrtf((x_diff_0 * x_diff_0) + (y_diff_0 * y_diff_0))) <= threshold) return true; + int x_diff_1 = (px - small->x2); + int y_diff_1 = (py - small->y2); + if (fast_roundf(fast_sqrtf((x_diff_1 * x_diff_1) + (y_diff_1 * y_diff_1))) <= threshold) return true; + + if (dxabs >= dyabs) { // the line is more horizontal than vertical + for(int i = 0; i < dxabs; i++) { + y += dyabs; + + if (y >= dxabs) { + y -= dxabs; + py += sdy; + } + + px += sdx; + + x_diff_0 = (px - small->x1); + y_diff_0 = (py - small->y1); + if (fast_roundf(fast_sqrtf((x_diff_0 * x_diff_0) + (y_diff_0 * y_diff_0))) <= threshold) return true; + x_diff_1 = (px - small->x2); + y_diff_1 = (py - small->y2); + if (fast_roundf(fast_sqrtf((x_diff_1 * x_diff_1) + (y_diff_1 * y_diff_1))) <= threshold) return true; + } + } else { // the line is more vertical than horizontal + for(int i = 0; i < dyabs; i++) { + x += dxabs; + + if (x >= dyabs) { + x -= dyabs; + px += sdx; + } + + py += sdy; + + x_diff_0 = (px - small->x1); + y_diff_0 = (py - small->y1); + if (fast_roundf(fast_sqrtf((x_diff_0 * x_diff_0) + (y_diff_0 * y_diff_0))) <= threshold) return true; + x_diff_1 = (px - small->x2); + y_diff_1 = (py - small->y2); + if (fast_roundf(fast_sqrtf((x_diff_1 * x_diff_1) + (y_diff_1 * y_diff_1))) <= threshold) return true; + } + } + + return false; +} + +static void merge_alot(list_t *out, int threshold, int theta_threshold) +{ + for (;;) { + bool merge_occured = false; + + list_t out_temp; + list_init(&out_temp, sizeof(find_lines_list_lnk_data_t)); + + while (list_size(out)) { + find_lines_list_lnk_data_t lnk_line; + list_pop_front(out, &lnk_line); + + for (size_t k = 0, l = list_size(out); k < l; k++) { + find_lines_list_lnk_data_t tmp_line; + list_pop_front(out, &tmp_line); + + int x_diff_0 = (lnk_line.line.x2 - lnk_line.line.x1); + int y_diff_0 = (lnk_line.line.y2 - lnk_line.line.y1); + int length_0 = fast_roundf(fast_sqrtf((x_diff_0 * x_diff_0) + (y_diff_0 * y_diff_0))); + + int x_diff_1 = (tmp_line.line.x2 - tmp_line.line.x1); + int y_diff_1 = (tmp_line.line.y2 - tmp_line.line.y1); + int length_1 = fast_roundf(fast_sqrtf((x_diff_1 * x_diff_1) + (y_diff_1 * y_diff_1))); + + int theta_diff = abs(lnk_line.theta - tmp_line.theta); + int theta_diff_2 = (theta_diff >= 90) ? (180 - theta_diff) : theta_diff; + + if ((theta_diff_2 <= theta_threshold) + && merge_line((length_0 > length_1) ? &lnk_line.line : &tmp_line.line, (length_0 <= length_1) ? &lnk_line.line : &tmp_line.line, threshold)) { + + if (abs(x_diff_0) >= abs(y_diff_0)) { // the line is more horizontal than vertical + if (x_diff_0 < 0) { // Make sure x slope is positive for the next part. + int temp_x = lnk_line.line.x1; + lnk_line.line.x1 = lnk_line.line.x2; + lnk_line.line.x2 = temp_x; + int temp_y = lnk_line.line.y1; + lnk_line.line.y1 = lnk_line.line.y2; + lnk_line.line.y2 = temp_y; + x_diff_0 = (lnk_line.line.x2 - lnk_line.line.x1); + y_diff_0 = (lnk_line.line.y2 - lnk_line.line.y1); + } + + if (x_diff_1 < 0) { // Make sure x slope is positive for the next part. + int temp_x = tmp_line.line.x1; + tmp_line.line.x1 = tmp_line.line.x2; + tmp_line.line.x2 = temp_x; + int temp_y = tmp_line.line.y1; + tmp_line.line.y1 = tmp_line.line.y2; + tmp_line.line.y2 = temp_y; + x_diff_1 = (tmp_line.line.x2 - tmp_line.line.x1); + y_diff_1 = (tmp_line.line.y2 - tmp_line.line.y1); + } + + if (length_0 > length_1) { + int x_min = IM_MIN(lnk_line.line.x1, tmp_line.line.x1); + int x_max = IM_MAX(lnk_line.line.x2, tmp_line.line.x2); + lnk_line.line.y1 = lnk_line.line.y1 - ((y_diff_0 * (lnk_line.line.x1 - x_min)) / x_diff_0); + lnk_line.line.x1 = x_min; + lnk_line.line.y2 = lnk_line.line.y2 + ((y_diff_0 * (x_max - lnk_line.line.x2)) / x_diff_0); + lnk_line.line.x2 = x_max; + } else { + int x_min = IM_MIN(tmp_line.line.x1, lnk_line.line.x1); + int x_max = IM_MAX(tmp_line.line.x2, lnk_line.line.x2); + lnk_line.line.y1 = tmp_line.line.y1 - ((y_diff_0 * (tmp_line.line.x1 - x_min)) / x_diff_0); + lnk_line.line.x1 = x_min; + lnk_line.line.y2 = tmp_line.line.y2 + ((y_diff_0 * (x_max - tmp_line.line.x2)) / x_diff_0); + lnk_line.line.x2 = x_max; + } + } else { // the line is more vertical than horizontal + if (y_diff_0 < 0) { // Make sure y slope is positive for the next part. + int temp_x = lnk_line.line.x1; + lnk_line.line.x1 = lnk_line.line.x2; + lnk_line.line.x2 = temp_x; + int temp_y = lnk_line.line.y1; + lnk_line.line.y1 = lnk_line.line.y2; + lnk_line.line.y2 = temp_y; + x_diff_0 = (lnk_line.line.x2 - lnk_line.line.x1); + y_diff_0 = (lnk_line.line.y2 - lnk_line.line.y1); + } + + if (y_diff_1 < 0) { // Make sure y slope is positive for the next part. + int temp_x = tmp_line.line.x1; + tmp_line.line.x1 = tmp_line.line.x2; + tmp_line.line.x2 = temp_x; + int temp_y = tmp_line.line.y1; + tmp_line.line.y1 = tmp_line.line.y2; + tmp_line.line.y2 = temp_y; + x_diff_1 = (tmp_line.line.x2 - tmp_line.line.x1); + y_diff_1 = (tmp_line.line.y2 - tmp_line.line.y1); + } + + if (length_0 > length_1) { + int y_min = IM_MIN(lnk_line.line.y1, tmp_line.line.y1); + int y_max = IM_MAX(lnk_line.line.y2, tmp_line.line.y2); + lnk_line.line.x1 = lnk_line.line.x1 - ((x_diff_0 * (lnk_line.line.y1 - y_min)) / y_diff_0); + lnk_line.line.y1 = y_min; + lnk_line.line.x2 = lnk_line.line.x2 + ((x_diff_0 * (y_max - lnk_line.line.y2)) / y_diff_0); + lnk_line.line.y2 = y_max; + } else { + int y_min = IM_MIN(tmp_line.line.y1, lnk_line.line.y1); + int y_max = IM_MAX(tmp_line.line.y2, lnk_line.line.y2); + lnk_line.line.x1 = tmp_line.line.x1 - ((x_diff_0 * (tmp_line.line.y1 - y_min)) / y_diff_0); + lnk_line.line.y1 = y_min; + lnk_line.line.x2 = tmp_line.line.x2 + ((x_diff_0 * (y_max - tmp_line.line.y2)) / y_diff_0); + lnk_line.line.y2 = y_max; + } + } + + merge_occured = true; + } else { + list_push_back(out, &tmp_line); + } + } + + list_push_back(&out_temp, &lnk_line); + } + + list_copy(out, &out_temp); + + if (!merge_occured) { + break; + } + } +} + +void imlib_find_line_segments(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride, + uint32_t threshold, unsigned int theta_margin, unsigned int rho_margin, + uint32_t segment_threshold) +{ + const unsigned int max_theta_diff = 15; + const unsigned int max_gap_pixels = 5; + + list_t temp_out; + imlib_find_lines(&temp_out, ptr, roi, x_stride, y_stride, threshold, theta_margin, rho_margin); + list_init(out, sizeof(find_lines_list_lnk_data_t)); + + const int r_diag_len = fast_roundf(fast_sqrtf((roi->w * roi->w) + (roi->h * roi->h))) * 2; + int *theta_buffer = fb_alloc(sizeof(int) * r_diag_len); + uint32_t *mag_buffer = fb_alloc(sizeof(uint32_t) * r_diag_len); + point_t *point_buffer = fb_alloc(sizeof(point_t) * r_diag_len); + + for (size_t i = 0; list_size(&temp_out); i++) { + find_lines_list_lnk_data_t lnk_data; + list_pop_front(&temp_out, &lnk_data); + + list_t line_out; + list_init(&line_out, sizeof(find_lines_list_lnk_data_t)); + + for (int k = -2; k <= 2; k += 2) { + line_t l; + + if (abs(lnk_data.line.x2 - lnk_data.line.x1) >= abs(lnk_data.line.y2 - lnk_data.line.y1)) { // the line is more horizontal than vertical + l.x1 = lnk_data.line.x1; + l.y1 = lnk_data.line.y1 + k; + l.x2 = lnk_data.line.x2; + l.y2 = lnk_data.line.y2 + k; + } else { // the line is more vertical than horizontal + l.x1 = lnk_data.line.x1 + k; + l.y1 = lnk_data.line.y1; + l.x2 = lnk_data.line.x2 + k; + l.y2 = lnk_data.line.y2; + } + + if(!lb_clip_line(&l, 0, 0, ptr->w, ptr->h)) { + continue; + } + + find_lines_list_lnk_data_t tmp_line; + tmp_line.magnitude = lnk_data.magnitude; + tmp_line.theta = lnk_data.theta; + tmp_line.rho = lnk_data.rho; + + size_t index = trace_line(ptr, &l, theta_buffer, mag_buffer, point_buffer); + unsigned int max_gap = 0; + + for (size_t j = 0; j < index; j++) { + int theta_diff = abs(tmp_line.theta - theta_buffer[j]); + int theta_diff_2 = (theta_diff >= 90) ? (180 - theta_diff) : theta_diff; + bool ok = (mag_buffer[j] >= segment_threshold) && (theta_diff_2 <= max_theta_diff); + + if (!max_gap) { + if (ok) { + max_gap = max_gap_pixels + 1; // (start) auto connect segments max_gap_pixels px apart... + tmp_line.line.x1 = point_buffer[j].x; + tmp_line.line.y1 = point_buffer[j].y; + tmp_line.line.x2 = point_buffer[j].x; + tmp_line.line.y2 = point_buffer[j].y; + } + } else { + if (ok) { + max_gap = max_gap_pixels + 1; // (reset) auto connect segments max_gap_pixels px apart... + tmp_line.line.x2 = point_buffer[j].x; + tmp_line.line.y2 = point_buffer[j].y; + } else if (!--max_gap) { + list_push_back(&line_out, &tmp_line); + } + } + } + + if (max_gap) { + list_push_back(&line_out, &tmp_line); + } + } + + merge_alot(&line_out, max_gap_pixels + 1, max_theta_diff); + + while (list_size(&line_out)) { + find_lines_list_lnk_data_t lnk_line; + list_pop_front(&line_out, &lnk_line); + list_push_back(out, &lnk_line); + } + } + + merge_alot(out, max_gap_pixels + 1, max_theta_diff); + + fb_free(); // point_buffer + fb_free(); // mag_buffer + fb_free(); // theta_buffer +} diff --git a/src/omv/img/imlib.c b/src/omv/img/imlib.c index 118163f5f..ff171f21a 100644 --- a/src/omv/img/imlib.c +++ b/src/omv/img/imlib.c @@ -41,6 +41,59 @@ int point_quadrance(point_t *ptr0, point_t *ptr1) return (delta_x * delta_x) + (delta_y * delta_y); } +//////////////// +// Line Stuff // +//////////////// + +// http://www.skytopia.com/project/articles/compsci/clipping.html +bool lb_clip_line(line_t *l, int x, int y, int w, int h) // line is drawn if this returns true +{ + int xdelta = l->x2 - l->x1, ydelta = l->y2 - l->y1, p[4], q[4]; + float umin = 0, umax = 1; + + p[0] = -(xdelta); + p[1] = +(xdelta); + p[2] = -(ydelta); + p[3] = +(ydelta); + + q[0] = l->x1 - (x); + q[1] = (x + w - 1) - l->x1; + q[2] = l->y1 - (y); + q[3] = (y + h - 1) - l->y1; + + for (int i = 0; i < 4; i++) { + if (p[i]) { + float u = ((float) q[i]) / ((float) p[i]); + + if (p[i] < 0) { // outside to inside + if (u > umax) return false; + if (u > umin) umin = u; + } + + if (p[i] > 0) { // inside to outside + if (u < umin) return false; + if (u < umax) umax = u; + } + + } else if (q[i] < 0) { + return false; + } + } + + if (umax < umin) return false; + + int x1_c = l->x1 + (xdelta * umin); + int y1_c = l->y1 + (ydelta * umin); + int x2_c = l->x1 + (xdelta * umax); + int y2_c = l->y1 + (ydelta * umax); + l->x1 = x1_c; + l->y1 = y1_c; + l->x2 = x2_c; + l->y2 = y2_c; + + return true; +} + ///////////////////// // Rectangle Stuff // ///////////////////// diff --git a/src/omv/img/imlib.h b/src/omv/img/imlib.h index 71232b743..106fc586b 100644 --- a/src/omv/img/imlib.h +++ b/src/omv/img/imlib.h @@ -87,6 +87,8 @@ typedef struct line { int16_t y2; } line_t; +bool lb_clip_line(line_t *l, int x, int y, int w, int h); + ///////////////////// // Rectangle Stuff // ///////////////////// @@ -890,7 +892,7 @@ find_blobs_list_lnk_data_t; typedef struct find_lines_list_lnk_data { line_t line; - float magnitude; + uint32_t magnitude; int16_t theta, rho; } find_lines_list_lnk_data_t; @@ -1145,7 +1147,11 @@ void imlib_find_blobs(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int bool (*threshold_cb)(void*,find_blobs_list_lnk_data_t*), void *threshold_cb_arg, bool (*merge_cb)(void*,find_blobs_list_lnk_data_t*,find_blobs_list_lnk_data_t*), void *merge_cb_arg); // Shape Detection -void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, int x_stride, int y_stride, float threshold, int theta_margin, int rho_margin); +void imlib_find_lines(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride, + uint32_t threshold, unsigned int theta_margin, unsigned int rho_margin); +void imlib_find_line_segments(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride, + uint32_t threshold, unsigned int theta_margin, unsigned int rho_margin, + uint32_t segment_threshold); // 1/2D Bar Codes void imlib_find_qrcodes(list_t *out, image_t *ptr, rectangle_t *roi); void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_families_t families, diff --git a/src/omv/py/py_image.c b/src/omv/py/py_image.c index 8ffb60206..fc2adfa23 100644 --- a/src/omv/py/py_image.c +++ b/src/omv/py/py_image.c @@ -2133,22 +2133,23 @@ static mp_obj_t py_image_find_blobs(uint n_args, const mp_obj_t *args, mp_map_t } // Line Object // -#define py_line_obj_size 7 +#define py_line_obj_size 8 typedef struct py_line_obj { mp_obj_base_t base; - mp_obj_t x1, y1, x2, y2, magnitude, theta, rho; + mp_obj_t x1, y1, x2, y2, length, magnitude, theta, rho; } py_line_obj_t; static void py_line_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { py_line_obj_t *self = self_in; mp_printf(print, - "{x1:%d, y1:%d, x2:%d, y2:%d, magnitude:%f, theta:%d, rho:%d}", + "{x1:%d, y1:%d, x2:%d, y2:%d, length:%d, magnitude:%d, theta:%d, rho:%d}", mp_obj_get_int(self->x1), mp_obj_get_int(self->y1), mp_obj_get_int(self->x2), mp_obj_get_int(self->y2), - (double) mp_obj_get_float(self->magnitude), + mp_obj_get_int(self->length), + mp_obj_get_int(self->magnitude), mp_obj_get_int(self->theta), mp_obj_get_int(self->rho)); } @@ -2171,9 +2172,10 @@ static mp_obj_t py_line_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value) case 1: return self->y1; case 2: return self->x2; case 3: return self->y2; - case 4: return self->magnitude; - case 5: return self->theta; - case 6: return self->rho; + case 4: return self->length; + case 5: return self->magnitude; + case 6: return self->theta; + case 7: return self->rho; } } return MP_OBJ_NULL; // op not supported @@ -2191,6 +2193,7 @@ mp_obj_t py_line_x1(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->x1; mp_obj_t py_line_y1(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->y1; } mp_obj_t py_line_x2(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->x2; } mp_obj_t py_line_y2(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->y2; } +mp_obj_t py_line_length(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->length; } mp_obj_t py_line_magnitude(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->magnitude; } mp_obj_t py_line_theta(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->theta; } mp_obj_t py_line_rho(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->rho; } @@ -2200,6 +2203,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_x1_obj, py_line_x1); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_y1_obj, py_line_y1); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_x2_obj, py_line_x2); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_y2_obj, py_line_y2); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_length_obj, py_line_length); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_magnitude_obj, py_line_magnitude); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_theta_obj, py_line_theta); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_rho_obj, py_line_rho); @@ -2210,6 +2214,7 @@ STATIC const mp_rom_map_elem_t py_line_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_y1), MP_ROM_PTR(&py_line_y1_obj) }, { MP_ROM_QSTR(MP_QSTR_x2), MP_ROM_PTR(&py_line_x2_obj) }, { MP_ROM_QSTR(MP_QSTR_y2), MP_ROM_PTR(&py_line_y2_obj) }, + { MP_ROM_QSTR(MP_QSTR_length), MP_ROM_PTR(&py_line_length_obj) }, { MP_ROM_QSTR(MP_QSTR_magnitude), MP_ROM_PTR(&py_line_magnitude_obj) }, { MP_ROM_QSTR(MP_QSTR_theta), MP_ROM_PTR(&py_line_theta_obj) }, { MP_ROM_QSTR(MP_QSTR_rho), MP_ROM_PTR(&py_line_rho_obj) }, @@ -2240,7 +2245,7 @@ static mp_obj_t py_image_find_lines(uint n_args, const mp_obj_t *args, mp_map_t list_t out; fb_alloc_mark(); - imlib_find_lines(&out, arg_img, &roi, x_stride, y_stride, py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_threshold), 0.25), + imlib_find_lines(&out, arg_img, &roi, x_stride, y_stride, py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_threshold), 1000), py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_theta_margin), 25), py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_rho_margin), 25)); fb_alloc_free_till_mark(); @@ -2256,7 +2261,55 @@ static mp_obj_t py_image_find_lines(uint n_args, const mp_obj_t *args, mp_map_t o->y1 = mp_obj_new_int(lnk_data.line.y1); o->x2 = mp_obj_new_int(lnk_data.line.x2); o->y2 = mp_obj_new_int(lnk_data.line.y2); - o->magnitude = mp_obj_new_float(lnk_data.magnitude); + int x_diff = lnk_data.line.x2 - lnk_data.line.x1; + int y_diff = lnk_data.line.y2 - lnk_data.line.y1; + o->length = mp_obj_new_int(fast_roundf(fast_sqrtf((x_diff * x_diff) + (y_diff * y_diff)))); + o->magnitude = mp_obj_new_int(lnk_data.magnitude); + o->theta = mp_obj_new_int(lnk_data.theta); + o->rho = mp_obj_new_int(lnk_data.rho); + + objects_list->items[i] = o; + } + + return objects_list; +} + +static mp_obj_t py_image_find_line_segments(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) +{ + image_t *arg_img = py_image_cobj(args[0]); + PY_ASSERT_FALSE_MSG(IM_IS_JPEG(arg_img), "Operation not supported on JPEG or RAW frames."); + + rectangle_t roi; + py_helper_lookup_rectangle(kw_args, arg_img, &roi); + + unsigned int x_stride = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_x_stride), 2); + PY_ASSERT_TRUE_MSG(x_stride > 0, "x_stride must not be zero."); + unsigned int y_stride = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_y_stride), 1); + PY_ASSERT_TRUE_MSG(y_stride > 0, "y_stride must not be zero."); + + list_t out; + fb_alloc_mark(); + imlib_find_line_segments(&out, arg_img, &roi, x_stride, y_stride, py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_threshold), 1000), + py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_theta_margin), 25), + py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_rho_margin), 25), + py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_segment_threshold), 100)); + fb_alloc_free_till_mark(); + + mp_obj_list_t *objects_list = mp_obj_new_list(list_size(&out), NULL); + for (size_t i = 0; list_size(&out); i++) { + find_lines_list_lnk_data_t lnk_data; + list_pop_front(&out, &lnk_data); + + py_line_obj_t *o = m_new_obj(py_line_obj_t); + o->base.type = &py_line_type; + o->x1 = mp_obj_new_int(lnk_data.line.x1); + o->y1 = mp_obj_new_int(lnk_data.line.y1); + o->x2 = mp_obj_new_int(lnk_data.line.x2); + o->y2 = mp_obj_new_int(lnk_data.line.y2); + int x_diff = lnk_data.line.x2 - lnk_data.line.x1; + int y_diff = lnk_data.line.y2 - lnk_data.line.y1; + o->length = mp_obj_new_int(fast_roundf(fast_sqrtf((x_diff * x_diff) + (y_diff * y_diff)))); + o->magnitude = mp_obj_new_int(lnk_data.magnitude); o->theta = mp_obj_new_int(lnk_data.theta); o->rho = mp_obj_new_int(lnk_data.rho); @@ -3309,6 +3362,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_get_statistics_obj, 1, py_image_get_s STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_blobs_obj, 2, py_image_find_blobs); /* Shape Detection */ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_lines_obj, 1, py_image_find_lines); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_line_segments_obj, 1, py_image_find_line_segments); /* Code Detection */ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_qrcodes_obj, 1, py_image_find_qrcodes); #ifdef OMV_ENABLE_APRILTAGS @@ -3396,6 +3450,7 @@ static const mp_map_elem_t locals_dict_table[] = { {MP_OBJ_NEW_QSTR(MP_QSTR_find_blobs), (mp_obj_t)&py_image_find_blobs_obj}, /* Shape Detection */ {MP_OBJ_NEW_QSTR(MP_QSTR_find_lines), (mp_obj_t)&py_image_find_lines_obj}, + {MP_OBJ_NEW_QSTR(MP_QSTR_find_line_segments), (mp_obj_t)&py_image_find_line_segments_obj}, /* Code Detection */ {MP_OBJ_NEW_QSTR(MP_QSTR_find_qrcodes), (mp_obj_t)&py_image_find_qrcodes_obj}, #ifdef OMV_ENABLE_APRILTAGS diff --git a/src/omv/py/qstrdefsomv.h b/src/omv/py/qstrdefsomv.h index 59e0bfb42..576ad24b7 100644 --- a/src/omv/py/qstrdefsomv.h +++ b/src/omv/py/qstrdefsomv.h @@ -422,10 +422,21 @@ Q(x1) Q(y1) Q(x2) Q(y2) +Q(length) Q(magnitude) Q(theta) Q(rho) +// Find Line Segments +Q(find_line_segments) +// duplicate Q(roi) +// duplicate Q(x_stride) +// duplicate Q(y_stride) +// duplicate Q(threshold) +// duplicate Q(theta_margin) +// duplicate Q(rho_margin) +Q(segment_threshold) + // Find QRCodes Q(find_qrcodes) // duplicate Q(roi) diff --git a/usr/examples/09-Feature-Detection/find_line_segments.py b/usr/examples/09-Feature-Detection/find_line_segments.py new file mode 100644 index 000000000..f8b9632a0 --- /dev/null +++ b/usr/examples/09-Feature-Detection/find_line_segments.py @@ -0,0 +1,55 @@ +# Find Line Segments Example +# +# This example shows off how to find line segments in the image. For each line object +# found in the image a line object is returned which includes the line's rotation. + +# Note: Line detection is done by using the Hough Transform: +# http://en.wikipedia.org/wiki/Hough_transform +# Please read about it above for more information on what `theta` and `rho` are. + +enable_lens_corr = True # turn on for straighter lines... + +import sensor, image, time + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) # grayscale is faster +sensor.set_framesize(sensor.QQVGA) +sensor.skip_frames(30) +clock = time.clock() + +# All lines also have `x1()`, `y1()`, `x2()`, and `y2()` methods to get their end-points +# and a `line()` method to get all the above as one 4 value tuple for `draw_line()`. + +while(True): + clock.tick() + img = sensor.snapshot() + if enable_lens_corr: img.lens_corr(1.8) # for 2.8mm lens... + + # `threshold` controls how many lines in the image are found. Only lines with + # edge difference magnitude sums greater than `threshold` are detected... + + # More about `threshold` - each pixel in the image contributes a magnitude value + # to a line. The sum of all contributions is the magintude for that line. Then + # when lines are merged their magnitudes are added togheter. Note that `threshold` + # filters out lines with low magnitudes before merging. To see the magnitude of + # un-merged lines set `theta_margin` and `rho_margin` to 0... + + # `theta_margin` and `rho_margin` control merging similar lines. If two lines + # theta and rho value differences are less than the margins then they are merged. + + # Setting both the above to zero will greatly increase segment detection at the + # cost of a lot of FPS. This is because when less lines are merged more pixels + # are tested... which takes longer but covers more possibilities... + + # `segment_threshold` controls line segment extraction. It's a threshold on the + # magnitude response per pixel under an infinite line. Pixels with a magnitude + # above threshold are added to the line segment. + + # `find_line_segments` merges detected lines that are no more than 5 pixels apart + # and no more than 15 degrees different to create nice continous line segments. + + for l in img.find_line_segments(threshold = 1000, theta_margin = 15, rho_margin = 15, segment_threshold = 100): + img.draw_line(l.line(), color = (255, 0, 0)) + # print(l) + + print("FPS %f" % clock.fps()) diff --git a/usr/examples/09-Feature-Detection/find_lines.py b/usr/examples/09-Feature-Detection/find_lines.py index a032fffac..7d5ca45e0 100644 --- a/usr/examples/09-Feature-Detection/find_lines.py +++ b/usr/examples/09-Feature-Detection/find_lines.py @@ -1,18 +1,20 @@ # Find Lines Example # # This example shows off how to find lines in the image. For each line object -# found in the image a line object is returned which includes the lines rotation. +# found in the image a line object is returned which includes the line's rotation. # Note: Line detection is done by using the Hough Transform: # http://en.wikipedia.org/wiki/Hough_transform # Please read about it above for more information on what `theta` and `rho` are. +# find_lines() finds infinite length lines. Use find_line_segments() to find non-infinite lines. + enable_lens_corr = False # turn on for straighter lines... import sensor, image, time sensor.reset() -sensor.set_pixformat(sensor.GRAYSCALE) +sensor.set_pixformat(sensor.RGB565) # grayscale is faster sensor.set_framesize(sensor.QQVGA) sensor.skip_frames(30) clock = time.clock() @@ -31,13 +33,19 @@ while(True): img = sensor.snapshot() if enable_lens_corr: img.lens_corr(1.8) # for 2.8mm lens... - # `threshold` controls how many lines in the image are found. A lower threshold - # results in more lines being returned - between 0 and 1.0. + # `threshold` controls how many lines in the image are found. Only lines with + # edge difference magnitude sums greater than `threshold` are detected... + + # More about `threshold` - each pixel in the image contributes a magnitude value + # to a line. The sum of all contributions is the magintude for that line. Then + # when lines are merged their magnitudes are added togheter. Note that `threshold` + # filters out lines with low magnitudes before merging. To see the magnitude of + # un-merged lines set `theta_margin` and `rho_margin` to 0... # `theta_margin` and `rho_margin` control merging similar lines. If two lines # theta and rho value differences are less than the margins then they are merged. - for l in img.find_lines(threshold = 0.25, theta_margin = 25, rho_margin = 25): + for l in img.find_lines(threshold = 1000, theta_margin = 25, rho_margin = 25): if (min_degree <= l.theta()) and (l.theta() <= max_degree): img.draw_line(l.line(), color = (255, 0, 0)) # print(l)