stage perspective transformation added

1 files changed, 107 insertions, 8 deletions

diff --git a/color_pipe.c b/color_pipe.c
index b0a9ddf..3c4200f 100644
--- a/color_pipe.c
+++ b/color_pipe.c
@@ -400,6 +400,20 @@ static void set_default_value(struct color_pipe_t *pipe) {
 	pipe->trapcorr_data.wh = 0.0f;
 	pipe->trapcorr_data.wv_new = pipe->trapcorr_data.wv;
 	pipe->trapcorr_data.wh_new = pipe->trapcorr_data.wh;
+	
+	pipe->proj_data.enable = 0;
+	pipe->proj_data.map_init = 0;
+	pipe->proj_data.c_inv[0][0] = 1.0f;		// use identity matrix
+	pipe->proj_data.c_inv[0][1] = 0.0f;
+	pipe->proj_data.c_inv[0][2] = 0.0f;
+	pipe->proj_data.c_inv[1][0] = 0.0f;
+	pipe->proj_data.c_inv[1][1] = 1.0f;
+	pipe->proj_data.c_inv[1][2] = 0.0f;
+	pipe->proj_data.c_inv[2][0] = 0.0f;
+	pipe->proj_data.c_inv[2][1] = 0.0f;
+	pipe->proj_data.c_inv[2][2] = 1.0f;
+	memcpy(pipe->proj_data.c_inv_new, pipe->proj_data.c_inv, sizeof(pipe->proj_data.c_inv));
+	pipe->proj_data.c_upd = 0;
 }
 
 
@@ -463,7 +477,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, ts_trapcorr;
+	uint64_t ts_debayer, ts_awb, ts_calib, ts_ccm, ts_sharp, ts_gamma, ts_trapcorr, ts_projection;
 #endif // DEBUG_PROC_TIME
 	
 	/*
@@ -714,6 +728,40 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe,
 #endif // DEBUG_PROC_TIME
 	
 	
+	/*
+	 * Pipeline stage: Projective transformation
+	 */
+	if(color_pipe->proj_data.enable) {
+		
+		// auto-reinit perspective correction map if image format, resolution or weights have changed
+		if(	color_pipe->proj_data.bit_channel	!= bit_channel	||
+			color_pipe->proj_data.width			!= width		||
+			color_pipe->proj_data.height		!= height) {
+			
+			color_pipe->proj_data.map_init = 0;
+		}
+		
+		// apply user parameter (double buffered)
+		if(color_pipe->proj_data.c_upd) {
+			memcpy(color_pipe->proj_data.c_inv, color_pipe->proj_data.c_inv_new, sizeof(color_pipe->proj_data.c_inv));
+			color_pipe->proj_data.c_upd = 0;
+			color_pipe->proj_data.map_init = 0;
+		}
+		
+		color_pipe->proj_data.img_in = img_out;
+		color_pipe->proj_data.is_color = is_color;
+		color_pipe->proj_data.bit_channel = bit_channel;
+		color_pipe->proj_data.width = width;
+		color_pipe->proj_data.height = height;
+		
+		projection(&(color_pipe->proj_data));
+		
+		img_out = color_pipe->proj_data.img_out;
+	}
+#ifdef DEBUG_PROC_TIME
+	ts_projection = get_ts();
+#endif // DEBUG_PROC_TIME
+		
 	
 	/*
 	 * Return processed image depending on active pipeline stages.
@@ -726,13 +774,14 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe,
 	
 	
 #ifdef DEBUG_PROC_TIME
-	printf("        debayer:                %lld msec\n", ts_debayer - ts_start);
-	printf("        awb:                    %lld msec\n", ts_awb - ts_debayer);
-	printf("        camera calib:           %lld msec\n", ts_calib - ts_awb);
-	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);
+	printf("        debayer:                       %lld msec\n", ts_debayer - ts_start);
+	printf("        awb:                           %lld msec\n", ts_awb - ts_debayer);
+	printf("        camera calib:                  %lld msec\n", ts_calib - ts_awb);
+	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_gamma);
+	printf("        projective transformation:     %lld msec\n", ts_projection - ts_trapcorr);
 #endif // DEBUG_PROC_TIME
 }
 
@@ -917,6 +966,42 @@ void __stdcall color_pipe_stageconf_trapcorr(struct color_pipe_t *color_pipe, in
 
 
 /**
+ * Pipeline stage configuration: projection transformation
+ * 
+ * Project point p to p' using the the projection matrix C
+ * with homogeneous coordinates.
+ * 
+ * t * p' = C * p
+ * 
+ * where:
+ *         / u \              / x \
+ *   p' =  | v |        p  =  | y |        t = scaling factor
+ *         \ 1 /              \ 1 /
+ * 
+ * 
+ *       / c00 c01 c02 \
+ *   C = | c10 c11 c12 |
+ *       \ c20 c21 c22 /
+ * 
+ * p': projected points
+ * p:  point to be projected
+ * C:  projection matrix
+ * 
+ * The color-pipe requires the invers of matrix C because
+ * an inverse mapping is implemented.
+ * 
+ * @param color_pipe Pointer to pipeline context
+ * @param enable not 0: enable, 0: disable
+ * @param c_inv inverse of 3x3 projection matrix C
+ */ 
+void __stdcall color_pipe_stageconf_projection(struct color_pipe_t *color_pipe, int enable, float c_inv[3][3]) {
+	memcpy(color_pipe->proj_data.c_inv_new, c_inv, sizeof(color_pipe->proj_data.c_inv_new));
+	color_pipe->proj_data.c_upd = 1;
+	color_pipe->proj_data.enable = enable;
+}
+
+
+/**
  * 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.
@@ -1072,6 +1157,18 @@ int __stdcall color_pipe_open(struct color_pipe_t **color_pipe, const int max_im
 		goto _pipe_open_abort;
 	}
 	
+	// allocate memory for projective transformation
+	data->proj_data.img_out = do_aligned_alloc(ALIGNMENT_SIZE, max_img_size, __func__, __LINE__-1);
+	if(data->proj_data.img_out == NULL) {
+		goto _pipe_open_abort;
+	}
+	data->proj_data.map = do_aligned_alloc(ALIGNMENT_SIZE,
+										   sizeof(struct coord_t)*max_img_height*max_img_width,
+										   __func__, __LINE__-1);
+	if(data->proj_data.map == NULL) {
+		goto _pipe_open_abort;
+	}
+	
 	// set suitable  and valid defaults
 	set_default_value(data);
 	*color_pipe = data;
@@ -1120,6 +1217,8 @@ int __stdcall color_pipe_close(struct color_pipe_t *data) {
 	do_aligned_free(data->gamma_data.gamma_table);
 	do_aligned_free(data->trapcorr_data.img_out);
 	do_aligned_free(data->trapcorr_data.map);
+	do_aligned_free(data->proj_data.img_out);
+	do_aligned_free(data->proj_data.map);
 	
 	// free various image buffers
 	free(data);