Merge branch 'add-projection-matrix' into 'master'

Add projection matrix

See merge request o-3000/color-pipe!2

5 files changed, 418 insertions, 8 deletions

diff --git a/ChangeLog b/ChangeLog
index 8b630c2..6e01282 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -5,6 +5,7 @@
 Version 1.1.0
 	2021-07-01 PR
 	- horizontal isosceles trapeze perspective correction added
+	- perspective transformation added
 
 Version 1.0.2
 	2019-10-16 PR
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);
diff --git a/color_pipe.h b/color_pipe.h
index 9b52f23..26b229b 100644
--- a/color_pipe.h
+++ b/color_pipe.h
@@ -318,6 +318,40 @@ struct trapcorr_data_t {
 
 
 /**
+ * Projective transformation definition 
+ */
+struct projection_data_t {
+	int enable;								///< flag to enable projective transformation
+	void *img_out;							///< projected image. This buffer must be allocated externly.
+	void *img_in;							///< Input image.
+	int is_color;							///< Not 0 if it's a color image
+	int bit_channel;						///< Bits per color channel.
+	int width;								///< image width in number of pixels
+	int height;								///< image height in number of pixels
+	float c_inv[3][3];						///< inverse of 3x3 projection matrix C
+	float c_inv_new[3][3];					///< double buffered matrix
+	int c_upd;								///< projection matrix update flag
+	
+	int map_init;							///< flag indicating transformation map is initialized
+	
+	/**
+	 * projective transfomration map
+	 * A coordinate pair defines the transformed pixel location. It may be shifted (see @ref map_scale_fact).
+	 * 
+	 * This buffer must be allocated externly.
+	 */
+	struct coord_t *map;
+	
+	/**
+	 * Bit shift applied on correction map @ref map.
+	 * The pixel location may lay between a pixel pair. Therefore the coordinates defined at the map
+	 * @ref map are (scaled) shifted by this factor.
+	 */
+	int map_scale_fact;
+};
+
+
+/**
  * Color pipe definition structure holding all memory data from different pipeline
  * stages. This structure and image buffers are allocated dynamically.
  */
@@ -336,6 +370,7 @@ struct color_pipe_t {
 	struct sharp_data_t sharp_data;					///< image sharpening data
 	struct gamma_data_t gamma_data;					///< gamma correction data
 	struct trapcorr_data_t trapcorr_data;			///< isosceles trapezoid correction data
+	struct projection_data_t proj_data;				///< projective transformation data
 };
 
 
@@ -356,6 +391,7 @@ void __stdcall color_pipe_stageconf_color_calib(struct color_pipe_t *color_pipe,
 void __stdcall color_pipe_stageconf_sharp(struct color_pipe_t *color_pipe, int enable, float factor, enum sharp_alg_t alg, float sens);
 void __stdcall color_pipe_stageconf_gamma(struct color_pipe_t *color_pipe, int enable, float gamma);
 void __stdcall color_pipe_stageconf_trapcorr(struct color_pipe_t *color_pipe, int enable, float wv, float wh);
+void __stdcall color_pipe_stageconf_projection(struct color_pipe_t *color_pipe, int enable, float c_inv[3][3]);
 
 #if defined(__cplusplus) || defined(c_plusplus)
 } // extern "C"
diff --git a/color_pipe_private.h b/color_pipe_private.h
index b2954ac..431158b 100644
--- a/color_pipe_private.h
+++ b/color_pipe_private.h
@@ -49,6 +49,8 @@ int gamma_corr(struct gamma_data_t *gamma_data);
 
 int trapcorr(struct trapcorr_data_t *trapcorr_data);
 
+int projection(struct projection_data_t *data);
+
 #if defined(__cplusplus) || defined(c_plusplus)
 } // extern "C"
 #endif
