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