initial commit

import from github

1 files changed, 532 insertions, 0 deletions

diff --git a/filter.c b/filter.c
new file mode 100644
index 0000000..6c8d51a
--- /dev/null
+++ b/filter.c
@@ -0,0 +1,532 @@
+/**
+* @file			filter.c
+* @brief		various filtering algorithm
+* @author		Patrick Roth - roth@stettbacher.ch
+* @version		1.0
+* @date			2015-08-28
+* @copyright	2012-2016 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 <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+
+#define MAX_GAUSS_KERNEL_SIZE			25			///< maximum gaussian kernel size (must be an odd number)
+
+
+/**
+ * Filter pixel with given 3x3 kernel. Fixed-point is used.
+ * 
+ * @param a11 kernel weight at position 1/1
+ * @param a12 kernel weight at position 1/2
+ * @param a13 kernel weight at position 1/3
+ * @param a21 kernel weight at position 2/1
+ * @param a22 kernel weight at position 2/2
+ * @param a23 kernel weight at position 2/3
+ * @param a31 kernel weight at position 3/1
+ * @param a32 kernel weight at position 3/2
+ * @param a33 kernel weight at position 3/3
+ * @param p11 pixel value at position 1/1
+ * @param p12 pixel value at position 1/2
+ * @param p13 pixel value at position 1/3
+ * @param p21 pixel value at position 2/1
+ * @param p22 pixel value at position 2/2
+ * @param p23 pixel value at position 2/3
+ * @param p31 pixel value at position 3/1
+ * @param p32 pixel value at position 3/2
+ * @param p33 pixel value at position 3/3 
+ * @param shift_fact The shifting factor defines how many number of bits the kernel and pixel were shifted to left.
+ * @return filtered pixel value
+ */
+static inline int16_t calc_filter3x3(const int16_t a11, const int16_t a12, const int16_t a13,
+									 const int16_t a21, const int16_t a22, const int16_t a23,
+									 const int16_t a31, const int16_t a32, const int16_t a33,
+									 const int16_t p11, const int16_t p12, const int16_t p13,
+									 const int16_t p21, const int16_t p22, const int16_t p23,
+									 const int16_t p31, const int16_t p32, const int16_t p33,
+									 const int shift_fact) {
+	
+	int16_t out;
+	
+	out = (a11*p11 + a12*p12 + a13*p13 + a21*p21 + a22*p22 + a23*p23 + a31*p31 + a32*p32 + a33*p33) >> shift_fact;
+	return out;
+}
+
+
+/**
+ * Apply sobel kernel to given 3x3 pixels.
+ * 
+ * @param p11 pixel value at position 1/1
+ * @param p12 pixel value at position 1/2
+ * @param p13 pixel value at position 1/3
+ * @param p21 pixel value at position 2/1
+ * @param p22 pixel value at position 2/2
+ * @param p23 pixel value at position 2/3
+ * @param p31 pixel value at position 3/1
+ * @param p32 pixel value at position 3/2
+ * @param p33 pixel value at position 3/3 
+ * @return filtered pixel value
+ * 
+ * NOTE
+ * For performance reasons the return value is ret = abs(sobel_x) + (abs sobel_y) instead of ret = sqrt(abs(sobel_x) + (abs sobel_y))
+ */
+static inline int16_t calc_filter_sobel(const int16_t p11, const int16_t p12, const int16_t p13,
+										const int16_t p21, const int16_t p22, const int16_t p23,
+										const int16_t p31, const int16_t p32, const int16_t p33) {
+	
+	int16_t sobel_y, sobel_x;
+	
+	sobel_x = (-1)*p11 + p13 - 2*p21 + 2*p23 - p31 + p33;
+	if(sobel_x < 0) {
+		sobel_x = sobel_x * -1;
+	}
+	
+	sobel_y = (-1)*p11 - 2*p12 - p13 + p31 + 2*p32 + p33;
+	if(sobel_y < 0) {
+		sobel_y = sobel_y * -1;
+	}
+	
+	return (sobel_x+sobel_y);
+}
+
+
+/**
+ * Filter pixel at given image by applying NxN kernel. Each pixel at the image is a 32 bit signed value. The step size
+ * defines the offset to the next pixel. E. g. a step size of 3 means the image uses 3 32-bit channels.
+ * The image and the kernel uses fixed-point values. Therfore a shifting factor is used.
+ * 
+ * NOTE
+ * The kernel heigth and width must be an odd value (e. g. 3x5 is accepted). This function doesn't make any sanity checks
+ * due to performance reason. 
+ * 
+ * @param img start image address
+ * @param height image height in number of pixels
+ * @param width image width in number of pixels
+ * @param step_size step size
+ * @param coord_y y pixel coordinate to apply kernel
+ * @param coord_x x pixel coordinate to apply kernel
+ * @param a pointer to filter kernel (the kernel values must be shifted correctly)
+ * @param kernel_height kernel height in number of pixels
+ * @param kernel_width kernel width in number of pixels
+ * @param shift_fact shifting factor
+ * @return filtered pixel value
+ */
+static int16_t calc_filterNxN(const int16_t *img, const int height, const int width, const int step_size,
+							  const int coord_y, const int coord_x, const int16_t *a, const int kernel_height, const int kernel_width,
+							  const int shift_fact) {
+								
+	int y, x, y_start, y_end, x_start, x_end;
+	int index_img, index_kernel;
+	int64_t out;
+	
+	y_start = coord_y-kernel_height/2;
+	y_end = y_start+kernel_height;
+	x_start = coord_x-kernel_width/2;
+	x_end = x_start+kernel_width;
+	
+	index_kernel = 0;
+	out = 0;
+	for(y = y_start; y < y_end; y++) {
+		index_img = (y*width + x_start)*step_size;
+		for(x = x_start; x < x_end; x++) {
+			out += a[index_kernel]*img[index_img];
+			index_img += step_size;
+			index_kernel++;
+		}
+	}
+	return (out>>shift_fact);
+}
+
+
+/**
+ * Apply sobel filter (edge detection) on given input image of type: 3 channels, 16 bit signed fixed-point per channel
+ * 
+ * @param img_sobel On return: sobel filtered image
+ * @param img_in input image to be filtered
+ * @param height image height in number of pixels
+ * @param width image width in number of pixels
+ * @param skip_ch1 set to 0 if channel 1 should be filtered
+ * @param skip_ch2 set to 0 if channel 2 should be filtered
+ * @param skip_ch3 set to 0 if channel 3 should be filtered
+ */
+void filter_sobel_3s16(int16_t *img_sobel, const int16_t *img_in, const int height, const int width,
+					   const int skip_ch1, const int skip_ch2, const int skip_ch3) {
+	
+	int y, x, index_upper, index_center, index_lower, index_left, index_right;
+	
+	for(y = 1; y < (height-1); y++) {
+		
+		index_upper = (y-1)*width*3;
+		index_center = y*width*3;
+		index_lower = (y+1)*width*3;
+		
+		
+		for(x = 1; x < (width-1); x++) {
+			
+			if(!skip_ch1) {
+				img_sobel[index_center+3] = calc_filter_sobel(	img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
+																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
+																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6]);
+			}
+			else {
+				img_sobel[index_center+3] = 0;
+			}
+			
+			index_upper++;
+			index_center++;
+			index_lower++;
+			
+			if(!skip_ch2) {
+				img_sobel[index_center+3] = calc_filter_sobel(	img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
+																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
+																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6]);
+			}
+			else {
+				img_sobel[index_center+3] = 0;
+			}
+			
+			index_upper++;
+			index_center++;
+			index_lower++;
+			
+			if(!skip_ch3) {
+				img_sobel[index_center+3] = calc_filter_sobel(	img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
+																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
+																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6]);
+			}
+			else {
+				img_sobel[index_center+3] = 0;
+			}
+			
+			index_upper++;
+			index_center++;
+			index_lower++;
+		}
+	}
+	
+	
+	/*
+	 * Image border are set to 0
+	 */
+	index_upper = 0;
+	index_lower = (height-1)*width*3;
+	for(x = 0; x < width; x++) {
+		
+		// horizontal upper border
+		img_sobel[index_upper] = 0;
+		img_sobel[index_upper+1] = 0;
+		img_sobel[index_upper+2] = 0;
+		index_upper += 3;
+		
+		// horizontal lower border
+		img_sobel[index_lower] = 0;
+		img_sobel[index_lower+1] = 0;
+		img_sobel[index_lower+2] = 0;
+		index_lower += 3;
+	}
+	
+	index_left = 0;
+	index_right = width*3-3;
+	for(y = 0; y < height; y++) {
+		
+		// verticel left border
+		img_sobel[index_left] = 0;;
+		img_sobel[index_left+1] = 0;
+		img_sobel[index_left+2] = 0;
+		index_left += width*3;
+		
+		// verticel right border
+		img_sobel[index_right] = 0;;
+		img_sobel[index_right+1] = 0;
+		img_sobel[index_right+2] = 0;
+		index_right += width*3;
+	}
+}
+
+
+/**
+ * Apply gauss low-pass filter on given input image of type: 3 channels, 16 bit signed fixed-point per channel.
+ * An odd kernel size is expected.
+ * 
+ * @param img_gauss On return: gauss low-pass filtered image
+ * @param img_in input image to be filtered
+ * @param height image height in number of pixels
+ * @param width image width in number of pixels
+ * @param kernel_size filter kernel size (minimum of 3, maximum see @ref MAX_GAUSS_KERNEL_SIZE)
+ * @param spread gaussian curve spreading value, as higher as bigger spread (spread = sigma)
+ * @param skip_ch1 set to 0 if channel 1 should be filtered
+ * @param skip_ch2 set to 0 if channel 2 should be filtered
+ * @param skip_ch3 set to 0 if channel 3 should be filtered
+ */
+void filter_gauss_3s16(int16_t *img_gauss, const int16_t *img_in, const int height, const int width, int kernel_size, const float spread,
+					   const int skip_ch1, const int skip_ch2, const int skip_ch3) {
+	
+	int y, x, index, index_upper, index_center, index_lower;
+	int kernel_start, kernel_end;
+	float a[MAX_GAUSS_KERNEL_SIZE*MAX_GAUSS_KERNEL_SIZE];
+	int16_t a_s16[MAX_GAUSS_KERNEL_SIZE*MAX_GAUSS_KERNEL_SIZE];
+	float sum;
+	int x_start, x_end, y_start, y_end;
+	const int shift_fact = 10;		// 2^10 = 1024--> about 3 digits
+	
+	
+	if((kernel_size%2) == 0) {
+		// even kernel size!!
+		kernel_size++;
+	}
+	
+	if(kernel_size < 3) {
+		kernel_size = 3;
+	}
+	else if(kernel_size > MAX_GAUSS_KERNEL_SIZE) {
+		kernel_size = MAX_GAUSS_KERNEL_SIZE;
+	}
+	
+	
+	/*
+	 * Generate 2D-gaussian kernel depending on given size.
+	 * Norm kernel, that sum over each kernel element is 1.0.
+	 */
+	kernel_start = -1*kernel_size/2;
+	kernel_end = -1*kernel_start;
+	index = 0;
+	sum = 0;
+	for(y = kernel_start; y <= kernel_end; y++) {
+		for(x = kernel_start; x <= kernel_end; x++) {
+			a[index] = (1.0/(2*M_PI*spread*spread))*expf((-1)*((x*x+y*y)/(2*spread*spread)));
+			sum += a[index];
+			index++;
+		}
+	}
+	index = 0;
+	for(y = 0; y < kernel_size; y++) {
+		for(x = 0; x < kernel_size; x++) {
+			a[index] /= sum;
+			a_s16[index] = (int16_t)roundf(a[index] * (1<<shift_fact));
+// 			printf("XXX a[%d,%d] = %.20f --> %d\n", y, x, a[index], a_s16[index]);
+			index++;
+		}
+	}
+	
+	
+	/*
+	 * This loop filters the image without border region.
+	 */
+	y_start = kernel_size/2;
+	y_end = height-y_start;
+	x_start = kernel_size/2;
+	x_end = width-x_start;
+	for(y = y_start; y < y_end; y++) {
+		
+		index = (y*width+x_start)*3;
+		index_upper = (y-1)*width*3;
+		index_center = y*width*3;
+		index_lower = (y+1)*width*3;
+		
+		for(x = x_start; x < x_end; x++) {
+			
+			if(!skip_ch1) {
+				if(kernel_size == 3) {
+					img_gauss[index_center] = calc_filter3x3(	a_s16[0], a_s16[1], a_s16[2], a_s16[3], a_s16[4], a_s16[5], a_s16[6], a_s16[7], a_s16[8],
+																img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
+																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
+																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6],
+																shift_fact);
+				}
+				else {
+					img_gauss[index] = calc_filterNxN(img_in, height, width, 3, y, x, a_s16, kernel_size, kernel_size, shift_fact);
+				}
+			}
+			else {
+				img_gauss[index] = 0;
+			}
+			
+			index++;
+			index_upper++;
+			index_center++;
+			index_lower++;
+			
+			if(!skip_ch2) {
+				if(kernel_size == 3) {
+					img_gauss[index_center] = calc_filter3x3(	a_s16[0], a_s16[1], a_s16[2], a_s16[3], a_s16[4], a_s16[5], a_s16[6], a_s16[7], a_s16[8],
+																img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
+																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
+																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6],
+																shift_fact);
+				}
+				else {
+					img_gauss[index] = calc_filterNxN(img_in+1, height, width, 3, y, x, a_s16, kernel_size, kernel_size, shift_fact);
+				}
+			}
+			else {
+				img_gauss[index] = 0;
+			}
+			
+			index++;
+			index_upper++;
+			index_center++;
+			index_lower++;
+			
+			if(!skip_ch3) {
+				if(kernel_size == 3) {
+					img_gauss[index_center] = calc_filter3x3(	a_s16[0], a_s16[1], a_s16[2], a_s16[3], a_s16[4], a_s16[5], a_s16[6], a_s16[7], a_s16[8],
+																img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
+																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
+																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6],
+																shift_fact);
+				}
+				else {
+					img_gauss[index] = calc_filterNxN(img_in+2, height, width, 3, y, x, a_s16, kernel_size, kernel_size, shift_fact);
+				}
+			}
+			else {
+				img_gauss[index] = 0;
+			}
+			
+			index++;
+			index_upper++;
+			index_center++;
+			index_lower++;
+		}
+	}
+	
+	
+	/*
+	 * Image border are not filtered
+	 */
+	// handler upper horizontal border area
+	index = 0;
+	for(y = 0; y < y_start; y++) {
+		for(x = 0; x < width; x++) {
+			img_gauss[index] = img_in[index];
+			img_gauss[index+1] = img_in[index+1];
+			img_gauss[index+2] = img_in[index+2];
+			index += 3;
+		}
+	}
+	
+	// handler lower horizontal border area
+	index = y_end*width*3;
+	for(y = y_end; y < height; y++) {
+		for(x = 0; x < width; x++) {
+			img_gauss[index] = img_in[index];
+			img_gauss[index+1] = img_in[index+1];
+			img_gauss[index+2] = img_in[index+2];
+			index += 3;
+		}
+	}
+	
+	// handler left vertical border area
+	for(y = 0; y < height; y++) {
+		index = y*width*3;
+		for(x = 0; x < x_start; x++) {
+			img_gauss[index] = img_in[index];
+			img_gauss[index+1] = img_in[index+1];
+			img_gauss[index+2] = img_in[index+2];
+			index += 3;
+		}
+	}
+	
+	// handler right vertical border area
+	for(y = 0; y < height; y++) {
+		index = (y*width+x_end)*3;
+		for(x = x_end; x < width; x++) {
+			img_gauss[index] = img_in[index];
+			img_gauss[index+1] = img_in[index+1];
+			img_gauss[index+2] = img_in[index+2];
+			index += 3;
+		}
+	}
+}
+
+
+/**
+ * Generate binary image from given image of type: 3 channels, 16 bit signed fixed-point per channel.
+ * All pixel values above the given threshold are set to the defined value. Otherwise the values are set to 0.
+ * 
+ * @param img_gauss On return: gauss low-pass filtered image
+ * @param img_in input image to be filtered
+ * @param height image height in number of pixels
+ * @param width image width in number of pixels
+ * @param threshold binary threshold
+ * @param bin_value Pixel values above given threshold are set to this value. Values below the threshold are set to 0.
+ * @param skip_ch1 set to 0 if channel 1 should be filtered
+ * @param skip_ch2 set to 0 if channel 2 should be filtered
+ * @param skip_ch3 set to 0 if channel 3 should be filtered
+ */
+void filter_binary_3s16(int8_t *img_bin, const int16_t *img_in, const int height, const int width, const int16_t threshold, const int8_t bin_value,
+						const int skip_ch1, const int skip_ch2, const int skip_ch3) {
+	
+	int y, x, index;
+	
+	index = 0;
+		
+	for(y = 0; y < height; y++) {
+		for(x = 0; x < width; x++) {
+			if(!skip_ch1) {
+				if(img_in[index] >= threshold) {
+					img_bin[index] = bin_value;
+				}
+				else {
+					img_bin[index] = 0;
+				}
+			}
+			else {
+				img_bin[index] = 0;
+			}
+			
+			index++;
+			
+			if(!skip_ch2) {
+				if(img_in[index] >= threshold) {
+					img_bin[index] = bin_value;
+				}
+				else {
+					img_bin[index] = 0;
+				}
+			}
+			else {
+				img_bin[index] = 0;
+			}
+			
+			index++;
+			
+			if(!skip_ch3) {
+				if(img_in[index] >= threshold) {
+					img_bin[index] = bin_value;
+				}
+				else {
+					img_bin[index] = 0;
+				}
+			}
+			else {
+				img_bin[index] = 0;
+			}
+			
+			index++;
+		}
+	}
+	
+}
+