diff --git a/projection.c b/projection.c
new file mode 100644
index 0000000..c3742af
--- /dev/null
+++ b/projection.c
@@ -0,0 +1,272 @@
+/**
+* @file			projection.c
+* @brief		projective transformation
+* @author		Patrick Roth - roth@stettbacher.ch
+* @copyright	Stettbacher Signal Processing AG
+* 
+* @remarks
+*
+* <PRE>
+* This library is free software; you can redistribute it and/or
+* modify it under the terms of the GNU Lesser General Public
+* License as published by the Free Software Foundation; either
+* version 2.1 of the License, or (at your option) any later version.
+*
+* This library is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+* Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public
+* License along with this library; if not, write to the Free Software
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+* </PRE>
+*
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include "color_pipe_private.h"
+
+
+/**
+ * Pixel value interpolation of RGB image (8 bit per color channel).
+ * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
+ * 
+ * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
+ *          +------+------+
+ *          |      |      |
+ *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
+ *          |      |      |
+ *          +------+------+
+ *          |      |      |
+ *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
+ *          |      |      |
+ *          +------+------+
+ * 
+ * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
+ * 
+ * NOTE
+ * The input and output image must have the same pixel size.
+ * 
+ * @param img_out On return: image with interpolated values
+ * @param x saved interpolated pixel value at this x-coordinate
+ * @param y saved interpolated pixel value at this y-coordinate
+ * @param height image height of input and output image in number of pixels
+ * @param width image width of input and output image in number of pixels
+ * @param img_in input image to interpolate pixel values
+ * @param coord_x x-coordinate to interpolate
+ * @param coord_y y-coordinate to interpolate
+ * @param scale_fact coordinates are scaled by this factor
+ */
+static void interpolate_rgb8_scalar(uint8_t *img_out, const int x, const int y, const int height, const int width,
+						 const uint8_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
+#include "alg_interpolate_rgb_scalar.h"
+
+
+/**
+ * Pixel value interpolation of RGB image (16 bit per color channel).
+ * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
+ * 
+ * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
+ *          +------+------+
+ *          |      |      |
+ *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
+ *          |      |      |
+ *          +------+------+
+ *          |      |      |
+ *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
+ *          |      |      |
+ *          +------+------+
+ * 
+ * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
+ * 
+ * NOTE
+ * The input and output image must have the same pixel size.
+ * 
+ * @param img_out On return: image with interpolated values
+ * @param x saved interpolated pixel value at this x-coordinate
+ * @param y saved interpolated pixel value at this y-coordinate
+ * @param height image height of input and output image in number of pixels
+ * @param width image width of input and output image in number of pixels
+ * @param img_in input image to interpolate pixel values
+ * @param coord_x x-coordinate to interpolate
+ * @param coord_y y-coordinate to interpolate
+ * @param scale_fact coordinates are scaled by this factor
+ */
+static void interpolate_rgb16_scalar(uint16_t *img_out, const int x, const int y, const int height, const int width,
+						  const uint16_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
+#include "alg_interpolate_rgb_scalar.h"
+
+
+/**
+ * Pixel value interpolation of monochrome image (8 bit per pixel).
+ * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
+ * 
+ * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
+ *          +------+------+
+ *          |      |      |
+ *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
+ *          |      |      |
+ *          +------+------+
+ *          |      |      |
+ *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
+ *          |      |      |
+ *          +------+------+
+ * 
+ * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
+ * 
+ * NOTE
+ * The input and output image must have the same pixel size.
+ * 
+ * @param img_out On return: image with interpolated values
+ * @param x saved interpolated pixel value at this x-coordinate
+ * @param y saved interpolated pixel value at this y-coordinate
+ * @param height image height of input and output image in number of pixels
+ * @param width image width of input and output image in number of pixels
+ * @param img_in input image to interpolate pixel values
+ * @param coord_x x-coordinate to interpolate
+ * @param coord_y y-coordinate to interpolate
+ * @param scale_fact coordinates are scaled by this factor
+ */
+static void interpolate_mono8_scalar(uint8_t *img_out, const int x, const int y, const int height, const int width,
+						 const uint8_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
+#include "alg_interpolate_mono_scalar.h"
+
+
+/**
+ * Pixel value interpolation of monochrome image (16 bit per pixel).
+ * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
+ * 
+ * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
+ *          +------+------+
+ *          |      |      |
+ *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
+ *          |      |      |
+ *          +------+------+
+ *          |      |      |
+ *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
+ *          |      |      |
+ *          +------+------+
+ * 
+ * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
+ * 
+ * NOTE
+ * The input and output image must have the same pixel size.
+ * 
+ * @param img_out On return: image with interpolated values
+ * @param x saved interpolated pixel value at this x-coordinate
+ * @param y saved interpolated pixel value at this y-coordinate
+ * @param height image height of input and output image in number of pixels
+ * @param width image width of input and output image in number of pixels
+ * @param img_in input image to interpolate pixel values
+ * @param coord_x x-coordinate to interpolate
+ * @param coord_y y-coordinate to interpolate
+ * @param scale_fact coordinates are scaled by this factor
+ */
+static void interpolate_mono16_scalar(uint16_t *img_out, const int x, const int y, const int height, const int width,
+						  const uint16_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
+#include "alg_interpolate_mono_scalar.h"
+
+
+/**
+ * Apply projective transformaion.
+ * 
+ * @param data projective transformation data
+ */ 
+static void apply_projection(struct projection_data_t *data) {
+	int x, y, x_corr, y_corr;
+	const int width = data->width;
+	const int height = data->height;
+	int bit_channel =  data->bit_channel;
+	struct coord_t *map = data->map;
+	void *img_calib = data->img_out;
+	void *img_uncalib = data->img_in;
+	const int scale_fact = data->map_scale_fact;
+	const int is_color = data->is_color;
+	
+	for(y = 0; y < height; y++) {
+		for(x = 0; x < width; x++) {
+			x_corr = map->x;
+			y_corr = map->y;
+			map++;
+			
+			if(bit_channel <= 8) {
+				if(is_color) {
+					interpolate_rgb8_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
+				}
+				else {
+					interpolate_mono8_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
+				}
+			}
+			else if(bit_channel <= 16) {
+				if(is_color) {
+					interpolate_rgb16_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
+				}
+				else {
+					interpolate_mono16_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
+				}
+			}
+		}
+	}
+}
+
+
+/**
+ * Initialize perspective transformation map.
+ * 
+ * @param data projective transformation data
+ */ 
+static void init_map(struct projection_data_t *data) {
+	int x, y;
+	struct coord_t *map = data->map;
+	const int scale_fact = (1 << (data->map_scale_fact));
+	const int width = data->width;
+	const int height = data->height;
+	float x_corr, y_corr;
+	const float c00 = data->c_inv[0][0];
+	const float c01 = data->c_inv[0][1];
+	const float c02 = data->c_inv[0][2];
+	const float c10 = data->c_inv[1][0];
+	const float c11 = data->c_inv[1][1];
+	const float c12 = data->c_inv[1][2];
+	const float c20 = data->c_inv[2][0];
+	const float c21 = data->c_inv[2][1];
+	const float c22 = data->c_inv[2][2];
+	
+	for(y = 0; y < height; y++) {
+		for(x = 0; x < width; x++) {
+			x_corr = (c00*x+c01*y+c02)/(c20*x+c21*y+c22);
+			y_corr = (c10*x+c11*y+c12)/(c20*x+c21*y+c22);
+			
+			// apply scaling factor
+			map->x = (int)roundf(x_corr*scale_fact);
+			map->y = (int)roundf(y_corr*scale_fact);
+			map++;
+		}
+	}
+}
+
+
+/**
+ * Apply projective transformation.
+ * 
+ * @param data required data for projective transformation
+ * @return 0 on success otherwise -1
+ */
+int projection(struct projection_data_t *data) {
+	
+	/*
+	 * Create projective transformation map if needed.
+	 */ 
+	if(data->map_init == 0) {
+		data->map_scale_fact = 6;	// scale by 9 means 2^6 = 64
+		init_map(data);
+		data->map_init = 1;
+	}
+	
+	apply_projection(data);
+	return 0;
+}