[modules] implemented simple edgeflow

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;
+}
+
+
 

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 <string.h>
+#include <stdlib.h>
 
 #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_ */

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
     );