working copy

algorithm not working and has checked

1 files changed, 314 insertions, 0 deletions

diff --git a/trapcorr.c b/trapcorr.c
new file mode 100644
index 0000000..3ace25f
--- /dev/null
+++ b/trapcorr.c
@@ -0,0 +1,314 @@
+/**
+* @file			trapcorr.c
+* @brief		isosceles trapeze correction algorithm
+* @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 iosceles trapez correction.
+ * 
+ * @param data correction data
+ */ 
+static void correct(struct trapcorr_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);
+				}
+			}
+		}
+	}
+}
+
+
+/**
+ * Initialze perspective correction map.
+ * 
+ * @param data required correction data
+ */ 
+static void init_map(struct trapcorr_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;
+	const float p = data->wh * (width/2.0f);
+	float p_i, row_scale, x_corr, y_corr;
+	int x_start, x_end;
+	
+	
+// 	printf("XXX P = %f\n", p);
+	
+	/*
+	 * horizontal correction
+	 */ 
+	if((width%2) == 0) {
+		// even width
+		x_start = (-1)*width/2;
+		x_end = width/2-1;
+	}
+	else {
+		// odd width
+		x_start = (-1)*(width-1)/2;
+		x_end = (width-1)/2;
+	}
+	
+// 	printf("XXX x_start = %d, x_end = %d, %d x %d\n", x_start,  x_end, width, height);
+	
+	for(y = 0; y < height; y++) {
+		
+		p_i = ((width/2.0f - p) / height) * y + p;
+		row_scale = 2.0f * p_i / width;
+		
+		// we don't correct vertically
+		y_corr = y;
+		
+		for(x = x_start; x <= x_end; x++) {
+			x_corr = x/row_scale;
+			
+// 			if(x > -325 && x < -315 && y < 2) {
+// 				printf("%d/%d: p_i = %f, row_scale = %f, x_corr = %f\n", y, x, p_i, row_scale, x_corr);
+// 			}
+			
+			// trapeze is horizontally symmetrically centered --> shift right
+			x_corr -= x_start;
+			
+// 			if(x > -325 && x < -315 && y < 2) {
+// 				printf("    ----> x_corr (shift)= %f\n", x_corr);
+// 			}
+			
+			// range check
+			if(x_corr < 0.0f) {
+				x_corr = 0.0f;
+			}
+			else if(x_corr > (width-1.0f)) {
+				x_corr = width-1.0f;
+			}
+			
+			// apply scaling factor
+			map->x = (int)roundf(x_corr*scale_fact);
+			map->y = (int)roundf(y_corr*scale_fact);
+			map++;
+		}
+	}
+}
+
+
+/**
+ * Apply iosceles trapez correction to given image type.
+ * 
+ * @param trapcorr_data required data for trapez correction
+ * @return 0 on success otherwise -1
+ */
+int trapcorr(struct trapcorr_data_t *trapcorr_data) {
+	
+	/*
+	 * Create perspective correction map if needed.
+	 */ 
+	if(trapcorr_data->map_init == 0) {
+		trapcorr_data->map_scale_fact = 9;	// scale by 9 means 2^9 = 512
+		init_map(trapcorr_data);
+		trapcorr_data->map_init = 1;
+		printf("XXXXX map initialized!\n");
+	}
+	
+	// apply perspective correction
+	correct(trapcorr_data);
+	return 0;
+}