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 0642c42e3c..664307fea9 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c +++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.c @@ -13,57 +13,6 @@ void test_function(struct image_t *img,struct image_t *img_gray) } -void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, - struct opticflow_result_t *result) -{ - - static struct edge_hist_t edge_hist[MAX_HORIZON]; - static uint8_t current_frame_nr = 1; - - image_to_grayscale(img, &opticflow->img_gray); - // Copy to previous image if not set - if (!opticflow->got_first_img) { - image_copy(&opticflow->img_gray, &opticflow->prev_img_gray); - opticflow->got_first_img = TRUE; - } - int32_t *edge_hist_x = edge_hist[current_frame_nr].horizontal; - int32_t *edge_hist_y = edge_hist[current_frame_nr].vertical; - - calculate_edge_histogram(img, edge_hist_x, 'x',0); - calculate_edge_histogram(img, edge_hist_y, 'y',0); - - - uint8_t previous_frame_x = (current_frame_nr - 5 + MAX_HORIZON) % - MAX_HORIZON; // wrap index - uint8_t previous_frame_y = (current_frame_nr - 5 + MAX_HORIZON) % - MAX_HORIZON; // wrap index - - uint16_t i; - struct point_t point1; - struct point_t point2; - struct point_t point1_prev; - struct point_t point2_prev; - - - for(i = 120; i<240;i++) - { - point1.y = -(uint16_t)edge_hist_x[i]/100 + img->h/3; - point1.x = i; - point2.y = -(uint16_t)edge_hist_x[i+1]/100 + img->h/3; - point2.x = i+1; - - point1_prev.y = -(uint16_t)edge_hist[previous_frame_x].horizontal[i]/100 + img->h*2/3; - point1_prev.x = i; - point2_prev.y = -(uint16_t)edge_hist[previous_frame_x].horizontal[i+1]/100 + img->h*2/3; - point2_prev.x = i+1; - - image_draw_line(img, &point1,&point2); - image_draw_line(img, &point1_prev,&point2_prev); - - } - - current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; -} @@ -138,4 +87,209 @@ void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], while (1); // hang to show user something isn't right } +// Calculate_displacement calculates the displacement between two histograms +// D should be half the search disparity range +// W is local search window +void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement, + uint16_t size, + uint8_t window, uint8_t disp_range, int32_t der_shift) +{ + int32_t c = 0, r = 0; + uint32_t x = 0; + uint32_t SAD_temp[2 * DISP_RANGE_MAX + 1]; // size must be at least 2*D + 1 + + int32_t W = window; + int32_t D = disp_range; + + + uint8_t SHIFT_TOO_FAR = 0; + memset(displacement, 0, size); + + int32_t border[2]; + + if (der_shift < 0) + { + border[0] = W + D + der_shift; + border[1] = size - W - D; + } + else if(der_shift > 0) + { + border[0] = W + D; + border[1] = size - W - D - der_shift; + } + else + { + border[0] = W + D; + border[1] = size - W - D; + } + + 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; + for (c = -D; c <= D; c++) { + SAD_temp[c + D] = 0; + for (r = -W; r <= W; r++) { + SAD_temp[c + D] += abs(edge_histogram[x + r] - edge_histogram_prev[x + r + c + der_shift]); + } + } + if(!SHIFT_TOO_FAR) + displacement[x] = (int32_t)getMinimum(SAD_temp, 2 * D + 1) - D; + else + displacement[x]=0; + } + } + +} + +// Small supporting functions + +uint32_t getMinimum(uint32_t *a, uint32_t n) +{ + uint32_t i; + uint32_t min_ind = 0; + uint32_t min_err = a[min_ind]; + uint32_t min_err_tot = 0; + for (i = 1; i < n; i++) { + if (a[i] <= min_err) { + min_ind = i; + min_err = a[i]; + min_err_tot += min_err; + } + } + //*min_error = min_err_tot; + return min_ind; +} + +// Line_fit fits a line using least squares to the histogram disparity map +void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border, + uint16_t RES) +{ + int32_t x; + + int32_t count = 0; + int32_t sumY = 0; + int32_t sumX = 0; + int32_t sumX2 = 0; + int32_t sumXY = 0; + int32_t xMean = 0; + int32_t yMean = 0; + int32_t divergence_int = 0; + int32_t border_int = (int32_t)border; + int32_t size_int = (int32_t)size; + uint32_t total_error = 0; + + *divergence = 0; + *flow = 0; + + // 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; + xMean = (size_int - 1) / 2; + count = size_int - 2 * border_int; + + for (x = border_int; x < size - border_int; x++) { + sumY += displacement[x]; + sumXY += x * displacement[x]; + } + + yMean = RES * sumY / count; + + divergence_int = (RES * sumXY - sumX * yMean) / (sumX2 - sumX * xMean); // compute slope of line ax + b + *divergence = divergence_int; + *flow = yMean - *divergence * xMean; // compute b (or y) intercept of line ax + b + + for (x = border_int; x < size - border_int; x++) { + total_error += abs(RES * displacement[x] - divergence_int * x + yMean); + } + + //return total_error / size; +} + +void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, + struct opticflow_result_t *result) +{ + + static struct edge_hist_t edge_hist[MAX_HORIZON]; + static uint8_t current_frame_nr = 1; + struct edge_flow_t edgeflow; + + int32_t *edge_hist_x = edge_hist[current_frame_nr].horizontal; + int32_t *edge_hist_y = edge_hist[current_frame_nr].vertical; + + calculate_edge_histogram(img, edge_hist_x, 'x',0); + calculate_edge_histogram(img, edge_hist_y, 'y',0); + + uint8_t previous_frame_x = (current_frame_nr - 1 + MAX_HORIZON) % + MAX_HORIZON; // wrap index + uint8_t previous_frame_y = (current_frame_nr - 1 + MAX_HORIZON) % + MAX_HORIZON; // wrap index + + int32_t *prev_edge_histogram_x = edge_hist[previous_frame_x].horizontal; + int32_t *prev_edge_histogram_y = edge_hist[previous_frame_y].vertical; + + struct edgeflow_displacement_t displacement; + uint8_t disp_range = DISP_RANGE_MAX; + calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x, + displacement.horizontal, img->w, + opticflow->window_size, disp_range, 0); + calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y, + displacement.vertical, img->h, + opticflow->window_size, disp_range, 0); + + uint16_t RES = 100; + line_fit(displacement.horizontal, &edgeflow.horizontal_div, + &edgeflow.horizontal_flow, img->w, + opticflow->window_size + disp_range, RES); + line_fit(displacement.vertical, &edgeflow.vertical_div, + &edgeflow.vertical_flow, img->h, + opticflow->window_size + disp_range, RES); + + uint16_t i; + + result->flow_x = (int16_t)edgeflow.horizontal_flow/RES; + result->flow_y = (int16_t)edgeflow.vertical_flow/RES; + + + struct point_t point1; + struct point_t point2; + struct point_t point1_prev; + struct point_t point2_prev; + struct point_t point1_extra; + struct point_t point2_extra; + + for(i = 120; i<240;i++) + { + point1.y = -(uint16_t)edge_hist_x[i]/100 + img->h/3; + point1.x = i; + point2.y = -(uint16_t)edge_hist_x[i+1]/100 + img->h/3; + point2.x = i+1; + + + point1_prev.y = -(uint16_t)displacement.horizontal[i]*5 + img->h*2/3; + point1_prev.x = i; + point2_prev.y = -(uint16_t)displacement.horizontal[i+1]*5 + img->h*2/3; + point2_prev.x = i+1; + + image_draw_line(img, &point1,&point2); + image_draw_line(img, &point1_prev,&point2_prev); + + } + + point1_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * -180 )/ 100+ img->h/2; + point1_extra.x = 0; + point2_extra.y = (edgeflow.horizontal_flow+edgeflow.horizontal_div * 180 )/ 100 + img->h/2; + point2_extra.x = 360; + image_draw_line(img, &point1_extra,&point2_extra); + + + current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON; +} + + diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h index 2388a61186..3716a51fe5 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h +++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h @@ -14,7 +14,8 @@ #include "lib/vision/image.h" #include "lib/v4l/v4l2.h" #include "opticflow/opticflow_calculator.h" - +#include +#include #define MAX_HORIZON 10 #define IMAGE_HEIGHT 240 @@ -28,10 +29,29 @@ struct edge_hist_t { int16_t roll; int16_t pitch; }; + +struct edgeflow_displacement_t { + int32_t horizontal[IMAGE_WIDTH]; + int32_t vertical[IMAGE_HEIGHT]; +}; + +struct edge_flow_t { + int32_t horizontal_flow; + int32_t horizontal_div; + int32_t vertical_flow; + int32_t vertical_div; +}; + + +void line_fit(int32_t *displacement, int32_t *divergence, int32_t *flow, uint32_t size, uint32_t border, + uint16_t RES); void test_function(struct image_t *image,struct image_t *image_gray); void edgeflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result); void calculate_edge_histogram(struct image_t *img, int32_t edge_histogram[], char direction, uint16_t edge_threshold); +void calculate_edge_displacement(int32_t *edge_histogram, int32_t *edge_histogram_prev, int32_t *displacement, + uint16_t size, uint8_t window, uint8_t disp_range, int32_t der_shift); +uint32_t getMinimum(uint32_t *a, uint32_t n); #endif /* EDGE_FLOW_H_ */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 6b2394fd97..a601d5a9a0 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -269,12 +269,12 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) pthread_mutex_unlock(&opticflow_mutex); #if OPTICFLOW_DEBUG - jpeg_encode_image(&img, &img_jpeg, 70, FALSE); + jpeg_encode_image(&img, &img_jpeg, 50, FALSE); rtp_frame_send( &video_sock, // UDP device &img_jpeg, 0, // Format 422 - 70, // Jpeg-Quality + 50, // Jpeg-Quality 0, // DRI Header 0 // 90kHz time increment );