working copy

algorithm not working and has checked

1 files changed, 104 insertions, 12 deletions

diff --git a/color_pipe.c b/color_pipe.c
index 0b77c61..648e259 100644
--- a/color_pipe.c
+++ b/color_pipe.c
@@ -2,9 +2,7 @@
 * @file			main.cpp
 * @brief		Color Image Processing Pipeline with O-3000 USB camera
 * @author		Patrick Roth - roth@stettbacher.ch
-* @version		1.0
-* @date			2015-02-10
-* @copyright	2012-2016 Stettbacher Signal Processing AG
+* @copyright	Stettbacher Signal Processing AG
 * 
 * @remarks
 *
@@ -365,12 +363,12 @@ static void set_default_value(struct color_pipe_t *pipe) {
 	pipe->debayer_data.alg_new = pipe->debayer_data.alg;
 	
 	pipe->awb_data.enable = 0;
-	pipe->awb_data.gray_threshold = 0.3;
+	pipe->awb_data.gray_threshold = 0.3f;
 	pipe->awb_data.gray_threshold_new = pipe->awb_data.gray_threshold;
-	pipe->awb_data.ctrl_k = 0.01;
+	pipe->awb_data.ctrl_k = 0.01f;
 	pipe->awb_data.ctrl_k_new = pipe->awb_data.ctrl_k;
-	pipe->awb_data.gain_red = 1.0;
-	pipe->awb_data.gain_blue = 1.0;
+	pipe->awb_data.gain_red = 1.0f;
+	pipe->awb_data.gain_blue = 1.0f;
 	
 	pipe->cam_calib_data.enable = 0;
 	pipe->cam_calib_data.lense = O3000_LS_F2_8;
@@ -385,16 +383,23 @@ static void set_default_value(struct color_pipe_t *pipe) {
 	memcpy(pipe->color_calib_data.a, ccm_presets[pipe->color_calib_data.ccm], sizeof(pipe->color_calib_data.a));
 	
 	pipe->sharp_data.enable = 0;
-	pipe->sharp_data.sharp_factor = 5.0;
+	pipe->sharp_data.sharp_factor = 5.0f;
 	pipe->sharp_data.sharp_factor_new = pipe->sharp_data.sharp_factor;
 	pipe->sharp_data.sharp_alg = SHARP_ALG_LOCAL;
 	pipe->sharp_data.sharp_alg_new = pipe->sharp_data.sharp_alg;
-	pipe->sharp_data.local_sens = 94.0;
+	pipe->sharp_data.local_sens = 94.0f;
 	pipe->sharp_data.local_sens_new = pipe->sharp_data.local_sens_new;
 	
 	pipe->gamma_data.enable = 0;
-	pipe->gamma_data.gamma = 1.2;
+	pipe->gamma_data.gamma = 1.2f;
 	pipe->gamma_data.gamma_new = pipe->gamma_data.gamma;
+	
+	pipe->trapcorr_data.enable = 0;
+	pipe->trapcorr_data.map_init = 0;
+	pipe->trapcorr_data.wv = 1.0f;
+	pipe->trapcorr_data.wh = 1.0f;
+	pipe->trapcorr_data.wv_new = pipe->trapcorr_data.wv;
+	pipe->trapcorr_data.wh_new = pipe->trapcorr_data.wh;
 }
 
 
@@ -458,7 +463,7 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe,
 	
 #ifdef DEBUG_PROC_TIME
 	uint64_t ts_start = get_ts();
-	uint64_t ts_debayer, ts_awb, ts_calib, ts_ccm, ts_sharp, ts_gamma;
+	uint64_t ts_debayer, ts_awb, ts_calib, ts_ccm, ts_sharp, ts_gamma, ts_trapcorr;
 #endif // DEBUG_PROC_TIME
 	
 	/*
@@ -676,6 +681,41 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe,
 	
 	
 	/*
+	 * Pipeline stage: Isosceles trapezoid correction
+	 */
+	if(color_pipe->trapcorr_data.enable) {
+		
+		// auto-reinit perspective correction map if image format, resolution or weights have changed
+		if(	color_pipe->trapcorr_data.bit_channel	!= bit_channel	||
+			color_pipe->trapcorr_data.width			!= width		||
+			color_pipe->trapcorr_data.height		!= height		||
+			color_pipe->trapcorr_data.wv			!= color_pipe->trapcorr_data.wv_new ||
+			color_pipe->trapcorr_data.wh			!= color_pipe->trapcorr_data.wh_new) {
+			
+			color_pipe->trapcorr_data.map_init = 0;
+		}
+		
+		// apply user parameter (double buffered)
+		color_pipe->trapcorr_data.wv = color_pipe->trapcorr_data.wv_new;
+		color_pipe->trapcorr_data.wh = color_pipe->trapcorr_data.wh_new;
+		
+		color_pipe->trapcorr_data.img_in = img_out;
+		color_pipe->trapcorr_data.is_color = is_color;
+		color_pipe->trapcorr_data.bit_channel = bit_channel;
+		color_pipe->trapcorr_data.width = width;
+		color_pipe->trapcorr_data.height = height;
+		
+		trapcorr(&(color_pipe->trapcorr_data));
+		
+		img_out = color_pipe->trapcorr_data.img_out;
+	}
+#ifdef DEBUG_PROC_TIME
+	ts_trapcorr = get_ts();
+#endif // DEBUG_PROC_TIME
+	
+	
+	
+	/*
 	 * Return processed image depending on active pipeline stages.
 	 */
 	color_pipe->img_out = img_out;
@@ -692,6 +732,7 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe,
 	printf("        color correction:       %lld msec\n", ts_ccm - ts_calib);
 	printf("        sharpening:             %lld msec\n", ts_sharp - ts_ccm);
 	printf("        gamma:                  %lld msec\n", ts_gamma - ts_sharp);
+	printf("        trapeze correction:     %lld msec\n", ts_trapcorr - ts_sharp);
 #endif // DEBUG_PROC_TIME
 }
 
