diff options
Diffstat (limited to 'filter.c')
-rw-r--r-- | filter.c | 532 |
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++; + } + } + +} + |