diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor.xml b/conf/airframes/tudelft/bebop_opticflow_indoor.xml index e40c2e4298..1c91686818 100644 --- a/conf/airframes/tudelft/bebop_opticflow_indoor.xml +++ b/conf/airframes/tudelft/bebop_opticflow_indoor.xml @@ -29,23 +29,32 @@ - - - - - + + + + + + + + + + + + + - + + diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 2f6465f751..9f4c08aecf 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -6,6 +6,7 @@ Compute optical flow. Made for Linux video Devices. Computes x and y velocity using optical flow and distance to ground (using sonar). + Cannot define CAMERA2 without first defining CAMERA. @@ -39,7 +40,7 @@ - + @@ -57,51 +58,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - - - - + + + + + + + + + - - - - + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + video_thread -
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 13c1a3e3d8..d88fbb49e4 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -530,7 +530,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/video_rtp_stream.xml modules/video_capture.xml modules/cv_opticflow.xml modules/bebop_cam.xml modules/ins_hff_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml" + settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_rtp_stream.xml" gui_color="blue" release="398071ca8f7c8dc24becbe76bc5628b6ea604248" /> diff --git a/sw/airborne/modules/computer_vision/bebop_ae_awb.c b/sw/airborne/modules/computer_vision/bebop_ae_awb.c index 41cd747ed2..cb596b8535 100644 --- a/sw/airborne/modules/computer_vision/bebop_ae_awb.c +++ b/sw/airborne/modules/computer_vision/bebop_ae_awb.c @@ -146,7 +146,7 @@ PRINT_CONFIG_VAR(BEBOP_AWB_MIN_GREY_PIXELS) struct ae_setting_t ae_set; struct awb_setting_t awb_set; -static struct image_t *update_ae_awb(struct image_t *img) +static struct image_t *update_ae_awb(struct image_t *img, uint8_t camera_id) { static struct isp_yuv_stats_t yuv_stats; @@ -352,5 +352,5 @@ void bebop_ae_awb_init(void) awb_set.gain_scheduling_tolerance = BEBOP_AWB_GAIN_SCHEDULING_TOLERANCE; awb_set.gain_scheduling_step = BEBOP_AWB_GAIN_SCHEDULING_STEP; - cv_add_to_device_async(&front_camera, update_ae_awb, BEBOP_AE_AWB_NICE, 0); + cv_add_to_device_async(&front_camera, update_ae_awb, BEBOP_AE_AWB_NICE, 0, 0); } diff --git a/sw/airborne/modules/computer_vision/colorfilter.c b/sw/airborne/modules/computer_vision/colorfilter.c index 610071015b..fccbbf2d6a 100644 --- a/sw/airborne/modules/computer_vision/colorfilter.c +++ b/sw/airborne/modules/computer_vision/colorfilter.c @@ -56,7 +56,7 @@ volatile int color_count = 0; #include "subsystems/abi.h" // Function -static struct image_t *colorfilter_func(struct image_t *img) +static struct image_t *colorfilter_func(struct image_t *img, uint8_t camera_id) { // Filter color_count = image_yuv422_colorfilt(img, img, @@ -81,5 +81,5 @@ static struct image_t *colorfilter_func(struct image_t *img) void colorfilter_init(void) { - cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS); + cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS, 0); } diff --git a/sw/airborne/modules/computer_vision/cv.c b/sw/airborne/modules/computer_vision/cv.c index 38dd36e6e7..1952895ba9 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, uint16_t fps) +struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id) { // Create a new video listener struct video_listener *new_listener = malloc(sizeof(struct video_listener)); @@ -54,6 +54,7 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi new_listener->next = NULL; new_listener->async = NULL; new_listener->maximum_fps = fps; + new_listener->id = id; // Initialise the device that we want our function to use add_video_device(device); @@ -80,10 +81,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, - uint16_t fps) + uint16_t fps, uint8_t id) { // Create a normal listener - struct video_listener *listener = cv_add_to_device(device, func, fps); + struct video_listener *listener = cv_add_to_device(device, func, fps, id); // Add asynchronous structure to override default synchronous behavior listener->async = malloc(sizeof(struct cv_async)); @@ -163,7 +164,7 @@ void *cv_async_thread(void *args) } // Execute vision function from this thread - listener->func(&async->img_copy); + listener->func(&async->img_copy, listener->id); // Mark image as processed async->img_processed = true; @@ -198,7 +199,7 @@ void cv_run_device(struct video_config_t *device, struct image_t *img) } } else { // Execute the cvFunction and catch result - result = listener->func(img); + result = listener->func(img, listener->id); // If result gives an image pointer, use it in the next stage if (result != NULL) { diff --git a/sw/airborne/modules/computer_vision/cv.h b/sw/airborne/modules/computer_vision/cv.h index 9051e74d65..6ca4d47572 100644 --- a/sw/airborne/modules/computer_vision/cv.h +++ b/sw/airborne/modules/computer_vision/cv.h @@ -36,7 +36,7 @@ #include BOARD_CONFIG -typedef struct image_t *(*cv_function)(struct image_t *img); +typedef struct image_t *(*cv_function)(struct image_t *img, uint8_t camera_id); struct cv_async { pthread_t thread_id; @@ -53,6 +53,7 @@ struct video_listener { struct cv_async *async; struct timeval ts; cv_function func; + uint8_t id; // Can be set by user uint16_t maximum_fps; @@ -61,9 +62,9 @@ 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, uint16_t fps); +extern struct video_listener *cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id); extern struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, - uint16_t fps); + uint16_t fps, uint8_t id); 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 f85acc2201..a5d78a05b5 100644 --- a/sw/airborne/modules/computer_vision/cv_blob_locator.c +++ b/sw/airborne/modules/computer_vision/cv_blob_locator.c @@ -58,8 +58,8 @@ 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, uint8_t camera_id); +struct image_t *cv_marker_func(struct image_t *img, uint8_t camera_id) { if (!marker_enabled) { @@ -80,8 +80,8 @@ 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, uint8_t camera_id); +struct image_t *cv_window_func(struct image_t *img, uint8_t camera_id) { if (!window_enabled) { @@ -127,8 +127,8 @@ 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, uint8_t camera_id); +struct image_t *cv_blob_locator_func(struct image_t *img, uint8_t camera_id) { if (!blob_enabled) { @@ -249,9 +249,9 @@ void cv_blob_locator_init(void) georeference_init(); - 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); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_blob_locator_func, BLOB_LOCATOR_FPS, 0); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_marker_func, BLOB_LOCATOR_FPS, 0); + cv_add_to_device(&BLOB_LOCATOR_CAMERA, cv_window_func, BLOB_LOCATOR_FPS, 0); } void cv_blob_locator_periodic(void) diff --git a/sw/airborne/modules/computer_vision/cv_detect_color_object.c b/sw/airborne/modules/computer_vision/cv_detect_color_object.c index d2d619a8be..b0b4ccd042 100644 --- a/sw/airborne/modules/computer_vision/cv_detect_color_object.c +++ b/sw/airborne/modules/computer_vision/cv_detect_color_object.c @@ -139,14 +139,14 @@ static struct image_t *object_detector(struct image_t *img, uint8_t filter) return img; } -struct image_t *object_detector1(struct image_t *img); -struct image_t *object_detector1(struct image_t *img) +struct image_t *object_detector1(struct image_t *img, uint8_t camera_id); +struct image_t *object_detector1(struct image_t *img, uint8_t camera_id) { return object_detector(img, 1); } -struct image_t *object_detector2(struct image_t *img); -struct image_t *object_detector2(struct image_t *img) +struct image_t *object_detector2(struct image_t *img, uint8_t camera_id); +struct image_t *object_detector2(struct image_t *img, uint8_t camera_id) { return object_detector(img, 2); } @@ -168,7 +168,7 @@ void color_object_detector_init(void) cod_draw1 = COLOR_OBJECT_DETECTOR_DRAW1; #endif - cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1); + cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1, 0); #endif #ifdef COLOR_OBJECT_DETECTOR_CAMERA2 @@ -184,7 +184,7 @@ void color_object_detector_init(void) cod_draw2 = COLOR_OBJECT_DETECTOR_DRAW2; #endif - cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2); + cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2, 1); #endif } diff --git a/sw/airborne/modules/computer_vision/cv_opencvdemo.c b/sw/airborne/modules/computer_vision/cv_opencvdemo.c index f77bd8425f..98c36f04a7 100644 --- a/sw/airborne/modules/computer_vision/cv_opencvdemo.c +++ b/sw/airborne/modules/computer_vision/cv_opencvdemo.c @@ -33,8 +33,8 @@ 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, uint8_t camera_id); +struct image_t *opencv_func(struct image_t *img, uint8_t camera_id) { if (img->type == IMAGE_YUV422) { @@ -49,6 +49,6 @@ struct image_t *opencv_func(struct image_t *img) void opencvdemo_init(void) { - cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS); + cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func, OPENCVDEMO_FPS, 0); } diff --git a/sw/airborne/modules/computer_vision/detect_contour.c b/sw/airborne/modules/computer_vision/detect_contour.c index 141822ea95..4befaded28 100644 --- a/sw/airborne/modules/computer_vision/detect_contour.c +++ b/sw/airborne/modules/computer_vision/detect_contour.c @@ -33,8 +33,8 @@ PRINT_CONFIG_VAR(DETECT_CONTOUR_FPS) // Function -struct image_t *contour_func(struct image_t *img); -struct image_t *contour_func(struct image_t *img) +struct image_t *contour_func(struct image_t *img, uint8_t camera_id); +struct image_t *contour_func(struct image_t *img, uint8_t camera_id) { if (img->type == IMAGE_YUV422) { @@ -46,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, DETECT_CONTOUR_FPS); + cv_add_to_device(&DETECT_CONTOUR_CAMERA, contour_func, DETECT_CONTOUR_FPS, 0); // 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_gate.c b/sw/airborne/modules/computer_vision/detect_gate.c index b894442bdd..29ebcc9995 100644 --- a/sw/airborne/modules/computer_vision/detect_gate.c +++ b/sw/airborne/modules/computer_vision/detect_gate.c @@ -131,7 +131,7 @@ struct vision_relative_position_struct { // Function -static struct image_t *detect_gate_func(struct image_t *img) +static struct image_t *detect_gate_func(struct image_t *img, uint8_t camera_id) { // detect the gate and draw it in the image: if (just_filtering) { @@ -354,7 +354,7 @@ void detect_gate_init(void) detect_gate_y = 0; detect_gate_z = 0; - cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS); + cv_add_to_device(&DETECT_GATE_CAMERA, detect_gate_func, DETECT_GATE_FPS, 0); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VISION_POSITION_ESTIMATE, send_detect_gate_visual_position); } diff --git a/sw/airborne/modules/computer_vision/detect_window.c b/sw/airborne/modules/computer_vision/detect_window.c index 15ab5dd3ed..b5b50a915c 100644 --- a/sw/airborne/modules/computer_vision/detect_window.c +++ b/sw/airborne/modules/computer_vision/detect_window.c @@ -39,13 +39,13 @@ PRINT_CONFIG_VAR(DETECT_WINDOW_FPS) void detect_window_init(void) { #ifdef DETECT_WINDOW_CAMERA - cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS); + cv_add_to_device(&DETECT_WINDOW_CAMERA, detect_window, DETECT_WINDOW_FPS, 0); #else #warning "DETECT_WINDOW_CAMERA not defined, CV callback not added to device" #endif } -struct image_t *detect_window(struct image_t *img) +struct image_t *detect_window(struct image_t *img, uint8_t camera_id) { uint16_t coordinate[2]; diff --git a/sw/airborne/modules/computer_vision/detect_window.h b/sw/airborne/modules/computer_vision/detect_window.h index 5cfea9f661..b21730bf89 100644 --- a/sw/airborne/modules/computer_vision/detect_window.h +++ b/sw/airborne/modules/computer_vision/detect_window.h @@ -35,7 +35,7 @@ #include "inttypes.h" extern void detect_window_init(void); -extern struct image_t* detect_window(struct image_t *img); +extern struct image_t* detect_window(struct image_t *img, uint8_t camera_id); uint16_t detect_window_sizes(uint8_t *in, uint32_t image_width, uint32_t image_height, uint16_t *coordinate, uint32_t *integral_image, uint8_t MODE); diff --git a/sw/airborne/modules/computer_vision/lib/vision/act_fast.c b/sw/airborne/modules/computer_vision/lib/vision/act_fast.c index 2d9f237771..bf4cb5bec7 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/act_fast.c +++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.c @@ -22,16 +22,21 @@ All rights reserved. * de Croon, G.C.H.E. "ACT-FAST: efficiently finding corners by actively exploring images.", in submission. * */ - #include "fast_rosten.h" #include "act_fast.h" #include "math.h" #include "image.h" #include "../../opticflow/opticflow_calculator.h" +// ACT-FAST agents arrays // equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c +// TODO Currently hardcoded to two cameras #define MAX_AGENTS FAST9_MAX_CORNERS -struct agent_t agents[MAX_AGENTS]; +#ifndef OPTICFLOW_CAMERA2 + struct agent_t agents[1][MAX_AGENTS]; +#else + struct agent_t agents[2][MAX_AGENTS]; +#endif /** * Do an ACT-FAST corner detection. @@ -46,8 +51,9 @@ struct agent_t agents[MAX_AGENTS]; * @param[in] min_gradient The minimum gradient, in order to determine when to take a long or short step * @param[in] gradient_method: 0 = simple {-1, 0, 1}, 1 = Sobel {-1,0,1,-2,0,2,-1,0,1} */ -void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, - uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method) +void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, + uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, + int gradient_method, int camera_id) { /* @@ -56,7 +62,7 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners * 2) loop over the agents, moving and checking for corners */ - // ensure that n_agents is never bigger than MAX_AGENTS + // ensure that n_agents is never bigger than max_agents n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS; // min_gradient should be bigger than 0: min_gradient = (min_gradient == 0) ? 1 : min_gradient; @@ -84,7 +90,7 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners py = ((float)(rand() % 10000) + 1) / 10000.0f; pnorm = sqrtf(px * px + py * py); struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm}; - agents[a] = ag; + agents[camera_id][a] = ag; a++; if (a == n_agents) { break; } } @@ -105,41 +111,41 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners // loop over the agents for (a = 0; a < n_agents; a++) { // only do something if the agent is active: - if (agents[a].active) { + if (agents[camera_id][a].active) { // check if this position is a corner: - uint16_t x = (uint16_t) agents[a].x; - uint16_t y = (uint16_t) agents[a].y; + uint16_t x = (uint16_t) agents[camera_id][a].x; + uint16_t y = (uint16_t) agents[camera_id][a].y; if (fast9_detect_pixel(img, fast_threshold, x, y)) { // we arrived at a corner, yeah!!! - agents[a].active = 0; + agents[camera_id][a].active = 0; continue; } else { // make a step: - struct point_t loc = { .x = agents[a].x, .y = agents[a].y}; + struct point_t loc = { .x = agents[camera_id][a].x, .y = agents[camera_id][a].y}; image_gradient_pixel(img, &loc, gradient_method, &dx, &dy); int gradient = (abs(dx) + abs(dy)) / 2; if (abs(gradient) >= min_gradient) { // determine the angle and make a step in that direction: float norm_factor = sqrtf((float)(dx * dx + dy * dy)); - agents[a].x += (dy / norm_factor) * short_step; - agents[a].y += (dx / norm_factor) * short_step; + agents[camera_id][a].x += (dy / norm_factor) * short_step; + agents[camera_id][a].y += (dx / norm_factor) * short_step; } else { // make a step in the preferred direction: - agents[a].x += agents[a].preferred_dir_x * long_step; - agents[a].y += agents[a].preferred_dir_y * long_step; + agents[camera_id][a].x += agents[camera_id][a].preferred_dir_x * long_step; + agents[camera_id][a].y += agents[camera_id][a].preferred_dir_y * long_step; } } // let the agent move over the image in a toroid world: - if (agents[a].x > img->w - border) { - agents[a].x = border; - } else if (agents[a].x < border) { - agents[a].x = img->w - border; + if (agents[camera_id][a].x > img->w - border) { + agents[camera_id][a].x = border; + } else if (agents[camera_id][a].x < border) { + agents[camera_id][a].x = img->w - border; } - if (agents[a].y > img->h - border) { - agents[a].y = border; - } else if (agents[a].y < border) { - agents[a].y = img->h - border; + if (agents[camera_id][a].y > img->h - border) { + agents[camera_id][a].y = border; + } else if (agents[camera_id][a].y < border) { + agents[camera_id][a].y = img->h - border; } } } @@ -150,20 +156,20 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners for (a = 0; a < n_agents; a++) { // for active agents do a last check on the new position: - if (agents[a].active) { + if (agents[camera_id][a].active) { // check if the last step brought the agent to a corner: - uint16_t x = (uint16_t) agents[a].x; - uint16_t y = (uint16_t) agents[a].y; + uint16_t x = (uint16_t) agents[camera_id][a].x; + uint16_t y = (uint16_t) agents[camera_id][a].y; if (fast9_detect_pixel(img, fast_threshold, x, y)) { // we arrived at a corner, yeah!!! - agents[a].active = 0; + agents[camera_id][a].active = 0; } } // if inactive, the agent is a corner: - if (!agents[a].active) { - (*ret_corners)[(*num_corners)].x = (uint32_t) agents[a].x; - (*ret_corners)[(*num_corners)].y = (uint32_t) agents[a].y; + if (!agents[camera_id][a].active) { + (*ret_corners)[(*num_corners)].x = (uint32_t) agents[camera_id][a].x; + (*ret_corners)[(*num_corners)].y = (uint32_t) agents[camera_id][a].y; (*num_corners)++; } } diff --git a/sw/airborne/modules/computer_vision/lib/vision/act_fast.h b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h index 2e5802714b..0265dae0e3 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/act_fast.h +++ b/sw/airborne/modules/computer_vision/lib/vision/act_fast.h @@ -37,6 +37,7 @@ struct agent_t { #include "lib/vision/image.h" void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, - uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, int gradient_method); + uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient, + int gradient_method, int camera_id); #endif diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h index 74e0bafdc5..2eb0f03c71 100644 --- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -52,6 +52,7 @@ struct opticflow_result_t { float surface_roughness; ///< Surface roughness as determined with a linear optical flow fit float divergence; ///< Divergence as determined with a linear flow fit + uint8_t camera_id; ///< Camera id as passed to cv_add_to_device float noise_measurement; ///< noise of measurement, for state filter }; diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 9638b7d488..4ff117de0a 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -55,8 +55,8 @@ #define EXHAUSTIVE_FAST 0 #define ACT_FAST 1 // TODO: these are now adapted, but perhaps later could be a setting: -uint16_t n_time_steps = 10; -uint16_t n_agents = 25; +uint16_t n_time_steps[2] = {10, 10}; +uint16_t n_agents[2] = {25, 25}; // What methods are run to determine divergence, lateral flow, etc. // SIZE_DIV looks at line sizes and only calculates divergence @@ -68,68 +68,133 @@ uint16_t n_agents = 25; #ifndef OPTICFLOW_CORNER_METHOD #define OPTICFLOW_CORNER_METHOD ACT_FAST #endif + +#ifndef OPTICFLOW_CORNER_METHOD_CAMERA2 +#define OPTICFLOW_CORNER_METHOD_CAMERA2 ACT_FAST +#endif PRINT_CONFIG_VAR(OPTICFLOW_CORNER_METHOD) +PRINT_CONFIG_VAR(OPTICFLOW_CORNER_METHOD_CAMERA2) /* Set the default values */ #ifndef OPTICFLOW_MAX_TRACK_CORNERS #define OPTICFLOW_MAX_TRACK_CORNERS 25 #endif + +#ifndef OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2 +#define OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2 25 +#endif PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS) +PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2) #ifndef OPTICFLOW_WINDOW_SIZE #define OPTICFLOW_WINDOW_SIZE 10 #endif + +#ifndef OPTICFLOW_WINDOW_SIZE_CAMERA2 +#define OPTICFLOW_WINDOW_SIZE_CAMERA2 10 +#endif PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE) +PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE_CAMERA2) #ifndef OPTICFLOW_SEARCH_DISTANCE #define OPTICFLOW_SEARCH_DISTANCE 20 #endif + +#ifndef OPTICFLOW_SEARCH_DISTANCE_CAMERA2 +#define OPTICFLOW_SEARCH_DISTANCE_CAMERA2 20 +#endif PRINT_CONFIG_VAR(OPTICFLOW_SEARCH_DISTANCE) +PRINT_CONFIG_VAR(OPTICFLOW_SEARCH_DISTANCE_CAMERA2) #ifndef OPTICFLOW_SUBPIXEL_FACTOR #define OPTICFLOW_SUBPIXEL_FACTOR 10 #endif + +#ifndef OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2 +#define OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2 10 +#endif PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR) +PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2) #ifndef OPTICFLOW_RESOLUTION_FACTOR #define OPTICFLOW_RESOLUTION_FACTOR 100 #endif + +#ifndef OPTICFLOW_RESOLUTION_FACTOR_CAMERA2 +#define OPTICFLOW_RESOLUTION_FACTOR_CAMERA2 100 +#endif PRINT_CONFIG_VAR(OPTICFLOW_RESOLUTION_FACTOR) +PRINT_CONFIG_VAR(OPTICFLOW_RESOLUTION_FACTOR_CAMERA2) #ifndef OPTICFLOW_MAX_ITERATIONS #define OPTICFLOW_MAX_ITERATIONS 10 #endif + +#ifndef OPTICFLOW_MAX_ITERATIONS_CAMERA2 +#define OPTICFLOW_MAX_ITERATIONS_CAMERA2 10 +#endif PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS) +PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS_CAMERA2) #ifndef OPTICFLOW_THRESHOLD_VEC #define OPTICFLOW_THRESHOLD_VEC 2 #endif + +#ifndef OPTICFLOW_THRESHOLD_VEC_CAMERA2 +#define OPTICFLOW_THRESHOLD_VEC_CAMERA2 2 +#endif PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC) +PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC_CAMERA2) #ifndef OPTICFLOW_PYRAMID_LEVEL #define OPTICFLOW_PYRAMID_LEVEL 2 #endif + +#ifndef OPTICFLOW_PYRAMID_LEVEL_CAMERA2 +#define OPTICFLOW_PYRAMID_LEVEL_CAMERA2 2 +#endif PRINT_CONFIG_VAR(OPTICFLOW_PYRAMID_LEVEL) +PRINT_CONFIG_VAR(OPTICFLOW_PYRAMID_LEVEL_CAMERA2) #ifndef OPTICFLOW_FAST9_ADAPTIVE #define OPTICFLOW_FAST9_ADAPTIVE TRUE #endif + +#ifndef OPTICFLOW_FAST9_ADAPTIVE_CAMERA2 +#define OPTICFLOW_FAST9_ADAPTIVE_CAMERA2 TRUE +#endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE) +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE_CAMERA2) #ifndef OPTICFLOW_FAST9_THRESHOLD #define OPTICFLOW_FAST9_THRESHOLD 20 #endif + +#ifndef OPTICFLOW_FAST9_THRESHOLD_CAMERA2 +#define OPTICFLOW_FAST9_THRESHOLD_CAMERA2 20 +#endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD) +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD_CAMERA2) #ifndef OPTICFLOW_FAST9_MIN_DISTANCE #define OPTICFLOW_FAST9_MIN_DISTANCE 10 #endif + +#ifndef OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2 +#define OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2 10 +#endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE) +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2) #ifndef OPTICFLOW_FAST9_PADDING #define OPTICFLOW_FAST9_PADDING 20 #endif + +#ifndef OPTICFLOW_FAST9_PADDING_CAMERA2 +#define OPTICFLOW_FAST9_PADDING_CAMERA2 20 +#endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_PADDING) +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_PADDING_CAMERA2) // thresholds FAST9 that are currently not set from the GCS: #define FAST9_LOW_THRESHOLD 5 @@ -139,66 +204,130 @@ PRINT_CONFIG_VAR(OPTICFLOW_FAST9_PADDING) #ifndef OPTICFLOW_METHOD #define OPTICFLOW_METHOD 0 #endif + +#ifndef OPTICFLOW_METHOD_CAMERA2 +#define OPTICFLOW_METHOD_CAMERA2 0 +#endif PRINT_CONFIG_VAR(OPTICFLOW_METHOD) +PRINT_CONFIG_VAR(OPTICFLOW_METHOD_CAMERA2) #if OPTICFLOW_METHOD > 1 -#error WARNING: Both Lukas Kanade and EdgeFlow are NOT selected +#error WARNING: Both Lukas Kanade and EdgeFlow are NOT selected camera1 +#endif + +#if OPTICFLOW_METHOD_CAMERA2 > 1 +#error WARNING: Both Lukas Kanade and EdgeFlow are NOT selected camera2 #endif #ifndef OPTICFLOW_DEROTATION #define OPTICFLOW_DEROTATION TRUE #endif + +#ifndef OPTICFLOW_DEROTATION_CAMERA2 +#define OPTICFLOW_DEROTATION_CAMERA2 TRUE +#endif PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION) +PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CAMERA2) #ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X #define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X 1.0 #endif + +#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2 +#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2 1.0 +#endif PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X) +PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2) #ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y #define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y 1.0 #endif + +#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2 +#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2 1.0 +#endif PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y) +PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2) #ifndef OPTICFLOW_MEDIAN_FILTER #define OPTICFLOW_MEDIAN_FILTER FALSE #endif + +#ifndef OPTICFLOW_MEDIAN_FILTER_CAMERA2 +#define OPTICFLOW_MEDIAN_FILTER_CAMERA2 FALSE +#endif PRINT_CONFIG_VAR(OPTICFLOW_MEDIAN_FILTER) +PRINT_CONFIG_VAR(OPTICFLOW_MEDIAN_FILTER_CAMERA2) #ifndef OPTICFLOW_FEATURE_MANAGEMENT #define OPTICFLOW_FEATURE_MANAGEMENT 0 #endif + +#ifndef OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2 +#define OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2 0 +#endif PRINT_CONFIG_VAR(OPTICFLOW_FEATURE_MANAGEMENT) +PRINT_CONFIG_VAR(OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2) #ifndef OPTICFLOW_FAST9_REGION_DETECT #define OPTICFLOW_FAST9_REGION_DETECT 1 #endif + +#ifndef OPTICFLOW_FAST9_REGION_DETECT_CAMERA2 +#define OPTICFLOW_FAST9_REGION_DETECT_CAMERA2 1 +#endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_REGION_DETECT) +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_REGION_DETECT_CAMERA2) #ifndef OPTICFLOW_FAST9_NUM_REGIONS #define OPTICFLOW_FAST9_NUM_REGIONS 9 #endif + +#ifndef OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2 +#define OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2 9 +#endif PRINT_CONFIG_VAR(OPTICFLOW_FAST9_NUM_REGIONS) +PRINT_CONFIG_VAR(OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2) #ifndef OPTICFLOW_ACTFAST_LONG_STEP #define OPTICFLOW_ACTFAST_LONG_STEP 10 #endif + +#ifndef OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2 +#define OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2 10 +#endif PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_LONG_STEP) +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2) #ifndef OPTICFLOW_ACTFAST_SHORT_STEP #define OPTICFLOW_ACTFAST_SHORT_STEP 2 #endif + +#ifndef OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2 +#define OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2 2 +#endif PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_SHORT_STEP) +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2) #ifndef OPTICFLOW_ACTFAST_GRADIENT_METHOD #define OPTICFLOW_ACTFAST_GRADIENT_METHOD 1 #endif + +#ifndef OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2 +#define OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2 1 +#endif PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_GRADIENT_METHOD) +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2) #ifndef OPTICFLOW_ACTFAST_MIN_GRADIENT #define OPTICFLOW_ACTFAST_MIN_GRADIENT 10 #endif + +#ifndef OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2 +#define OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2 10 +#endif PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_MIN_GRADIENT) +PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2) // Defaults for ARdrone #ifndef OPTICFLOW_BODY_TO_CAM_PHI @@ -211,26 +340,45 @@ PRINT_CONFIG_VAR(OPTICFLOW_ACTFAST_MIN_GRADIENT) #define OPTICFLOW_BODY_TO_CAM_PSI -M_PI_2 #endif +#ifndef OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2 +#define OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2 0 +#endif +#ifndef OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2 +#define OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2 0 +#endif +#ifndef OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2 +#define OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2 -M_PI_2 +#endif + // Tracking back flow to make the accepted flow vectors more robust: // Default is false, as it does take extra processing time #ifndef OPTICFLOW_TRACK_BACK #define OPTICFLOW_TRACK_BACK FALSE #endif + +#ifndef OPTICFLOW_TRACK_BACK_CAMERA2 +#define OPTICFLOW_TRACK_BACK_CAMERA2 FALSE +#endif PRINT_CONFIG_VAR(OPTICFLOW_TRACK_BACK) +PRINT_CONFIG_VAR(OPTICFLOW_TRACK_BACK_CAMERA2) // Whether to draw the flow on the image: // False by default, since it changes the image and costs time. #ifndef OPTICFLOW_SHOW_FLOW #define OPTICFLOW_SHOW_FLOW FALSE #endif -PRINT_CONFIG_VAR(OPTICFLOW_SHOW_FLOW) +#ifndef OPTICFLOW_SHOW_FLOW_CAMERA2 +#define OPTICFLOW_SHOW_FLOW_CAMERA2 FALSE +#endif +PRINT_CONFIG_VAR(OPTICFLOW_SHOW_FLOW) +PRINT_CONFIG_VAR(OPTICFLOW_SHOW_FLOW_CAMERA2) //Include median filter #include "filters/median_filter.h" struct MedianFilter3Float vel_filt; -struct FloatRMat body_to_cam; +struct FloatRMat body_to_cam[2]; /* Functions only used here */ static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime); @@ -245,47 +393,93 @@ static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t * Initialize the opticflow calculator * @param[out] *opticflow The new optical flow calculator */ -void opticflow_calc_init(struct opticflow_t *opticflow) +void opticflow_calc_init(struct opticflow_t opticflow[]) { /* Set the default values */ - opticflow->method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow - opticflow->window_size = OPTICFLOW_WINDOW_SIZE; - opticflow->search_distance = OPTICFLOW_SEARCH_DISTANCE; - opticflow->derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON - opticflow->derotation_correction_factor_x = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X; - opticflow->derotation_correction_factor_y = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y; - opticflow->track_back = OPTICFLOW_TRACK_BACK; - opticflow->show_flow = OPTICFLOW_SHOW_FLOW; - opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS; - opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR; - if (opticflow->subpixel_factor == 0) { - opticflow->subpixel_factor = 10; + opticflow[0].method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow + opticflow[0].window_size = OPTICFLOW_WINDOW_SIZE; + opticflow[0].search_distance = OPTICFLOW_SEARCH_DISTANCE; + opticflow[0].derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON + opticflow[0].derotation_correction_factor_x = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X; + opticflow[0].derotation_correction_factor_y = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y; + opticflow[0].track_back = OPTICFLOW_TRACK_BACK; + opticflow[0].show_flow = OPTICFLOW_SHOW_FLOW; + opticflow[0].max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS; + opticflow[0].subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR; + if (opticflow[0].subpixel_factor == 0) { + opticflow[0].subpixel_factor = 10; } - opticflow->resolution_factor = OPTICFLOW_RESOLUTION_FACTOR; - opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS; - opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC; - opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL; - opticflow->median_filter = OPTICFLOW_MEDIAN_FILTER; - opticflow->feature_management = OPTICFLOW_FEATURE_MANAGEMENT; - opticflow->fast9_region_detect = OPTICFLOW_FAST9_REGION_DETECT; - opticflow->fast9_num_regions = OPTICFLOW_FAST9_NUM_REGIONS; + opticflow[0].resolution_factor = OPTICFLOW_RESOLUTION_FACTOR; + opticflow[0].max_iterations = OPTICFLOW_MAX_ITERATIONS; + opticflow[0].threshold_vec = OPTICFLOW_THRESHOLD_VEC; + opticflow[0].pyramid_level = OPTICFLOW_PYRAMID_LEVEL; + opticflow[0].median_filter = OPTICFLOW_MEDIAN_FILTER; + opticflow[0].feature_management = OPTICFLOW_FEATURE_MANAGEMENT; + opticflow[0].fast9_region_detect = OPTICFLOW_FAST9_REGION_DETECT; + opticflow[0].fast9_num_regions = OPTICFLOW_FAST9_NUM_REGIONS; - opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE; - opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD; - opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE; - opticflow->fast9_padding = OPTICFLOW_FAST9_PADDING; - opticflow->fast9_rsize = FAST9_MAX_CORNERS; - opticflow->fast9_ret_corners = calloc(opticflow->fast9_rsize, sizeof(struct point_t)); + opticflow[0].fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE; + opticflow[0].fast9_threshold = OPTICFLOW_FAST9_THRESHOLD; + opticflow[0].fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE; + opticflow[0].fast9_padding = OPTICFLOW_FAST9_PADDING; + opticflow[0].fast9_rsize = FAST9_MAX_CORNERS; + opticflow[0].fast9_ret_corners = calloc(opticflow[0].fast9_rsize, sizeof(struct point_t)); - opticflow->corner_method = OPTICFLOW_CORNER_METHOD; - opticflow->actfast_long_step = OPTICFLOW_ACTFAST_LONG_STEP; - opticflow->actfast_short_step = OPTICFLOW_ACTFAST_SHORT_STEP; - opticflow->actfast_min_gradient = OPTICFLOW_ACTFAST_MIN_GRADIENT; - opticflow->actfast_gradient_method = OPTICFLOW_ACTFAST_GRADIENT_METHOD; + opticflow[0].corner_method = OPTICFLOW_CORNER_METHOD; + opticflow[0].actfast_long_step = OPTICFLOW_ACTFAST_LONG_STEP; + opticflow[0].actfast_short_step = OPTICFLOW_ACTFAST_SHORT_STEP; + opticflow[0].actfast_min_gradient = OPTICFLOW_ACTFAST_MIN_GRADIENT; + opticflow[0].actfast_gradient_method = OPTICFLOW_ACTFAST_GRADIENT_METHOD; - struct FloatEulers euler = {OPTICFLOW_BODY_TO_CAM_PHI, OPTICFLOW_BODY_TO_CAM_THETA, OPTICFLOW_BODY_TO_CAM_PSI}; - float_rmat_of_eulers(&body_to_cam, &euler); + opticflow[0].camera = &OPTICFLOW_CAMERA; + opticflow[0].id = 0; + struct FloatEulers euler_cam1 = {OPTICFLOW_BODY_TO_CAM_PHI, OPTICFLOW_BODY_TO_CAM_THETA, OPTICFLOW_BODY_TO_CAM_PSI}; + float_rmat_of_eulers(&body_to_cam[0], &euler_cam1); + +#ifdef OPTICFLOW_CAMERA2 + opticflow[1].method = OPTICFLOW_METHOD_CAMERA2; //0 = LK_fast9, 1 = Edgeflow + opticflow[1].window_size = OPTICFLOW_WINDOW_SIZE_CAMERA2; + opticflow[1].search_distance = OPTICFLOW_SEARCH_DISTANCE_CAMERA2; + opticflow[1].derotation = OPTICFLOW_DEROTATION_CAMERA2; //0 = OFF, 1 = ON + opticflow[1].derotation_correction_factor_x = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X_CAMERA2; + opticflow[1].derotation_correction_factor_y = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y_CAMERA2; + opticflow[1].track_back = OPTICFLOW_TRACK_BACK_CAMERA2; + opticflow[1].show_flow = OPTICFLOW_SHOW_FLOW_CAMERA2; + opticflow[1].max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS_CAMERA2; + opticflow[1].subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR_CAMERA2; + if (opticflow[1].subpixel_factor == 0) { + opticflow[1].subpixel_factor = 10; + } + opticflow[1].resolution_factor = OPTICFLOW_RESOLUTION_FACTOR_CAMERA2; + opticflow[1].max_iterations = OPTICFLOW_MAX_ITERATIONS_CAMERA2; + opticflow[1].threshold_vec = OPTICFLOW_THRESHOLD_VEC_CAMERA2; + opticflow[1].pyramid_level = OPTICFLOW_PYRAMID_LEVEL_CAMERA2; + opticflow[1].median_filter = OPTICFLOW_MEDIAN_FILTER_CAMERA2; + opticflow[1].feature_management = OPTICFLOW_FEATURE_MANAGEMENT_CAMERA2; + opticflow[1].fast9_region_detect = OPTICFLOW_FAST9_REGION_DETECT_CAMERA2; + opticflow[1].fast9_num_regions = OPTICFLOW_FAST9_NUM_REGIONS_CAMERA2; + + opticflow[1].fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE_CAMERA2; + opticflow[1].fast9_threshold = OPTICFLOW_FAST9_THRESHOLD_CAMERA2; + opticflow[1].fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE_CAMERA2; + opticflow[1].fast9_padding = OPTICFLOW_FAST9_PADDING_CAMERA2; + opticflow[1].fast9_rsize = FAST9_MAX_CORNERS; + opticflow[1].fast9_ret_corners = calloc(opticflow[0].fast9_rsize, sizeof(struct point_t)); + + opticflow[1].corner_method = OPTICFLOW_CORNER_METHOD_CAMERA2; + opticflow[1].actfast_long_step = OPTICFLOW_ACTFAST_LONG_STEP_CAMERA2; + opticflow[1].actfast_short_step = OPTICFLOW_ACTFAST_SHORT_STEP_CAMERA2; + opticflow[1].actfast_min_gradient = OPTICFLOW_ACTFAST_MIN_GRADIENT_CAMERA2; + opticflow[1].actfast_gradient_method = OPTICFLOW_ACTFAST_GRADIENT_METHOD_CAMERA2; + + opticflow[1].camera = &OPTICFLOW_CAMERA2; + opticflow[1].id = 1; + + struct FloatEulers euler_cam2 = {OPTICFLOW_BODY_TO_CAM_PHI_CAMERA2, OPTICFLOW_BODY_TO_CAM_THETA_CAMERA2, + OPTICFLOW_BODY_TO_CAM_PSI_CAMERA2}; + float_rmat_of_eulers(&body_to_cam[1], &euler_cam2); +#endif } /** * Run the optical flow with fast9 and lukaskanade on a new image frame @@ -342,23 +536,19 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, } else if (!opticflow->feature_management) { // needs to be set to 0 because result is now static result->corner_cnt = 0; - if (opticflow->corner_method == EXHAUSTIVE_FAST) { // 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); - + opticflow->fast9_padding, opticflow->fast9_padding, &result->corner_cnt, &opticflow->fast9_rsize, + &opticflow->fast9_ret_corners, NULL); } else if (opticflow->corner_method == ACT_FAST) { // ACT-FAST corner detection: act_fast(&opticflow->prev_img_gray, opticflow->fast9_threshold, &result->corner_cnt, - &opticflow->fast9_ret_corners, n_agents, n_time_steps, + &opticflow->fast9_ret_corners, n_agents[opticflow->id], n_time_steps[opticflow->id], opticflow->actfast_long_step, opticflow->actfast_short_step, opticflow->actfast_min_gradient, - opticflow->actfast_gradient_method); + opticflow->actfast_gradient_method, opticflow->id); } // Adaptive threshold @@ -366,26 +556,26 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, // This works well for exhaustive FAST, but drives the threshold to the minimum for ACT-FAST: // Decrease and increase the threshold based on previous values - if (result->corner_cnt < 40) { // TODO: Replace 40 with OPTICFLOW_MAX_TRACK_CORNERS / 2 + if (result->corner_cnt < opticflow->max_track_corners / 2) { // make detections easier: if (opticflow->fast9_threshold > FAST9_LOW_THRESHOLD) { opticflow->fast9_threshold--; } - if (opticflow->corner_method == ACT_FAST && n_agents < opticflow->fast9_rsize) { - n_time_steps++; - n_agents++; + if (opticflow->corner_method == ACT_FAST && n_agents[opticflow->id] < opticflow->fast9_rsize) { + n_time_steps[opticflow->id]++; + n_agents[opticflow->id]++; } - } else if (result->corner_cnt > OPTICFLOW_MAX_TRACK_CORNERS * 2) { + } else if (result->corner_cnt > opticflow->max_track_corners * 2) { if (opticflow->fast9_threshold < FAST9_HIGH_THRESHOLD) { opticflow->fast9_threshold++; } - if (opticflow->corner_method == ACT_FAST && n_time_steps > 5 && n_agents > 10) { - n_time_steps--; - n_agents--; + if (opticflow->corner_method == ACT_FAST && n_time_steps[opticflow->id] > 5 && n_agents[opticflow->id] > 10) { + n_time_steps[opticflow->id]--; + n_agents[opticflow->id]--; } } } @@ -400,7 +590,8 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, // Clear the result otherwise the previous values will be returned for this frame too VECT3_ASSIGN(result->vel_cam, 0, 0, 0); VECT3_ASSIGN(result->vel_body, 0, 0, 0); - result->div_size = 0; result->divergence = 0; + result->div_size = 0; + result->divergence = 0; result->noise_measurement = 5.0; image_switch(&opticflow->img_gray, &opticflow->prev_img_gray); @@ -542,10 +733,10 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, float theta_diff = opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta; float psi_diff = opticflow->img_gray.eulers.psi - opticflow->prev_img_gray.eulers.psi; - if (strcmp(OPTICFLOW_CAMERA.dev_name, bottom_camera.dev_name) == 0) { + if (strcmp(opticflow->camera->dev_name, bottom_camera.dev_name) == 0) { // bottom cam: just subtract a scaled version of the roll and pitch difference from the global flow vector: - diff_flow_x = phi_diff * OPTICFLOW_CAMERA.camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p) - diff_flow_y = theta_diff * OPTICFLOW_CAMERA.camera_intrinsics.focal_y; + diff_flow_x = phi_diff * opticflow->camera->camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p) + diff_flow_y = theta_diff * opticflow->camera->camera_intrinsics.focal_y; result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor * opticflow->derotation_correction_factor_x; result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor * @@ -581,15 +772,16 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, } } } + result->camera_id = opticflow->id; // Velocity calculation // Right now this formula is under assumption that the flow only exist in the center axis of the camera. // TODO: Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane. // TODO: This is actually only correct for the bottom camera: result->vel_cam.x = (float)result->flow_der_x * result->fps * agl_dist_value_filtered / - (opticflow->subpixel_factor * OPTICFLOW_CAMERA.camera_intrinsics.focal_x); + (opticflow->subpixel_factor * opticflow->camera->camera_intrinsics.focal_x); result->vel_cam.y = (float)result->flow_der_y * result->fps * agl_dist_value_filtered / - (opticflow->subpixel_factor * OPTICFLOW_CAMERA.camera_intrinsics.focal_y); + (opticflow->subpixel_factor * opticflow->camera->camera_intrinsics.focal_y); result->vel_cam.z = result->divergence * result->fps * agl_dist_value_filtered; //Apply a median filter to the velocity if wanted @@ -620,7 +812,6 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, } free(vectors); image_switch(&opticflow->img_gray, &opticflow->prev_img_gray); - return true; } @@ -634,16 +825,16 @@ static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t // reserve memory for the predicted flow vectors: struct flow_t *predicted_flow_vectors = malloc(sizeof(struct flow_t) * n_points); - float K[9] = {OPTICFLOW_CAMERA.camera_intrinsics.focal_x, 0.0f, OPTICFLOW_CAMERA.camera_intrinsics.center_x, - 0.0f, OPTICFLOW_CAMERA.camera_intrinsics.focal_y, OPTICFLOW_CAMERA.camera_intrinsics.center_y, + float K[9] = {opticflow->camera->camera_intrinsics.focal_x, 0.0f, opticflow->camera->camera_intrinsics.center_x, + 0.0f, opticflow->camera->camera_intrinsics.focal_y, opticflow->camera->camera_intrinsics.center_y, 0.0f, 0.0f, 1.0f }; // TODO: make an option to not do distortion / undistortion (Dhane_k = 1) - float k = OPTICFLOW_CAMERA.camera_intrinsics.Dhane_k; + float k = opticflow->camera->camera_intrinsics.Dhane_k; float A, B, C; // as in Longuet-Higgins - if (strcmp(OPTICFLOW_CAMERA.dev_name, front_camera.dev_name) == 0) { + if (strcmp(opticflow->camera->dev_name, front_camera.dev_name) == 0) { // specific for the x,y swapped Bebop 2 images: A = -psi_diff; B = theta_diff; @@ -825,7 +1016,6 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, struct edgeflow_displacement_t displacement; displacement.x = calloc(img->w, sizeof(int32_t)); displacement.y = calloc(img->h, sizeof(int32_t)); - // If the methods just switched to this one, reintialize the // array of edge_hist structure. if (opticflow->just_switched_method == 1 && edge_hist[0].x == NULL) { @@ -836,7 +1026,6 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, FLOAT_EULERS_ZERO(edge_hist[i].eulers); } } - uint16_t disp_range; if (opticflow->search_distance < DISP_RANGE_MAX) { disp_range = opticflow->search_distance; @@ -878,12 +1067,11 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, //Calculate the corresponding derotation of the two frames int16_t der_shift_x = 0; int16_t der_shift_y = 0; - if (opticflow->derotation) { der_shift_x = (int16_t)((edge_hist[current_frame_nr].eulers.phi - edge_hist[previous_frame_nr[0]].eulers.phi) * - OPTICFLOW_CAMERA.camera_intrinsics.focal_x * opticflow->derotation_correction_factor_x); + opticflow->camera->camera_intrinsics.focal_x * opticflow->derotation_correction_factor_x); der_shift_y = (int16_t)((edge_hist[current_frame_nr].eulers.theta - edge_hist[previous_frame_nr[1]].eulers.theta) * - OPTICFLOW_CAMERA.camera_intrinsics.focal_y * opticflow->derotation_correction_factor_y); + opticflow->camera->camera_intrinsics.focal_y * opticflow->derotation_correction_factor_y); } // Estimate pixel wise displacement of the edge histograms for x and y direction @@ -925,10 +1113,9 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, result->tracked_cnt = getAmountPeaks(edge_hist_x, 500, img->w); result->divergence = -1.0 * (float)edgeflow.div_x / RES; // Also multiply the divergence with -1.0 to make it on par with the LK algorithm of - result->div_size = - result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF + result->div_size = result->divergence; // Fill the div_size with the divergence to atleast get some divergenge measurement when switching from LK to EF + result->camera_id = opticflow->id; result->surface_roughness = 0.0f; - //......................Calculating VELOCITY ..................... // /*Estimate fps per direction @@ -948,9 +1135,9 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, // TODO scale flow to rad/s here // Calculate velocity - result->vel_cam.x = edgeflow.flow_x * fps_x * agl_dist_value_filtered * OPTICFLOW_CAMERA.camera_intrinsics.focal_x / + result->vel_cam.x = edgeflow.flow_x * fps_x * agl_dist_value_filtered * opticflow->camera->camera_intrinsics.focal_x / RES; - result->vel_cam.y = edgeflow.flow_y * fps_y * agl_dist_value_filtered * OPTICFLOW_CAMERA.camera_intrinsics.focal_y / + result->vel_cam.y = edgeflow.flow_y * fps_y * agl_dist_value_filtered * opticflow->camera->camera_intrinsics.focal_y / RES; result->vel_cam.z = result->divergence * fps_x * agl_dist_value_filtered; @@ -960,17 +1147,14 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, } result->noise_measurement = 0.2; - -#if OPTICFLOW_SHOW_FLOW - draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x); -#endif + if (opticflow->show_flow) { + draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x); + } // Increment and wrap current time frame current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; - // Free alloc'd variables free(displacement.x); free(displacement.y); - return true; } @@ -988,10 +1172,10 @@ bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, bool flow_successful = false; // A switch counter that checks in the loop if the current method is similar, // to the previous (for reinitializing structs) - static int8_t switch_counter = -1; - if (switch_counter != opticflow->method) { + static int8_t switch_counter[2] = {-1, -1}; + if (switch_counter[opticflow->id] != opticflow->method) { opticflow->just_switched_method = true; - switch_counter = opticflow->method; + switch_counter[opticflow->id] = opticflow->method; // Clear the static result memset(result, 0, sizeof(struct opticflow_result_t)); } else { @@ -1004,13 +1188,12 @@ bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, } else if (opticflow->method == 1) { flow_successful = calc_edgeflow_tot(opticflow, img, result); } - /* Rotate velocities from camera frame coordinates to body coordinates for control * IMPORTANT!!! This frame to body orientation should be the case for the Parrot * ARdrone and Bebop, however this can be different for other quadcopters * ALWAYS double check! */ - float_rmat_transp_vmult(&result->vel_body, &body_to_cam, &result->vel_cam); + float_rmat_transp_vmult(&result->vel_body, &body_to_cam[opticflow->id], &result->vel_cam); return flow_successful; } diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index 38f9363076..f8118c1c3b 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -79,12 +79,13 @@ struct opticflow_t { int actfast_min_gradient; ///< Threshold that decides when there is sufficient texture for edge following int actfast_gradient_method; ///< Whether to use a simple or Sobel filter - + const struct video_config_t *camera; + uint8_t id; }; #define FAST9_MAX_CORNERS 512 -extern void opticflow_calc_init(struct opticflow_t *opticflow); +extern void opticflow_calc_init(struct opticflow_t opticflow[]); extern bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result); diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index a6cce7e1bf..670e754cc8 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -48,17 +48,28 @@ PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID) #ifndef OPTICFLOW_FPS #define OPTICFLOW_FPS 0 ///< Default FPS (zero means run at camera fps) #endif + +#ifndef OPTICFLOW_FPS_CAMERA2 +#define OPTICFLOW_FPS_CAMERA2 0 ///< Default FPS (zero means run at camera fps) +#endif PRINT_CONFIG_VAR(OPTICFLOW_FPS) +PRINT_CONFIG_VAR(OPTICFLOW_FPS_CAMERA2) + +#ifdef OPTICFLOW_CAMERA2 +#define ACTIVE_CAMERAS 2 +#else +#define ACTIVE_CAMERAS 1 +#endif /* The main opticflow variables */ -struct opticflow_t opticflow; ///< Opticflow calculations -static struct opticflow_result_t opticflow_result; ///< The opticflow result +struct opticflow_t opticflow[ACTIVE_CAMERAS]; ///< Opticflow calculations +static struct opticflow_result_t opticflow_result[ACTIVE_CAMERAS]; ///< The opticflow result -static bool opticflow_got_result; ///< When we have an optical flow calculation -static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety +static bool opticflow_got_result[ACTIVE_CAMERAS]; ///< When we have an optical flow calculation +static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety /* Static functions */ -struct image_t *opticflow_module_calc(struct image_t *img); ///< The main optical flow calculation thread +struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id); ///< The main optical flow calculation thread #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -70,15 +81,19 @@ struct image_t *opticflow_module_calc(struct image_t *img); ///< The main op static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev) { pthread_mutex_lock(&opticflow_mutex); - if (opticflow_result.noise_measurement < 0.8) { - pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID, - &opticflow_result.fps, &opticflow_result.corner_cnt, - &opticflow_result.tracked_cnt, &opticflow_result.flow_x, - &opticflow_result.flow_y, &opticflow_result.flow_der_x, - &opticflow_result.flow_der_y, &opticflow_result.vel_body.x, - &opticflow_result.vel_body.y, &opticflow_result.vel_body.z, - &opticflow_result.div_size, &opticflow_result.surface_roughness, - &opticflow_result.divergence); // TODO: no noise measurement here... + for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) { + if (opticflow_result[idx_camera].noise_measurement < 0.8) { + pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID, + &opticflow_result[idx_camera].fps, &opticflow_result[idx_camera].corner_cnt, + &opticflow_result[idx_camera].tracked_cnt, &opticflow_result[idx_camera].flow_x, + &opticflow_result[idx_camera].flow_y, &opticflow_result[idx_camera].flow_der_x, + &opticflow_result[idx_camera].flow_der_y, &opticflow_result[idx_camera].vel_body.x, + &opticflow_result[idx_camera].vel_body.y, &opticflow_result[idx_camera].vel_body.z, + &opticflow_result[idx_camera].div_size, + &opticflow_result[idx_camera].surface_roughness, + &opticflow_result[idx_camera].divergence, + &opticflow_result[idx_camera].camera_id); // TODO: no noise measurement here... + } } pthread_mutex_unlock(&opticflow_mutex); } @@ -90,10 +105,15 @@ static void opticflow_telem_send(struct transport_tx *trans, struct link_device void opticflow_module_init(void) { // Initialize the opticflow calculation - opticflow_got_result = false; - opticflow_calc_init(&opticflow); + for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) { + opticflow_got_result[idx_camera] = false; + } + opticflow_calc_init(opticflow); - cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS); + cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc, OPTICFLOW_FPS, 0); +#ifdef OPTICFLOW_CAMERA2 + cv_add_to_device(&OPTICFLOW_CAMERA2, opticflow_module_calc, OPTICFLOW_FPS_CAMERA2, 1); +#endif #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTIC_FLOW_EST, opticflow_telem_send); @@ -109,27 +129,29 @@ void opticflow_module_run(void) { pthread_mutex_lock(&opticflow_mutex); // Update the stabilization loops on the current calculation - if (opticflow_got_result) { - uint32_t now_ts = get_sys_time_usec(); - AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID, now_ts, - opticflow_result.flow_x, - opticflow_result.flow_y, - opticflow_result.flow_der_x, - opticflow_result.flow_der_y, - opticflow_result.noise_measurement, - opticflow_result.div_size); - //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt - if (opticflow_result.noise_measurement < 0.8) { - AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID, now_ts, - opticflow_result.vel_body.x, - opticflow_result.vel_body.y, - 0.0f, //opticflow_result.vel_body.z, - opticflow_result.noise_measurement, - opticflow_result.noise_measurement, - -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity. - ); + for (int idx_camera = 0; idx_camera < ACTIVE_CAMERAS; idx_camera++) { + if (opticflow_got_result[idx_camera]) { + uint32_t now_ts = get_sys_time_usec(); + AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID + idx_camera, now_ts, + opticflow_result[idx_camera].flow_x, + opticflow_result[idx_camera].flow_y, + opticflow_result[idx_camera].flow_der_x, + opticflow_result[idx_camera].flow_der_y, + opticflow_result[idx_camera].noise_measurement, + opticflow_result[idx_camera].div_size); + //TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt + if (opticflow_result[idx_camera].noise_measurement < 0.8) { + AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts, + opticflow_result[idx_camera].vel_body.x, + opticflow_result[idx_camera].vel_body.y, + 0.0f, //opticflow_result.vel_body.z, + opticflow_result[idx_camera].noise_measurement, + opticflow_result[idx_camera].noise_measurement, + -1.0f //opticflow_result.noise_measurement // negative value disables filter updates with OF-based vertical velocity. + ); + } + opticflow_got_result[idx_camera] = false; } - opticflow_got_result = false; } pthread_mutex_unlock(&opticflow_mutex); } @@ -139,9 +161,10 @@ void opticflow_module_run(void) * This thread passes the images trough the optical flow * calculator * @param[in] *img The image_t structure of the captured image + * @param[in] camera_id The camera index id * @return *img The processed image structure */ -struct image_t *opticflow_module_calc(struct image_t *img) +struct image_t *opticflow_module_calc(struct image_t *img, uint8_t camera_id) { // Copy the state // TODO : put accelerometer values at pose of img timestamp @@ -150,12 +173,12 @@ struct image_t *opticflow_module_calc(struct image_t *img) img->eulers = pose.eulers; // Do the optical flow calculation - static struct opticflow_result_t temp_result; // static so that the number of corners is kept between frames - if(opticflow_calc_frame(&opticflow, img, &temp_result)){ + static struct opticflow_result_t temp_result[ACTIVE_CAMERAS]; // static so that the number of corners is kept between frames + if(opticflow_calc_frame(&opticflow[camera_id], img, &temp_result[camera_id])){ // Copy the result if finished pthread_mutex_lock(&opticflow_mutex); - opticflow_result = temp_result; - opticflow_got_result = true; + opticflow_result[camera_id] = temp_result[camera_id]; + opticflow_got_result[camera_id] = true; pthread_mutex_unlock(&opticflow_mutex); } return img; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index 8ed930a51a..745135623a 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -31,7 +31,7 @@ #include "opticflow/opticflow_calculator.h" // Needed for settings -extern struct opticflow_t opticflow; +extern struct opticflow_t opticflow[]; // Module functions extern void opticflow_module_init(void); diff --git a/sw/airborne/modules/computer_vision/qrcode/qr_code.c b/sw/airborne/modules/computer_vision/qrcode/qr_code.c index 261c5d121d..1951421aa2 100644 --- a/sw/airborne/modules/computer_vision/qrcode/qr_code.c +++ b/sw/airborne/modules/computer_vision/qrcode/qr_code.c @@ -42,7 +42,7 @@ 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, QRCODE_FPS); + cv_add_to_device(&QRCODE_CAMERA, qrscan, QRCODE_FPS, 0); } // Telemetry @@ -51,7 +51,7 @@ void qrcode_init(void) zbar_image_scanner_t *scanner = 0; -struct image_t *qrscan(struct image_t *img) +struct image_t *qrscan(struct image_t *img, uint8_t camera_id) { int i, j; diff --git a/sw/airborne/modules/computer_vision/qrcode/qr_code.h b/sw/airborne/modules/computer_vision/qrcode/qr_code.h index 38b43ce826..1f32545e91 100644 --- a/sw/airborne/modules/computer_vision/qrcode/qr_code.h +++ b/sw/airborne/modules/computer_vision/qrcode/qr_code.h @@ -35,7 +35,7 @@ extern bool drawRectangleAroundQRCode; extern void qrcode_init(void); -extern struct image_t *qrscan(struct image_t *img); +extern struct image_t *qrscan(struct image_t *img, uint8_t camera_id); #endif diff --git a/sw/airborne/modules/computer_vision/undistort_image.c b/sw/airborne/modules/computer_vision/undistort_image.c index a8c4b87936..e8f71450fd 100644 --- a/sw/airborne/modules/computer_vision/undistort_image.c +++ b/sw/airborne/modules/computer_vision/undistort_image.c @@ -43,7 +43,7 @@ static float K[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f}; // Function -static struct image_t *undistort_image_func(struct image_t *img) +static struct image_t *undistort_image_func(struct image_t *img, uint8_t camera_id) { // TODO: These commands could actually only be run when the parameters or image size are changed float normalized_step = (max_x_normalized - min_x_normalized) / img->w; @@ -118,5 +118,5 @@ void undistort_image_init(void) min_x_normalized = UNDISTORT_MIN_X_NORMALIZED; max_x_normalized = UNDISTORT_MAX_X_NORMALIZED; center_ratio = UNDISTORT_CENTER_RATIO; - listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS); + listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS), 0; } diff --git a/sw/airborne/modules/computer_vision/video_capture.c b/sw/airborne/modules/computer_vision/video_capture.c index 6d8cc1928f..d4cf2cb6ec 100644 --- a/sw/airborne/modules/computer_vision/video_capture.c +++ b/sw/airborne/modules/computer_vision/video_capture.c @@ -61,7 +61,7 @@ int video_capture_index = 0; static char save_dir[256]; // Forward function declarations -struct image_t *video_capture_func(struct image_t *img); +struct image_t *video_capture_func(struct image_t *img, uint8_t camera_id); void video_capture_save(struct image_t *img); @@ -80,11 +80,11 @@ void video_capture_init(void) // This prevents empty folders if nothing is actually recorded. // Add function to computer vision pipeline - cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS); + cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS, 0); } -struct image_t *video_capture_func(struct image_t *img) +struct image_t *video_capture_func(struct image_t *img, uint8_t camera_id) { // If take_shot bool is set, save the image if (video_capture_take_shot || video_capture_record_video) { diff --git a/sw/airborne/modules/computer_vision/video_usb_logger.c b/sw/airborne/modules/computer_vision/video_usb_logger.c index 5a105b7e7b..e7444fd3fe 100644 --- a/sw/airborne/modules/computer_vision/video_usb_logger.c +++ b/sw/airborne/modules/computer_vision/video_usb_logger.c @@ -111,7 +111,7 @@ static void save_shot_on_disk(struct image_t *img, struct image_t *img_jpeg) } -static struct image_t *log_image(struct image_t *img) +static struct image_t *log_image(struct image_t *img, uint8_t camera_id) { if (!created_jpeg) { @@ -148,7 +148,7 @@ void video_usb_logger_start(void) } // Subscribe to a camera - cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS); + cv_add_to_device(&VIDEO_USB_LOGGER_CAMERA, log_image, VIDEO_USB_LOGGER_FPS, 0); } /** 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 e27e6dae89..ce16e956e8 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -191,7 +191,7 @@ static struct image_t *viewvideo_function(struct UdpSocket *viewvideo_socket, st } #ifdef VIEWVIDEO_CAMERA -static struct image_t *viewvideo_function1(struct image_t *img) +static struct image_t *viewvideo_function1(struct image_t *img, uint8_t camera_id) { static uint16_t rtp_packet_nr = 0; static uint32_t rtp_frame_time = 0; @@ -202,7 +202,7 @@ static struct image_t *viewvideo_function1(struct image_t *img) #endif #ifdef VIEWVIDEO_CAMERA2 -static struct image_t *viewvideo_function2(struct image_t *img) +static struct image_t *viewvideo_function2(struct image_t *img, uint8_t camera_id) { static uint16_t rtp_packet_nr = 0; static uint32_t rtp_frame_time = 0; @@ -263,13 +263,13 @@ void viewvideo_init(void) #ifdef VIEWVIDEO_CAMERA cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function1, - VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS); + VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS, 0); fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA1 at %u FPS \n", VIEWVIDEO_FPS); #endif #ifdef VIEWVIDEO_CAMERA2 cv_add_to_device_async(&VIEWVIDEO_CAMERA2, viewvideo_function2, - VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS); + VIEWVIDEO_NICE_LEVEL, VIEWVIDEO_FPS, 1); fprintf(stderr, "[viewvideo] Added asynchronous video streamer listener for CAMERA2 at %u FPS \n", VIEWVIDEO_FPS); #endif } diff --git a/sw/airborne/modules/pano_unwrap/pano_unwrap.c b/sw/airborne/modules/pano_unwrap/pano_unwrap.c index 45f2b572b4..9e70ed039e 100644 --- a/sw/airborne/modules/pano_unwrap/pano_unwrap.c +++ b/sw/airborne/modules/pano_unwrap/pano_unwrap.c @@ -300,7 +300,7 @@ static void unwrap_LUT(struct image_t *img_raw, struct image_t *img) } } -static struct image_t *camera_cb(struct image_t *img) +static struct image_t *camera_cb(struct image_t *img, uint8_t camera_id) { set_output_image_size(); update_LUT(img); @@ -314,6 +314,6 @@ void pano_unwrap_init() { image_create(&pano_unwrapped_image, 0, 0, IMAGE_YUV422); set_output_image_size(); - cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS); + cv_add_to_device(&PANO_UNWRAP_CAMERA, camera_cb, PANO_UNWRAP_FPS, 0); } diff --git a/sw/airborne/modules/sensors/opticflow_pmw3901.c b/sw/airborne/modules/sensors/opticflow_pmw3901.c index 28ee54d5a5..fa31a525be 100644 --- a/sw/airborne/modules/sensors/opticflow_pmw3901.c +++ b/sw/airborne/modules/sensors/opticflow_pmw3901.c @@ -143,6 +143,7 @@ static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t #if SENSOR_SYNC_SEND_OPTICFLOW_PMW3901 float dummy_f = 0.f; uint16_t dummy_u16 = 0; + uint8_t dummy_u8 = 0; float fps = 1.f / dt; DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice, &fps, /* fps */ @@ -157,7 +158,8 @@ static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t &dummy_f, /* vel_z */ &dummy_f, /* div_size */ &dummy_f, /* surface_roughness */ - &dummy_f /* divergence */ + &dummy_f, /* divergence */ + &dummy_u8 /* camera_id */ ); #endif } diff --git a/sw/airborne/modules/wedgebug/wedgebug.c b/sw/airborne/modules/wedgebug/wedgebug.c index 9b473bb571..25725476b9 100644 --- a/sw/airborne/modules/wedgebug/wedgebug.c +++ b/sw/airborne/modules/wedgebug/wedgebug.c @@ -299,9 +299,9 @@ uint8_t getMedian(uint8_t *a, uint32_t n); //Core static struct image_t *copy_left_img_func(struct image_t - *img); // Function 1: Copies left image into a buffer (buf_left) + *img, uint8_t camera_id); // Function 1: Copies left image into a buffer (buf_left) static struct image_t *copy_right_img_func(struct image_t - *img); // Function 2: Copies left image into a buffer (buf_right) + *img, uint8_t camera_id); // Function 2: Copies left image into a buffer (buf_right) void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left, struct image_t *right); // Function 3: Copies gray pixel values of left and right UYVY images into merged YY image void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right); @@ -540,7 +540,7 @@ uint16_t getMedian16bit(uint16_t *a, uint32_t n) // Core: // Function 1 -static struct image_t *copy_left_img_func(struct image_t *img) +static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id) { image_copy(img, &img_left); //show_image_data(img); @@ -550,7 +550,7 @@ static struct image_t *copy_left_img_func(struct image_t *img) // Function 2 -static struct image_t *copy_right_img_func(struct image_t *img) +static struct image_t *copy_right_img_func(struct image_t *img, uint8_t camera_id) { image_copy(img, &img_right); //show_image_data(img); @@ -1655,8 +1655,8 @@ void wedgebug_init() // Adding callback functions - cv_add_to_device(&WEDGEBUG_CAMERA_LEFT, copy_left_img_func, WEDGEBUG_CAMERA_LEFT_FPS); - cv_add_to_device(&WEDGEBUG_CAMERA_RIGHT, copy_right_img_func, WEDGEBUG_CAMERA_RIGHT_FPS); + cv_add_to_device(&WEDGEBUG_CAMERA_LEFT, copy_left_img_func, WEDGEBUG_CAMERA_LEFT_FPS, 0); + cv_add_to_device(&WEDGEBUG_CAMERA_RIGHT, copy_right_img_func, WEDGEBUG_CAMERA_RIGHT_FPS, 1); //Initialization of constant rotation matrices and transition vectors for frame to frame transformations diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 4f986979fd..4854ed38de 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -355,8 +355,16 @@ #define FLOW_OPTICFLOW_ID 1 #endif +#ifndef FLOW_OPTICFLOW_CAM1_ID +#define FLOW_OPTICFLOW_CAM1_ID 1 +#endif + +#ifndef FLOW_OPTICFLOW_CAM2_ID +#define FLOW_OPTICFLOW_CAM2_ID 2 +#endif + #ifndef FLOW_OPTICFLOW_PMW3901_ID -#define FLOW_OPTICFLOW_PMW3901_ID 2 +#define FLOW_OPTICFLOW_PMW3901_ID 3 #endif /* @@ -374,12 +382,20 @@ #define VEL_OPTICFLOW_ID 3 #endif +#ifndef VEL_OPTICFLOW_CAM1_ID +#define VEL_OPTICFLOW_CAM1_ID 3 +#endif + +#ifndef VEL_OPTICFLOW_CAM2_ID +#define VEL_OPTICFLOW_CAM2_ID 4 +#endif + #ifndef VEL_STEREOCAM_ID -#define VEL_STEREOCAM_ID 4 +#define VEL_STEREOCAM_ID 5 #endif #ifndef VEL_OPTICFLOW_PMW3901_ID -#define VEL_OPTICFLOW_PMW3901_ID 5 +#define VEL_OPTICFLOW_PMW3901_ID 6 #endif /* diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index f8e9822347..46c4c210ac 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit f8e9822347ee2d306e30c4f675ac1897d28f3768 +Subproject commit 46c4c210ac5e3d1ee49d36a174a3555b29a37196