diff --git a/conf/modules/cv_blob_locator.xml b/conf/modules/cv_blob_locator.xml index 90667f127c..a8453d2b29 100644 --- a/conf/modules/cv_blob_locator.xml +++ b/conf/modules/cv_blob_locator.xml @@ -5,6 +5,7 @@ Find a colored item and track its geo-location and update a waypoint to it + diff --git a/conf/modules/cv_colorfilter.xml b/conf/modules/cv_colorfilter.xml index 1c1ef97fb0..4cd98fd1fb 100644 --- a/conf/modules/cv_colorfilter.xml +++ b/conf/modules/cv_colorfilter.xml @@ -4,6 +4,7 @@ ColorFilter + diff --git a/conf/modules/cv_detect_contour.xml b/conf/modules/cv_detect_contour.xml index aecad43302..8dacd055ef 100644 --- a/conf/modules/cv_detect_contour.xml +++ b/conf/modules/cv_detect_contour.xml @@ -6,6 +6,7 @@ +
diff --git a/conf/modules/cv_detect_window.xml b/conf/modules/cv_detect_window.xml index b96726354d..6ecdd92a7c 100644 --- a/conf/modules/cv_detect_window.xml +++ b/conf/modules/cv_detect_window.xml @@ -5,6 +5,8 @@ Detect window + +
diff --git a/conf/modules/cv_opencvdemo.xml b/conf/modules/cv_opencvdemo.xml index 27469ced5b..7a8eee0d48 100644 --- a/conf/modules/cv_opencvdemo.xml +++ b/conf/modules/cv_opencvdemo.xml @@ -9,6 +9,7 @@ +
diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index b3b7e07c22..b6625bcb26 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -36,6 +36,8 @@ + + @@ -47,6 +49,8 @@ + + @@ -64,6 +68,7 @@ + @@ -74,6 +79,8 @@ + + diff --git a/conf/modules/cv_qrcode.xml b/conf/modules/cv_qrcode.xml index dd97c15bf6..5b1c7bd749 100644 --- a/conf/modules/cv_qrcode.xml +++ b/conf/modules/cv_qrcode.xml @@ -8,6 +8,7 @@ A telemetry message with the code content is sent when a QR code is detected when qrscan is called. + diff --git a/conf/modules/video_capture.xml b/conf/modules/video_capture.xml index 23450d25a9..be32dd7338 100644 --- a/conf/modules/video_capture.xml +++ b/conf/modules/video_capture.xml @@ -12,6 +12,7 @@ + diff --git a/conf/modules/video_usb_logger.xml b/conf/modules/video_usb_logger.xml index 54d76e4c35..5ad91d947a 100644 --- a/conf/modules/video_usb_logger.xml +++ b/conf/modules/video_usb_logger.xml @@ -11,6 +11,7 @@ + video_thread,pose_history
diff --git a/sw/airborne/modules/computer_vision/colorfilter.c b/sw/airborne/modules/computer_vision/colorfilter.c index 501247499b..43bc02738b 100644 --- a/sw/airborne/modules/computer_vision/colorfilter.c +++ b/sw/airborne/modules/computer_vision/colorfilter.c @@ -29,6 +29,11 @@ #include "modules/computer_vision/lib/vision/image.h" +#ifndef COLORFILTER_FPS +#define COLORFILTER_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(COLORFILTER_FPS) + struct video_listener *listener = NULL; // Filter Settings @@ -58,5 +63,5 @@ struct image_t *colorfilter_func(struct image_t *img) void colorfilter_init(void) { - listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func); -} \ No newline at end of file + listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS); +} diff --git a/sw/airborne/modules/computer_vision/cv.c b/sw/airborne/modules/computer_vision/cv.c index 5c347ee15d..b829122f2b 100644 --- a/sw/airborne/modules/computer_vision/cv.c +++ b/sw/airborne/modules/computer_vision/cv.c @@ -43,7 +43,7 @@ static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B) } -struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func) +struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps) { // Create a new video listener struct video_listener *new_listener = malloc(sizeof(struct video_listener)); @@ -53,7 +53,7 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi new_listener->func = func; new_listener->next = NULL; new_listener->async = NULL; - new_listener->maximum_fps = 0; + new_listener->maximum_fps = fps; // Initialise the device that we want our function to use add_video_device(device); @@ -79,10 +79,10 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi } -struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level) +struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps) { // Create a normal listener - struct video_listener *listener = cv_add_to_device(device, func); + struct video_listener *listener = cv_add_to_device(device, func, fps); // Add asynchronous structure to override default synchronous behavior listener->async = malloc(sizeof(struct cv_async)); diff --git a/sw/airborne/modules/computer_vision/cv.h b/sw/airborne/modules/computer_vision/cv.h index a662b9011e..9d0c999fa8 100644 --- a/sw/airborne/modules/computer_vision/cv.h +++ b/sw/airborne/modules/computer_vision/cv.h @@ -61,8 +61,8 @@ struct video_listener { extern bool add_video_device(struct video_config_t *device); -extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func); -extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level); +extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps); +extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps); extern void cv_run_device(struct video_config_t *device, struct image_t *img); diff --git a/sw/airborne/modules/computer_vision/cv_blob_locator.c b/sw/airborne/modules/computer_vision/cv_blob_locator.c index 8bd5e8943f..1953d53567 100644 --- a/sw/airborne/modules/computer_vision/cv_blob_locator.c +++ b/sw/airborne/modules/computer_vision/cv_blob_locator.c @@ -23,6 +23,11 @@ * Find a colored item and track its geo-location and update a waypoint to it */ +#ifndef BLOB_LOCATOR_FPS +#define BLOB_LOCATOR_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(BLOB_LOCATOR_FPS) + #include "modules/computer_vision/cv_blob_locator.h" #include "modules/computer_vision/cv.h" #include "modules/computer_vision/blob/blob_finder.h" @@ -53,11 +58,13 @@ volatile bool marker_enabled = false; volatile bool window_enabled = false; // Computer vision thread -struct image_t* cv_marker_func(struct image_t *img); -struct image_t* cv_marker_func(struct image_t *img) { +struct image_t *cv_marker_func(struct image_t *img); +struct image_t *cv_marker_func(struct image_t *img) +{ - if (!marker_enabled) + if (!marker_enabled) { return NULL; + } struct marker_deviation_t m = marker(img, marker_size); @@ -73,14 +80,16 @@ struct image_t* cv_marker_func(struct image_t *img) { // Computer vision thread -struct image_t* cv_window_func(struct image_t *img); -struct image_t* cv_window_func(struct image_t *img) { +struct image_t *cv_window_func(struct image_t *img); +struct image_t *cv_window_func(struct image_t *img) +{ - if (!window_enabled) + if (!window_enabled) { return NULL; + } - uint16_t coordinate[2] = {0,0}; + uint16_t coordinate[2] = {0, 0}; uint16_t response = 0; uint32_t integral_image[img->w * img->h]; @@ -88,7 +97,7 @@ struct image_t* cv_window_func(struct image_t *img) { image_create(&gray, img->w, img->h, IMAGE_GRAYSCALE); image_to_grayscale(img, &gray); - response = detect_window_sizes( (uint8_t*)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT); + response = detect_window_sizes((uint8_t *)gray.buf, (uint32_t)img->w, (uint32_t)img->h, coordinate, integral_image, MODE_BRIGHT); image_free(&gray); @@ -98,13 +107,13 @@ struct image_t* cv_window_func(struct image_t *img) { if (response < 92) { - for (int y = 0; y < img->h-1; y++) { + for (int y = 0; y < img->h - 1; y++) { Img(px, y) = 65; - Img(px+1, y) = 255; + Img(px + 1, y) = 255; } - for (int x = 0; x < img->w-1; x+=2) { + for (int x = 0; x < img->w - 1; x += 2) { Img(x, py) = 65; - Img(x+1, py) = 255; + Img(x + 1, py) = 255; } uint32_t temp = coordinate[0]; @@ -118,11 +127,13 @@ struct image_t* cv_window_func(struct image_t *img) { } -struct image_t* cv_blob_locator_func(struct image_t *img); -struct image_t* cv_blob_locator_func(struct image_t *img) { +struct image_t *cv_blob_locator_func(struct image_t *img); +struct image_t *cv_blob_locator_func(struct image_t *img) +{ - if (!blob_enabled) + if (!blob_enabled) { return NULL; + } // Color Filter @@ -137,9 +148,9 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { // Output image struct image_t dst; image_create(&dst, - img->w, - img->h, - IMAGE_GRADIENT); + img->w, + img->h, + IMAGE_GRADIENT); // Labels uint16_t labels_count = 512; @@ -152,7 +163,7 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { int largest_size = 0; // Find largest - for (int i=0; i 50) { if (labels[i].pixel_cnt > largest_size) { @@ -162,21 +173,20 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { } } - if (largest_id >= 0) - { - uint8_t *p = (uint8_t*) img->buf; - uint16_t* l = (uint16_t*) dst.buf; - for (int y=0;y= 0) { + uint8_t *p = (uint8_t *) img->buf; + uint16_t *l = (uint16_t *) dst.buf; + for (int y = 0; y < dst.h; y++) { + for (int x = 0; x < dst.w / 2; x++) { + if (l[y * dst.w + x] != 0xffff) { + uint8_t c = 0xff; + if (l[y * dst.w + x] == largest_id) { c = 0; } - p[y*dst.w*2+x*4]=c; - p[y*dst.w*2+x*4+1]=0x80; - p[y*dst.w*2+x*4+2]=c; - p[y*dst.w*2+x*4+3]=0x80; + p[y * dst.w * 2 + x * 4] = c; + p[y * dst.w * 2 + x * 4 + 1] = 0x80; + p[y * dst.w * 2 + x * 4 + 2] = c; + p[y * dst.w * 2 + x * 4 + 3] = 0x80; } } } @@ -185,19 +195,19 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { uint16_t cgx = labels[largest_id].x_sum / labels[largest_id].pixel_cnt * 2; uint16_t cgy = labels[largest_id].y_sum / labels[largest_id].pixel_cnt; - if ((cgx > 1) && (cgx < (dst.w-2)) && - (cgy > 1) && (cgy < (dst.h-2)) - ) { - p[cgy*dst.w*2+cgx*2-4] = 0xff; - p[cgy*dst.w*2+cgx*2-2] = 0x00; - p[cgy*dst.w*2+cgx*2] = 0xff; - p[cgy*dst.w*2+cgx*2+2] = 0x00; - p[cgy*dst.w*2+cgx*2+4] = 0xff; - p[cgy*dst.w*2+cgx*2+6] = 0x00; - p[(cgy-1)*dst.w*2+cgx*2] = 0xff; - p[(cgy-1)*dst.w*2+cgx*2+2] = 0x00; - p[(cgy+1)*dst.w*2+cgx*2] = 0xff; - p[(cgy+1)*dst.w*2+cgx*2+2] = 0x00; + if ((cgx > 1) && (cgx < (dst.w - 2)) && + (cgy > 1) && (cgy < (dst.h - 2)) + ) { + p[cgy * dst.w * 2 + cgx * 2 - 4] = 0xff; + p[cgy * dst.w * 2 + cgx * 2 - 2] = 0x00; + p[cgy * dst.w * 2 + cgx * 2] = 0xff; + p[cgy * dst.w * 2 + cgx * 2 + 2] = 0x00; + p[cgy * dst.w * 2 + cgx * 2 + 4] = 0xff; + p[cgy * dst.w * 2 + cgx * 2 + 6] = 0x00; + p[(cgy - 1)*dst.w * 2 + cgx * 2] = 0xff; + p[(cgy - 1)*dst.w * 2 + cgx * 2 + 2] = 0x00; + p[(cgy + 1)*dst.w * 2 + cgx * 2] = 0xff; + p[(cgy + 1)*dst.w * 2 + cgx * 2 + 2] = 0x00; } @@ -217,7 +227,8 @@ struct image_t* cv_blob_locator_func(struct image_t *img) { #include -void cv_blob_locator_init(void) { +void cv_blob_locator_init(void) +{ // Red board in sunlight color_lum_min = 100; color_lum_max = 200; @@ -238,40 +249,41 @@ void cv_blob_locator_init(void) { georeference_init(); - cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func); - cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func); - cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS); } -void cv_blob_locator_periodic(void) { +void cv_blob_locator_periodic(void) +{ } -void cv_blob_locator_event(void) { - switch (cv_blob_locator_type) - { - case 1: - blob_enabled = true; - marker_enabled = false; - window_enabled = false; - break; - case 2: - blob_enabled = false; - marker_enabled = true; - window_enabled = false; - break; - case 3: - blob_enabled = false; - marker_enabled = false; - window_enabled = true; - break; - default: - blob_enabled = false; - marker_enabled = false; - window_enabled = false; - break; +void cv_blob_locator_event(void) +{ + switch (cv_blob_locator_type) { + case 1: + blob_enabled = true; + marker_enabled = false; + window_enabled = false; + break; + case 2: + blob_enabled = false; + marker_enabled = true; + window_enabled = false; + break; + case 3: + blob_enabled = false; + marker_enabled = false; + window_enabled = true; + break; + default: + blob_enabled = false; + marker_enabled = false; + window_enabled = false; + break; } if (blob_locator != 0) { // CV thread has results: import @@ -282,11 +294,11 @@ void cv_blob_locator_event(void) { uint16_t y = temp & 0x0000ffff; temp = temp >> 16; uint16_t x = temp & 0x0000ffff; - printf("Found %d %d \n",x,y); + printf("Found %d %d \n", x, y); struct camera_frame_t cam; - cam.px = x/2; - cam.py = y/2; + cam.px = x / 2; + cam.py = y / 2; cam.f = 400; cam.h = 240; cam.w = 320; @@ -295,31 +307,36 @@ void cv_blob_locator_event(void) { georeference_project(&cam, WP_p1); #endif #ifdef WP_CAM - georeference_filter(FALSE,WP_CAM, geofilter_length); + georeference_filter(FALSE, WP_CAM, geofilter_length); #endif } } -extern void cv_blob_locator_start(void) { +extern void cv_blob_locator_start(void) +{ georeference_init(); } -extern void cv_blob_locator_stop(void) { +extern void cv_blob_locator_stop(void) +{ } -void start_vision(void) { +void start_vision(void) +{ georeference_init(); record_video = 1; cv_blob_locator_type = 3; } -void start_vision_land(void) { +void start_vision_land(void) +{ georeference_init(); record_video = 1; cv_blob_locator_type = 2; } -void stop_vision(void) { +void stop_vision(void) +{ georeference_init(); record_video = 0; cv_blob_locator_type = 0; diff --git a/sw/airborne/modules/computer_vision/cv_opencvdemo.c b/sw/airborne/modules/computer_vision/cv_opencvdemo.c index cbb022120e..f77bd8425f 100644 --- a/sw/airborne/modules/computer_vision/cv_opencvdemo.c +++ b/sw/airborne/modules/computer_vision/cv_opencvdemo.c @@ -27,16 +27,19 @@ #include "modules/computer_vision/cv_opencvdemo.h" #include "modules/computer_vision/opencv_example.h" +#ifndef OPENCVDEMO_FPS +#define OPENCVDEMO_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(OPENCVDEMO_FPS) // Function -struct image_t* opencv_func(struct image_t* img); -struct image_t* opencv_func(struct image_t* img) +struct image_t *opencv_func(struct image_t *img); +struct image_t *opencv_func(struct image_t *img) { - if (img->type == IMAGE_YUV422) - { + if (img->type == IMAGE_YUV422) { // Call OpenCV (C++ from paparazzi C function) - opencv_example((char*) img->buf, img->w, img->h); + opencv_example((char *) img->buf, img->w, img->h); } // opencv_example(NULL, 10,10); @@ -46,6 +49,6 @@ struct image_t* opencv_func(struct image_t* img) void opencvdemo_init(void) { - cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func); + cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS); } diff --git a/sw/airborne/modules/computer_vision/detect_contour.c b/sw/airborne/modules/computer_vision/detect_contour.c index 4d3db0dc17..2f5d8c4d3e 100644 --- a/sw/airborne/modules/computer_vision/detect_contour.c +++ b/sw/airborne/modules/computer_vision/detect_contour.c @@ -23,6 +23,11 @@ * */ +#ifndef DETECT_CONTOUR_FPS +#define DETECT_CONTOUR_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(DETECT_CONTOUR_FPS) + #include "modules/computer_vision/cv.h" #include "modules/computer_vision/detect_contour.h" #include "modules/computer_vision/opencv_contour.h" @@ -41,7 +46,7 @@ struct image_t *contour_func(struct image_t *img) void detect_contour_init(void) { - cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func); + cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS); // in the mavlab, bright cont_thres.lower_y = 16; cont_thres.lower_u = 135; cont_thres.lower_v = 80; cont_thres.upper_y = 100; cont_thres.upper_u = 175; cont_thres.upper_v = 165; diff --git a/sw/airborne/modules/computer_vision/detect_window.c b/sw/airborne/modules/computer_vision/detect_window.c index c0cf08b3a5..3576084bcc 100644 --- a/sw/airborne/modules/computer_vision/detect_window.c +++ b/sw/airborne/modules/computer_vision/detect_window.c @@ -27,16 +27,21 @@ #define RES 100 #define N_WINDOW_SIZES 1 +#ifndef DETECT_WINDOW_FPS +#define DETECT_WINDOW_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(DETECT_WINDOW_FPS) + #include "cv.h" #include "detect_window.h" #include void detect_window_init(void) { - cv_add_to_device(&BLOB_LOCATOR_CAMERA, detect_window); + cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS); } -struct image_t* detect_window(struct image_t *img) +struct image_t *detect_window(struct image_t *img) { uint16_t coordinate[2]; 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..79fd141312 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -61,6 +61,10 @@ PRINT_CONFIG_VAR(OPTICFLOW_BODY_TO_IMU_ID) #endif PRINT_CONFIG_VAR(OPTICFLOW_SEND_ABI_ID) +#ifndef OPTICFLOW_FPS +#define OPTICFLOW_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(OPTICFLOW_FPS) /* The main opticflow variables */ struct opticflow_t opticflow; ///< Opticflow calculations @@ -126,7 +130,7 @@ void opticflow_module_init(void) opticflow_got_result = false; opticflow_calc_init(&opticflow); - cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc); + cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send); @@ -183,7 +187,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 diff --git a/sw/airborne/modules/computer_vision/qrcode/qr_code.c b/sw/airborne/modules/computer_vision/qrcode/qr_code.c index e9bbcd8c56..261c5d121d 100644 --- a/sw/airborne/modules/computer_vision/qrcode/qr_code.c +++ b/sw/airborne/modules/computer_vision/qrcode/qr_code.c @@ -34,10 +34,15 @@ #endif bool drawRectangleAroundQRCode = QRCODE_DRAW_RECTANGLE; +#ifndef QRCODE_FPS +#define QRCODE_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(QRCODE_FPS) + void qrcode_init(void) { // Add qrscan to the list of image processing tasks in video_thread - cv_add_to_device(&QRCODE_CAMERA, qrscan); + cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS); } // Telemetry diff --git a/sw/airborne/modules/computer_vision/video_capture.c b/sw/airborne/modules/computer_vision/video_capture.c index aa20d15633..d815aef535 100644 --- a/sw/airborne/modules/computer_vision/video_capture.c +++ b/sw/airborne/modules/computer_vision/video_capture.c @@ -46,6 +46,11 @@ #define VIDEO_CAPTURE_JPEG_QUALITY 99 #endif +#ifndef VIDEO_CAPTURE_FPS +#define VIDEO_CAPTURE_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(VIDEO_CAPTURE_FPS) + // Module settings bool video_capture_take_shot = false; int video_capture_index = 0; @@ -66,7 +71,7 @@ void video_capture_init(void) } // Add function to computer vision pipeline - cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func); + cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS); } diff --git a/sw/airborne/modules/computer_vision/video_usb_logger.c b/sw/airborne/modules/computer_vision/video_usb_logger.c index 3296e96be8..5a105b7e7b 100644 --- a/sw/airborne/modules/computer_vision/video_usb_logger.c +++ b/sw/airborne/modules/computer_vision/video_usb_logger.c @@ -43,6 +43,11 @@ #define VIDEO_USB_LOGGER_PATH /data/video/usb #endif +#ifndef VIDEO_USB_LOGGER_FPS +#define VIDEO_USB_LOGGER_FPS 0 ///< Default FPS (zero means run at camera fps) +#endif +PRINT_CONFIG_VAR(VIDEO_USB_LOGGER_FPS) + /** The file pointer */ static FILE *video_usb_logger = NULL; struct image_t img_jpeg_global; @@ -143,7 +148,7 @@ void video_usb_logger_start(void) } // Subscribe to a camera - cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image); + cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS); } /** Stop the logger an nicely close the file */ diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index 2a233c58e5..8e9f294398 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -246,15 +246,13 @@ void viewvideo_init(void) #ifdef VIEWVIDEO_CAMERA struct video_listener *listener1 = cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function1, - VIEWVIDEO_NICE_LEVEL); - listener1->maximum_fps = VIEWVIDEO_FPS; - fprintf(stderr, "[viewvideo] Added asynchronous video streamer lister for CAMERA1\n"); + VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS); + fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA1 at %u FPS \n", VIEWVIDEO_FPS); #endif #ifdef VIEWVIDEO_CAMERA2 struct video_listener *listener2 = cv_add_to_device_async(&VIEWVIDEO_CAMERA2, viewvideo_function2, - VIEWVIDEO_NICE_LEVEL); - listener2->maximum_fps = VIEWVIDEO_FPS; - fprintf(stderr, "[viewvideo] Added asynchronous video streamer lister for CAMERA2\n"); + VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS); + fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA2 at %u FPS \n", VIEWVIDEO_FPS); #endif }