diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml index db1ddb389b..503f0c6b91 100644 --- a/conf/airframes/examples/bebop2_opticflow.xml +++ b/conf/airframes/examples/bebop2_opticflow.xml @@ -29,10 +29,6 @@ - - - - diff --git a/conf/airframes/tudelft/bebop2_opticflow.xml b/conf/airframes/tudelft/bebop2_opticflow.xml index 923aa29e86..3fe50808e1 100644 --- a/conf/airframes/tudelft/bebop2_opticflow.xml +++ b/conf/airframes/tudelft/bebop2_opticflow.xml @@ -39,10 +39,6 @@ - - - - diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml index 01f44939ce..f50a8fa130 100644 --- a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml +++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml @@ -56,11 +56,7 @@ - - - - - + diff --git a/conf/airframes/tudelft/bebop_opticflow.xml b/conf/airframes/tudelft/bebop_opticflow.xml index 0ea59192dc..fe1ebc4368 100644 --- a/conf/airframes/tudelft/bebop_opticflow.xml +++ b/conf/airframes/tudelft/bebop_opticflow.xml @@ -37,10 +37,6 @@ - - - - diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 9064139c89..1dbfb00540 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -17,10 +17,6 @@ - - - - @@ -136,5 +132,7 @@ + + diff --git a/sw/airborne/boards/bebop/mt9f002.c b/sw/airborne/boards/bebop/mt9f002.c index 38b861ab57..cd09f6671a 100644 --- a/sw/airborne/boards/bebop/mt9f002.c +++ b/sw/airborne/boards/bebop/mt9f002.c @@ -65,7 +65,14 @@ struct video_config_t front_camera = { .buf_cnt = 5, .filters = VIDEO_FILTER_ISP, .cv_listener = NULL, - .fps = MT9F002_TARGET_FPS + .fps = MT9F002_TARGET_FPS, + .camera_intrinsics = { + .focal_x = MT9F002_FOCAL_X, + .focal_y = MT9F002_FOCAL_Y, + .center_x = MT9F002_CENTER_X, + .center_y = MT9F002_CENTER_Y, + .Dhane_k = MT9F002_DHANE_K + } }; /** diff --git a/sw/airborne/boards/bebop/mt9f002.h b/sw/airborne/boards/bebop/mt9f002.h index 64fbad1cd7..4eb767a1ae 100644 --- a/sw/airborne/boards/bebop/mt9f002.h +++ b/sw/airborne/boards/bebop/mt9f002.h @@ -103,6 +103,23 @@ #define MT9F002_Y_ODD_INC_VAL 1 #endif +// parameters for undistortion +#ifndef MT9F002_FOCAL_X +#define MT9F002_FOCAL_X 311.59304538f +#endif +#ifndef MT9F002_FOCAL_Y +#define MT9F002_FOCAL_Y 313.01338397f +#endif +#ifndef MT9F002_CENTER_X +#define MT9F002_CENTER_X 158.37457814f +#endif +#ifndef MT9F002_CENTER_Y +#define MT9F002_CENTER_Y 326.49375925f +#endif +#ifndef MT9F002_DHANE_K +#define MT9F002_DHANE_K 1.25f +#endif + /* Interface types for the MT9F002 connection */ enum mt9f002_interface { MT9F002_MIPI, ///< MIPI type connection diff --git a/sw/airborne/boards/bebop/mt9v117.c b/sw/airborne/boards/bebop/mt9v117.c index 833945a6e4..450d17ff72 100644 --- a/sw/airborne/boards/bebop/mt9v117.c +++ b/sw/airborne/boards/bebop/mt9v117.c @@ -60,7 +60,14 @@ struct video_config_t bottom_camera = { .buf_cnt = 5, .filters = 0, .cv_listener = NULL, - .fps = MT9V117_TARGET_FPS + .fps = MT9V117_TARGET_FPS, + .camera_intrinsics = { + .focal_x = MT9V117_FOCAL_X, + .focal_y = MT9V117_FOCAL_Y, + .center_x = MT9V117_CENTER_X, + .center_y = MT9V117_CENTER_Y, + .Dhane_k = MT9V117_DHANE_K + } }; diff --git a/sw/airborne/boards/bebop/mt9v117.h b/sw/airborne/boards/bebop/mt9v117.h index fe2a70a86a..b3fdecd23b 100644 --- a/sw/airborne/boards/bebop/mt9v117.h +++ b/sw/airborne/boards/bebop/mt9v117.h @@ -34,6 +34,24 @@ #define MT9V117_TARGET_FPS 0 #endif +// parameters for undistortion +#ifndef MT9V117_FOCAL_X +#define MT9V117_FOCAL_X 347.22f +#endif +#ifndef MT9V117_FOCAL_Y +#define MT9V117_FOCAL_Y 347.22f +#endif +#ifndef MT9V117_CENTER_X +#define MT9V117_CENTER_X 120.0f +#endif +#ifndef MT9V117_CENTER_Y +#define MT9V117_CENTER_Y 120.0f +#endif +#ifndef MT9V117_DHANE_K +#define MT9V117_DHANE_K 1.0f +#endif + + struct mt9v117_t { struct i2c_periph *i2c_periph; ///< I2C peripheral used to communicate over struct i2c_transaction i2c_trans; ///< I2C transaction for comminication with CMOS chip diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c index c52800e917..8214ba3e73 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.c +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -601,17 +601,26 @@ void image_show_points_color(struct image_t *img, struct point_t *points, uint16 } } +void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor) +{ + static uint8_t color[4] = {255, 255, 255, 255}; + static uint8_t bad_color[4] = {0, 0, 0, 0}; + image_show_flow_color(img, vectors, points_cnt, subpixel_factor, color, bad_color); +} + /** * Shows the flow from a specific point to a new point * This works on YUV422 and Grayscale images * @param[in,out] *img The image to show the flow on * @param[in] *vectors The flow vectors to show * @param[in] *points_cnt The amount of points and vectors to show + * @param[in] subpixel_factor + * @param[in] color: color for good vectors + * @param[in] bad_color: color for bad vectors */ -void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor) +void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor, + const uint8_t *color, const uint8_t *bad_color) { - static uint8_t color[4] = {255, 255, 255, 255}; - static uint8_t bad_color[4] = {0, 0, 0, 0}; static int size_crosshair = 5; // Go through all the points diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h index a8ecd0b718..402c186002 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.h +++ b/sw/airborne/modules/computer_vision/lib/vision/image.h @@ -102,6 +102,8 @@ uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct i int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult); void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt); void image_show_points_color(struct image_t *img, struct point_t *points, uint16_t points_cnt, uint8_t *color); +void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor, + const uint8_t *color, const uint8_t *bad_color); void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor); void image_draw_crosshair(struct image_t *img, struct point_t *loc, uint8_t *color, int size_crosshair); void image_draw_rectangle(struct image_t *img, int x_min, int x_max, int y_min, int y_max, uint8_t *color); diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 31544a95b3..2c71926410 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -41,10 +41,15 @@ #include "lib/vision/fast_rosten.h" #include "lib/vision/act_fast.h" #include "lib/vision/edge_flow.h" +#include "lib/vision/undistortion.h" #include "size_divergence.h" #include "linear_flow_fit.h" #include "modules/sonar/agl_dist.h" + +// to get the definition of front_camera / bottom_camera +#include BOARD_CONFIG + // whether to show the flow and corners: #define OPTICFLOW_SHOW_CORNERS 0 @@ -66,29 +71,6 @@ uint16_t n_agents = 25; #endif PRINT_CONFIG_VAR(OPTICFLOW_CORNER_METHOD) -// Camera parameters (defaults are from an ARDrone 2) -#ifndef OPTICFLOW_FOV_W -#define OPTICFLOW_FOV_W 0.89360857702 -#endif -PRINT_CONFIG_VAR(OPTICFLOW_FOV_W) - -#ifndef OPTICFLOW_FOV_H -#define OPTICFLOW_FOV_H 0.67020643276 -#endif -PRINT_CONFIG_VAR(OPTICFLOW_FOV_H) - -#ifndef OPTICFLOW_FX -// This can be estimated by total possible image width / total Field of view -#define OPTICFLOW_FX 343.1211 -#endif -PRINT_CONFIG_VAR(OPTICFLOW_FX) - -#ifndef OPTICFLOW_FY -// This can be estimated by total possible image height / total Field of view -#define OPTICFLOW_FY 348.5053 -#endif -PRINT_CONFIG_VAR(OPTICFLOW_FY) - /* Set the default values */ #ifndef OPTICFLOW_MAX_TRACK_CORNERS #define OPTICFLOW_MAX_TRACK_CORNERS 25 @@ -257,6 +239,8 @@ static int cmp_array(const void *a, const void *b); static void manage_flow_features(struct image_t *img, struct opticflow_t *opticflow, struct opticflow_result_t *result); +static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t n_points, float phi_diff, + float theta_diff, float psi_diff, struct opticflow_t *opticflow); /** * Initialize the opticflow calculator * @param[out] *opticflow The new optical flow calculator @@ -475,7 +459,9 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, } if (opticflow->show_flow) { - image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor); + uint8_t color[4] = {0, 0, 0, 0}; + uint8_t bad_color[4] = {0, 0, 0, 0}; + image_show_flow_color(img, vectors, result->tracked_cnt, opticflow->subpixel_factor, color, bad_color); } static int n_samples = 100; @@ -528,38 +514,80 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, // TODO scale flow to rad/s here + // *************** // Flow Derotation + // *************** + float diff_flow_x = 0.f; float diff_flow_y = 0.f; if (opticflow->derotation && result->tracked_cnt > 5) { - diff_flow_x = (opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi) * OPTICFLOW_FX; - diff_flow_y = (opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta) * OPTICFLOW_FY; - /*diff_flow_x = (cam_state->rates.p) / result->fps * img->w / - OPTICFLOW_FOV_W;// * img->w / OPTICFLOW_FOV_W; - diff_flow_y = (cam_state->rates.q) / result->fps * img->h / - OPTICFLOW_FOV_H;// * img->h / OPTICFLOW_FOV_H;*/ - } - float rotation_threshold = M_PI / 180.0f; - if (fabs(opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi) > rotation_threshold - || fabs(opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta) > rotation_threshold) { - result->flow_der_x = 0.0f; - result->flow_der_y = 0.0f; - } else { - 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 * - opticflow->derotation_correction_factor_y; + float rotation_threshold = M_PI / 180.0f; + if (fabs(opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi) > rotation_threshold + || fabs(opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta) > rotation_threshold) { + + // do not apply the derotation if the rotation rates are too high: + result->flow_der_x = 0.0f; + result->flow_der_y = 0.0f; + + } else { + + // determine the roll, pitch, yaw differencces between the images. + float phi_diff = opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi; + 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, "/dev/video0") == 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; + 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 * + opticflow->derotation_correction_factor_y; + } else { + + // frontal cam, predict individual flow vectors: + struct flow_t *predicted_flow_vectors = predict_flow_vectors(vectors, result->tracked_cnt, phi_diff, theta_diff, + psi_diff, opticflow); + if (opticflow->show_flow) { + uint8_t color[4] = {255, 255, 255, 255}; + uint8_t bad_color[4] = {255, 255, 255, 255}; + image_show_flow_color(img, predicted_flow_vectors, result->tracked_cnt, opticflow->subpixel_factor, color, bad_color); + } + + for (int i = 0; i < result->tracked_cnt; i++) { + // subtract the flow: + vectors[i].flow_x -= predicted_flow_vectors[i].flow_x; + vectors[i].flow_y -= predicted_flow_vectors[i].flow_y; + } + + // vectors have to be re-sorted after derotation: + qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow); + + if (result->tracked_cnt % 2) { + // Take the median point + result->flow_der_x = vectors[result->tracked_cnt / 2].flow_x; + result->flow_der_y = vectors[result->tracked_cnt / 2].flow_y; + } else { + // Take the average of the 2 median points + result->flow_der_x = (vectors[result->tracked_cnt / 2 - 1].flow_x + vectors[result->tracked_cnt / 2].flow_x) / 2.f; + result->flow_der_y = (vectors[result->tracked_cnt / 2 - 1].flow_y + vectors[result->tracked_cnt / 2].flow_y) / 2.f; + } + } + } } // 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: 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_FX); + (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_FY); + (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 @@ -594,6 +622,75 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, return true; } +/* + * Predict flow vectors by means of the rotation rates: + */ +static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t n_points, float phi_diff, + float theta_diff, float psi_diff, struct opticflow_t *opticflow) +{ + + // 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, + 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 A, B, C; // as in Longuet-Higgins + + if (strcmp(OPTICFLOW_CAMERA.dev_name, "/dev/video1") == 0) { + // specific for the x,y swapped Bebop 2 images: + A = -psi_diff; + B = theta_diff; + C = phi_diff; + } else { + A = theta_diff; + B = phi_diff; + C = psi_diff; + } + + float x_n, y_n; + float x_n_new, y_n_new, x_pix_new, y_pix_new; + float predicted_flow_x, predicted_flow_y; + for (uint16_t i = 0; i < n_points; i++) { + // the from-coordinate is always the same: + predicted_flow_vectors[i].pos.x = flow_vectors[i].pos.x; + predicted_flow_vectors[i].pos.y = flow_vectors[i].pos.y; + + bool success = distorted_pixels_to_normalized_coords((float)flow_vectors[i].pos.x / opticflow->subpixel_factor, + (float)flow_vectors[i].pos.y / opticflow->subpixel_factor, &x_n, &y_n, k, K); + if (success) { + // predict flow as in a linear pinhole camera model: + predicted_flow_x = A * x_n * y_n - B * x_n * x_n - B + C * y_n; + predicted_flow_y = -C * x_n + A + A * y_n * y_n - B * x_n * y_n; + + x_n_new = x_n + predicted_flow_x; + y_n_new = y_n + predicted_flow_y; + + success = normalized_coords_to_distorted_pixels(x_n_new, y_n_new, &x_pix_new, &y_pix_new, k, K); + + if (success) { + predicted_flow_vectors[i].flow_x = (int16_t)(x_pix_new * opticflow->subpixel_factor - (float)flow_vectors[i].pos.x); + predicted_flow_vectors[i].flow_y = (int16_t)(y_pix_new * opticflow->subpixel_factor - (float)flow_vectors[i].pos.y); + predicted_flow_vectors[i].error = 0; + } else { + predicted_flow_vectors[i].flow_x = 0; + predicted_flow_vectors[i].flow_y = 0; + predicted_flow_vectors[i].error = LARGE_FLOW_ERROR; + } + } else { + predicted_flow_vectors[i].flow_x = 0; + predicted_flow_vectors[i].flow_y = 0; + predicted_flow_vectors[i].error = LARGE_FLOW_ERROR; + } + } + return predicted_flow_vectors; +} + + /* manage_flow_features - Update list of corners to be tracked by LK * Remembers previous points and tries to find new points in less dense * areas of the image first. @@ -782,9 +879,9 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, if (opticflow->derotation) { der_shift_x = (int16_t)((edge_hist[current_frame_nr].eulers.phi - edge_hist[previous_frame_nr[0]].eulers.phi) * - OPTICFLOW_FX * 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_FY * 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 @@ -849,8 +946,10 @@ 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_FX / RES; - result->vel_cam.y = edgeflow.flow_y * fps_y * agl_dist_value_filtered * OPTICFLOW_FY / RES; + 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 / + RES; result->vel_cam.z = result->divergence * fps_x * agl_dist_value_filtered; //Apply a median filter to the velocity if wanted diff --git a/sw/airborne/peripherals/video_device.h b/sw/airborne/peripherals/video_device.h index 25308117f0..ad927f8943 100644 --- a/sw/airborne/peripherals/video_device.h +++ b/sw/airborne/peripherals/video_device.h @@ -40,6 +40,17 @@ struct video_thread_t { struct v4l2_device *dev; ///< The V4L2 device that is used for the video stream }; +// camera intrinsics: capture lens properties that determine how world points are projected to image points +// These properties can be obtained with a camera calibration (as done in openCV) +// The Dhane (un)distortion parameter is normally tuned by hand, for example with the undistortion module in Paparazzi +struct camera_intrinsics_t { + float focal_x; ///< focal length in the x-direction in pixels + float focal_y; ///< focal length in the y-direction in pixels + float center_x; ///< center image coordinate in the x-direction + float center_y; ///< center image coordinate in the y-direction + float Dhane_k; //< (un)distortion parameter for a fish-eye lens +}; + /** V4L2 device settings */ struct video_config_t { struct img_size_t output_size; ///< Output image size @@ -54,8 +65,9 @@ struct video_config_t { struct video_thread_t thread; ///< Information about the thread this camera is running on struct video_listener *cv_listener; ///< The first computer vision listener in the linked list for this video device int fps; ///< Target FPS + struct camera_intrinsics_t + camera_intrinsics; ///< Intrinsics of the camera; camera calibration parameters and distortion parameter(s) }; extern struct video_config_t dummy_camera; - #endif