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

Add projection matrix

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

1 files changed, 272 insertions, 0 deletions

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