@@ -834,6 +875,42 @@ void __stdcall color_pipe_stageconf_gamma(struct color_pipe_t *color_pipe, int e
 
 
 /**
+ * Pipeline stage configuration: Isosceles Trapeze Correction
+ * 
+ * The vertical and horizontal correction weight are positive
+ * numbers starting from 0. A weight of 1.0 means no correction. A weight lesser than
+ * 1.0 means the upper trapeze line is shorter than the lower trapeze line.
+ * Therfore a weight greater than 1.0 means the upper trapeze line is longer than
+ * the lower line.
+ * 
+ * @param color_pipe Pointer to pipeline context
+ * @param enable not 0: enable, 0: disable
+ * @param wv vertical weight 
+ * @param wh horizontal weight
+ */
+void __stdcall color_pipe_stageconf_trapcorr(struct color_pipe_t *color_pipe, int enable, float wv, float wh) {
+	// paranoia
+	if(color_pipe == NULL) {
+		printf("%s: Pipeline pointer is NULL!\n", __func__);
+		return;
+	}
+	
+	// range check
+	if(wv < 0.0f) {
+		wv = 0.0f;
+	}
+	if(wh < 0.0f) {
+		wh = 0.0f;
+	}
+	
+	color_pipe->trapcorr_data.enable = enable;
+	color_pipe->trapcorr_data.wv_new = wv;
+	color_pipe->trapcorr_data.wh_new = wh;	
+	color_pipe->trapcorr_data.map_init = 0;
+}
+
+
+/**
  * Open color image processing pipeline.
  * This function allocates memory for various pipe algorithm. The pipeline is set up for a maximum possible image size defined
  * by the height, width and bitdepth per color channel.
@@ -927,7 +1004,7 @@ int __stdcall color_pipe_open(struct color_pipe_t **color_pipe, const int max_im
 		goto _pipe_open_abort;
 	}
 	data->cam_calib_data.calib_map = do_aligned_alloc(ALIGNMENT_SIZE,
-														sizeof(struct lense_undistort_coord_t)*max_img_height*max_img_width,
+														sizeof(struct coord_t)*max_img_height*max_img_width,
 														__func__, __LINE__-1);
 	if(data->cam_calib_data.calib_map == NULL) {
 		goto _pipe_open_abort;
@@ -977,6 +1054,19 @@ int __stdcall color_pipe_open(struct color_pipe_t **color_pipe, const int max_im
 		goto _pipe_open_abort;
 	}
 	
+	// allocate memory for isosceles trapeze correction algorithm
+	data->trapcorr_data.img_out = do_aligned_alloc(ALIGNMENT_SIZE, max_img_size, __func__, __LINE__-1);
+	if(data->trapcorr_data.img_out == NULL) {
+		goto _pipe_open_abort;
+	}
+	data->trapcorr_data.map = do_aligned_alloc(ALIGNMENT_SIZE,
+											   sizeof(struct coord_t)*max_img_height*max_img_width,
+											   __func__, __LINE__-1);
+	if(data->trapcorr_data.map == NULL) {
+		goto _pipe_open_abort;
+	}
+	
+	// set suitable  and valid defaults
 	set_default_value(data);
 	*color_pipe = data;
 	
@@ -1022,6 +1112,8 @@ int __stdcall color_pipe_close(struct color_pipe_t *data) {
 	do_aligned_free(data->sharp_data.sharp_mask);
 	do_aligned_free(data->gamma_data.img_gamma);
 	do_aligned_free(data->gamma_data.gamma_table);
+	do_aligned_free(data->trapcorr_data.img_out);
+	do_aligned_free(data->trapcorr_data.map);
 	
 	// free various image buffers
 	free(data);