diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml new file mode 100644 index 0000000000..da98cf1f98 --- /dev/null +++ b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml @@ -0,0 +1,224 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + +
+ + + + + + + + + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + +
+ +
+ + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + + +
+ +
+ + + + + +
+ diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index d8273e1fbc..b3b7e07c22 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -31,6 +31,7 @@ + @@ -58,6 +59,7 @@ + diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c index db5304603d..270b49be0e 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c +++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c @@ -121,10 +121,10 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], for (y = 0; y < image_height; y++) { sobel_sum = 0; - for (c = -1; c <= 1; c++) { - idx = interlace * (image_width * y + (x + c)); // 2 for interlace + for (c = -1; c <= 1; c+=2) { + idx = interlace * (image_width * y + (x + c)); - sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1]; + sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx]; } sobel_sum = abs(sobel_sum); if (sobel_sum > edge_threshold) { @@ -140,10 +140,10 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], for (x = 0; x < image_width; x++) { sobel_sum = 0; - for (c = -1; c <= 1; c++) { - idx = interlace * (image_width * (y + c) + x); // 2 for interlace + for (c = -1; c <= 1; c+=2) { + idx = interlace * (image_width * (y + c) + x); - sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx + 1]; + sobel_sum += Sobel[c + 1] * (int32_t)img_buf[idx]; } sobel_sum = abs(sobel_sum); if (sobel_sum > edge_threshold) { @@ -159,7 +159,7 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], * Calculate_displacement calculates the displacement between two histograms * @param[in] *edge_histogram The edge histogram from the current frame_step * @param[in] *edge_histogram_prev The edge histogram from the previous frame_step - * @param[in] *displacement array with pixel displacement of the sequential edge histograms + * @param[out] *displacement array with pixel displacement of the sequential edge histograms * @param[in] size Indicating the size of the displacement array * @param[in] window Indicating the search window size * @param[in] disp_range Indicating the maximum disparity range for the block matching @@ -196,10 +196,8 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra if (border[0] >= border[1] || abs(der_shift) >= 10) { SHIFT_TOO_FAR = 1; } - { // TODO: replace with arm offset subtract for (x = border[0]; x < border[1]; x++) { - displacement[x] = 0; if (!SHIFT_TOO_FAR) { for (c = -D; c <= D; c++) { SAD_temp[c + D] = 0; @@ -211,8 +209,6 @@ void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogra } else { } } - } - } /** @@ -271,7 +267,7 @@ void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_ // compute fixed sums int32_t xend = size_int - border_int - 1; sumX = xend * (xend + 1) / 2 - border_int * (border_int + 1) / 2 + border_int; - sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6; + sumX2 = xend * (xend + 1) * (2 * xend + 1) / 6 - border_int * (border_int + 1) * (2 * border_int + 1) / 6 + border_int*border_int; xMean = (size_int - 1) / 2; count = size_int - 2 * border_int; diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c index 4f91923975..1c8b01919e 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.c +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -353,6 +353,7 @@ void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t * This outputs a subpixel window image in grayscale * Currently only works with Grayscale images as input but could be upgraded to * also support YUV422 images. + * You can and should only ask a subpixel window of a center point that is w/2 pixels away from the edges * @param[in] *input Input image (grayscale only) * @param[out] *output Window output (width and height is used to calculate the window size) * @param[in] *center Center point in subpixel coordinates @@ -369,18 +370,18 @@ void image_subpixel_window(struct image_t *input, struct image_t *output, struct // Calculate the window size uint16_t half_window = output->w / 2; - uint32_t subpixel_w = input->w * subpixel_factor; - uint32_t subpixel_h = input->h * subpixel_factor; + uint32_t subpixel_w = (input->w -2) * subpixel_factor; + uint32_t subpixel_h = (input->h -2) * subpixel_factor; // Go through the whole window size in normal coordinates for (uint16_t i = 0; i < output->w; i++) { for (uint16_t j = 0; j < output->h; j++) { // Calculate the subpixel coordinate - uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor ; - uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor ; + uint32_t x = center->x + border_size * subpixel_factor + (i - half_window) * subpixel_factor; + uint32_t y = center->y + border_size * subpixel_factor + (j - half_window) * subpixel_factor; - BoundUpper(x, subpixel_w - 1); - BoundUpper(y, subpixel_h - 1); + BoundUpper(x, subpixel_w); + BoundUpper(y, subpixel_h); // Calculate the original pixel coordinate uint16_t orig_x = x / subpixel_factor; diff --git a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c index 7c03cc7150..25659cb0a1 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c +++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c @@ -331,8 +331,9 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img vectors[new_p].pos.y + vectors[new_p].flow_y }; // If the pixel is outside ROI, do not track it - if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) < half_window_size - || new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) < half_window_size) { + if (new_point.x / subpixel_factor < half_window_size || (old_img->w - new_point.x / subpixel_factor) <= half_window_size + || new_point.y / subpixel_factor < half_window_size || (old_img->h - new_point.y / subpixel_factor) <= half_window_size + || new_point.x / subpixel_factor > old_img->w || new_point.y / subpixel_factor > old_img->h) { tracked = FALSE; break; } diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 67c3a6b788..34ef440c5d 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -95,13 +95,18 @@ PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE) #ifndef OPTICFLOW_SEARCH_DISTANCE #define OPTICFLOW_SEARCH_DISTANCE 20 #endif -PRINT_CONFIG_VAR(OPTICFLOW_MAX_SEARCH_DISTANCE) +PRINT_CONFIG_VAR(OPTICFLOW_SEARCH_DISTANCE) #ifndef OPTICFLOW_SUBPIXEL_FACTOR #define OPTICFLOW_SUBPIXEL_FACTOR 10 #endif PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR) +#ifndef OPTICFLOW_RESOLUTION_FACTOR +#define OPTICFLOW_RESOLUTION_FACTOR 100 +#endif +PRINT_CONFIG_VAR(OPTICFLOW_RESOLUTION_FACTOR) + #ifndef OPTICFLOW_MAX_ITERATIONS #define OPTICFLOW_MAX_ITERATIONS 10 #endif @@ -194,23 +199,9 @@ static int cmp_flow(const void *a, const void *b); /** * Initialize the opticflow calculator * @param[out] *opticflow The new optical flow calculator - * @param[in] *w The image width - * @param[in] *h The image height */ -void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h) +void opticflow_calc_init(struct opticflow_t *opticflow) { - - init_median_filter(&vel_x_filt); - init_median_filter(&vel_y_filt); - - /* Create the image buffers */ - image_create(&opticflow->img_gray, w, h, IMAGE_GRAYSCALE); - image_create(&opticflow->prev_img_gray, w, h, IMAGE_GRAYSCALE); - - /* Set the previous values */ - opticflow->got_first_img = false; - FLOAT_RATES_ZERO(opticflow->prev_rates); - /* Set the default values */ opticflow->method = OPTICFLOW_METHOD; //0 = LK_fast9, 1 = Edgeflow opticflow->window_size = OPTICFLOW_WINDOW_SIZE; @@ -221,6 +212,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h) opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS; opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR; + opticflow->resolution_factor = OPTICFLOW_RESOLUTION_FACTOR; opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS; opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC; opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL; @@ -234,7 +226,6 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h) opticflow->fast9_padding = OPTICFLOW_FAST9_PADDING; opticflow->fast9_rsize = 512; opticflow->fast9_ret_corners = malloc(sizeof(struct point_t) * opticflow->fast9_rsize); - } /** * Run the optical flow with fast9 and lukaskanade on a new image frame @@ -247,7 +238,17 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta struct opticflow_result_t *result) { if (opticflow->just_switched_method) { - opticflow_calc_init(opticflow, img->w, img->h); + // Create the image buffers + image_create(&opticflow->img_gray, img->w, img->h, IMAGE_GRAYSCALE); + image_create(&opticflow->prev_img_gray, img->w, img->h, IMAGE_GRAYSCALE); + + // Set the previous values + opticflow->got_first_img = false; + FLOAT_RATES_ZERO(opticflow->prev_rates); + + // Init median filters with zeros + init_median_filter(&vel_x_filt); + init_median_filter(&vel_y_filt); } // variables for size_divergence: @@ -471,7 +472,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t * window_size = MAX_WINDOW_SIZE; } - uint16_t RES = opticflow->subpixel_factor; + uint16_t RES = opticflow->resolution_factor; //......................Calculating EdgeFlow..................... // @@ -526,7 +527,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t * /* Save Resulting flow in results * Warning: The flow detected here is different in sign - * and size, therefore this will be multiplied with + * and size, therefore this will be divided with * the same subpixel factor and -1 to make it on par with * the LK algorithm of t opticalflow_calculator.c * */ @@ -536,12 +537,15 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t * result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0]; result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1]; + result->flow_x = (int16_t)edgeflow.flow_x / RES; + result->flow_y = (int16_t)edgeflow.flow_y / RES; + //Fill up the results optic flow to be on par with LK_fast9 result->flow_der_x = result->flow_x; result->flow_der_y = result->flow_y; result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w); result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w); - result->divergence = (float)edgeflow.flow_x / RES; + result->divergence = (float)edgeflow.div_x / RES; result->div_size = 0.0f; result->noise_measurement = 0.0f; result->surface_roughness = 0.0f; @@ -584,6 +588,10 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t * #endif // Increment and wrap current time frame current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; + + // Free malloc'd variables + free(displacement.x); + free(displacement.y); } diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index 10a6805670..1fa444c642 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -57,6 +57,7 @@ struct opticflow_t { float derotation_correction_factor_y; ///< Correction factor for derotation in Y axis, determined from a fit from the gyros and flow rotation. (wrong FOV, camera not in center) uint16_t subpixel_factor; ///< The amount of subpixels per pixel + uint16_t resolution_factor; ///< The resolution in EdgeFlow to determine the Divergence uint8_t max_iterations; ///< The maximum amount of iterations the Lucas Kanade algorithm should do uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop uint8_t pyramid_level; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used) @@ -72,7 +73,7 @@ struct opticflow_t { }; -void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h); +void opticflow_calc_init(struct opticflow_t *opticflow); void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, 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 dcfa16d6cd..4bb6b6c90b 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -124,6 +124,7 @@ void opticflow_module_init(void) // Initialize the opticflow calculation opticflow_got_result = false; + opticflow_calc_init(&opticflow); cv_add_to_device(&OPTICFLOW_CAMERA, opticflow_module_calc);