Added an FPS parameter for all computer vision related functions. It sets the frequency of the listener attached to the camera by the module (by default set to the camera's frame rate)

This commit is contained in:
Manos Kyriakakis
2017-06-16 05:37:52 +02:00
parent 3ddd5d737b
commit 3f2f63d0ac
21 changed files with 128 additions and 109 deletions
+1
View File
@@ -5,6 +5,7 @@
<description>Find a colored item and track its geo-location and update a waypoint to it</description>
<define name="BLOB_LOCATOR_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="BLOB_LOCATOR_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<settings>
<dl_settings>
+1
View File
@@ -4,6 +4,7 @@
<doc>
<description>ColorFilter</description>
<define name="COLORFILTER_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="COLORFILTER_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<settings>
+1
View File
@@ -6,6 +6,7 @@
</description>
<define name="DETECT_CONTOUR_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="DETECT_CONTOUR_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<header>
<file name="detect_contour.h"/>
+2
View File
@@ -5,6 +5,8 @@
<description>
Detect window
</description>
<define name="DETECT_WINDOW_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="DETECT_WINDOW_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<header>
+1
View File
@@ -9,6 +9,7 @@
</description>
<define name="OPENCVDEMO_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="OPENCVDEMO_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<header>
<file name="cv_opencvdemo.h"/>
+1
View File
@@ -37,6 +37,7 @@
<define name="KALMAN_FILTER" value="1" description="A kalman filter on the resulting velocities to be turned on or off (fused with accelerometers)"/>
<define name="KALMAN_FILTER_PROCESS_NOISE" value="0.01" description="The expected variance of the error of the model's prediction in the kalman filter"/>
<define name="FEATURE_MANAGEMENT" value="1" description="Whether to keep already tracked corners in memory for the next frame or re-detect new ones every time"/>
<define name="FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
<!-- Lucas Kanade optical flow calculation parameters -->
<define name="MAX_TRACK_CORNERS" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
+1
View File
@@ -8,6 +8,7 @@
A telemetry message with the code content is sent when a QR code is detected when qrscan is called.
</description>
<define name="QRCODE_CAMERA" value="front_camera|bottom_camera" description="The V4L2 camera device that is used for searching a QR code"/>
<define name="QRCODE_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
<define name="QRCODE_DRAW_RECTANGLE" value="TRUE|FALSE" description="Whether or not to draw a rectangle around a found QR code"/>
</doc>
+1
View File
@@ -12,6 +12,7 @@
<define name="VIDEO_CAPTURE_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="VIDEO_CAPTURE_PATH" value="/data/video/images" description="Location to save images"/>
<define name="VIDEO_CAPTURE_JPEG_QUALITY" value="99" description="JPEG quality of images"/>
<define name="VIDEO_CAPTURE_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<settings>
+1
View File
@@ -11,6 +11,7 @@
<define name="VIDEO_USB_LOGGER_WIDTH" value="272" description="Size of the to log images"/>
<define name="VIDEO_USB_LOGGER_HEIGHTH" value="272" description="Size of the to log images"/>
<define name="VIDEO_USB_LOGGER_JPEG_WITH_EXIF_HEADER" value="TRUE" description="Whether to store data in the exif header or not"/>
<define name="VIDEO_USB_LOGGER_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
</doc>
<depends>video_thread,pose_history</depends>
<header>
@@ -58,5 +58,5 @@ struct image_t *colorfilter_func(struct image_t *img)
void colorfilter_init(void)
{
listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func);
}
listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS);
}
+4 -4
View File
@@ -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));
+2 -2
View File
@@ -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);
@@ -53,11 +53,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 +75,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 +92,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 +102,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 +122,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 +143,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 +158,7 @@ struct image_t* cv_blob_locator_func(struct image_t *img) {
int largest_size = 0;
// Find largest
for (int i=0; i<labels_count; i++) {
for (int i = 0; i < labels_count; i++) {
// Only consider large blobs
if (labels[i].pixel_cnt > 50) {
if (labels[i].pixel_cnt > largest_size) {
@@ -162,21 +168,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<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) {
if (largest_id >= 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 +190,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 +222,8 @@ struct image_t* cv_blob_locator_func(struct image_t *img) {
#include <stdio.h>
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 +244,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 +289,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 +302,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;
@@ -29,14 +29,13 @@
// 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 +45,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);
}
@@ -41,7 +41,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;
@@ -33,10 +33,10 @@
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];
@@ -126,7 +126,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);
@@ -37,7 +37,7 @@ bool drawRectangleAroundQRCode = QRCODE_DRAW_RECTANGLE;
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
@@ -66,7 +66,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);
}
@@ -143,7 +143,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 */
@@ -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
}