From aaa7ff32f4af51dffb5cee42eed96828abfd1484 Mon Sep 17 00:00:00 2001 From: Patrick Roth Date: Tue, 29 Jun 2021 14:20:32 +0200 Subject: working copy algorithm not working and has checked --- camera_calib.c | 12 +- color_pipe.c | 116 +++++++++++++++++-- color_pipe.h | 98 +++++++++++----- color_pipe_private.h | 2 + trapcorr.c | 314 +++++++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 497 insertions(+), 45 deletions(-) create mode 100644 trapcorr.c diff --git a/camera_calib.c b/camera_calib.c index 4b03a12..b55f954 100644 --- a/camera_calib.c +++ b/camera_calib.c @@ -488,7 +488,7 @@ static void init_undistort_map_scalar(struct cam_calib_data_t *data) { float dist_radial, dist_tang_x, dist_tang_y, dist_x, dist_y; float _x, _y, _y_sq, _x_sq, r_sq; float x_corr, y_corr; - struct lense_undistort_coord_t *map; + struct coord_t *map; const int scale_fact = (1 << (data->calib_map_scale_fact)); float x_resolution_scaling, y_resolution_scaling; // camera_matrix needs to be scaled depending on the camera resolution @@ -541,8 +541,8 @@ static void init_undistort_map_scalar(struct cam_calib_data_t *data) { x_corr = fx*dist_x + cx - fov_x_start; y_corr = fy*dist_y + cy - fov_y_start; - map->coord_x = (int)roundf(x_corr*scale_fact); - map->coord_y = (int)roundf(y_corr*scale_fact); + map->x = (int)roundf(x_corr*scale_fact); + map->y = (int)roundf(y_corr*scale_fact); map++; } } @@ -700,7 +700,7 @@ static void calib_scalar(struct cam_calib_data_t *data) { int x_corr, y_corr; void *img_calib, *img_uncalib; int bit_channel; - struct lense_undistort_coord_t *map; + struct coord_t *map; const int scale_fact = data->calib_map_scale_fact; const int is_color = data->is_color; @@ -718,8 +718,8 @@ static void calib_scalar(struct cam_calib_data_t *data) { for(v = fov_y_start; v <= fov_y_end; v++) { for(u = fov_x_start; u <= fov_x_end; u++) { - x_corr = map->coord_x; - y_corr = map->coord_y; + x_corr = map->x; + y_corr = map->y; map++; if(bit_channel <= 8) { diff --git a/color_pipe.c b/color_pipe.c index 0b77c61..648e259 100644 --- a/color_pipe.c +++ b/color_pipe.c @@ -2,9 +2,7 @@ * @file main.cpp * @brief Color Image Processing Pipeline with O-3000 USB camera * @author Patrick Roth - roth@stettbacher.ch -* @version 1.0 -* @date 2015-02-10 -* @copyright 2012-2016 Stettbacher Signal Processing AG +* @copyright Stettbacher Signal Processing AG * * @remarks * @@ -365,12 +363,12 @@ static void set_default_value(struct color_pipe_t *pipe) { pipe->debayer_data.alg_new = pipe->debayer_data.alg; pipe->awb_data.enable = 0; - pipe->awb_data.gray_threshold = 0.3; + pipe->awb_data.gray_threshold = 0.3f; pipe->awb_data.gray_threshold_new = pipe->awb_data.gray_threshold; - pipe->awb_data.ctrl_k = 0.01; + pipe->awb_data.ctrl_k = 0.01f; pipe->awb_data.ctrl_k_new = pipe->awb_data.ctrl_k; - pipe->awb_data.gain_red = 1.0; - pipe->awb_data.gain_blue = 1.0; + pipe->awb_data.gain_red = 1.0f; + pipe->awb_data.gain_blue = 1.0f; pipe->cam_calib_data.enable = 0; pipe->cam_calib_data.lense = O3000_LS_F2_8; @@ -385,16 +383,23 @@ static void set_default_value(struct color_pipe_t *pipe) { memcpy(pipe->color_calib_data.a, ccm_presets[pipe->color_calib_data.ccm], sizeof(pipe->color_calib_data.a)); pipe->sharp_data.enable = 0; - pipe->sharp_data.sharp_factor = 5.0; + pipe->sharp_data.sharp_factor = 5.0f; pipe->sharp_data.sharp_factor_new = pipe->sharp_data.sharp_factor; pipe->sharp_data.sharp_alg = SHARP_ALG_LOCAL; pipe->sharp_data.sharp_alg_new = pipe->sharp_data.sharp_alg; - pipe->sharp_data.local_sens = 94.0; + pipe->sharp_data.local_sens = 94.0f; pipe->sharp_data.local_sens_new = pipe->sharp_data.local_sens_new; pipe->gamma_data.enable = 0; - pipe->gamma_data.gamma = 1.2; + pipe->gamma_data.gamma = 1.2f; pipe->gamma_data.gamma_new = pipe->gamma_data.gamma; + + pipe->trapcorr_data.enable = 0; + pipe->trapcorr_data.map_init = 0; + pipe->trapcorr_data.wv = 1.0f; + pipe->trapcorr_data.wh = 1.0f; + pipe->trapcorr_data.wv_new = pipe->trapcorr_data.wv; + pipe->trapcorr_data.wh_new = pipe->trapcorr_data.wh; } @@ -458,7 +463,7 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe, #ifdef DEBUG_PROC_TIME uint64_t ts_start = get_ts(); - uint64_t ts_debayer, ts_awb, ts_calib, ts_ccm, ts_sharp, ts_gamma; + uint64_t ts_debayer, ts_awb, ts_calib, ts_ccm, ts_sharp, ts_gamma, ts_trapcorr; #endif // DEBUG_PROC_TIME /* @@ -675,6 +680,41 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe, #endif // DEBUG_PROC_TIME + /* + * Pipeline stage: Isosceles trapezoid correction + */ + if(color_pipe->trapcorr_data.enable) { + + // auto-reinit perspective correction map if image format, resolution or weights have changed + if( color_pipe->trapcorr_data.bit_channel != bit_channel || + color_pipe->trapcorr_data.width != width || + color_pipe->trapcorr_data.height != height || + color_pipe->trapcorr_data.wv != color_pipe->trapcorr_data.wv_new || + color_pipe->trapcorr_data.wh != color_pipe->trapcorr_data.wh_new) { + + color_pipe->trapcorr_data.map_init = 0; + } + + // apply user parameter (double buffered) + color_pipe->trapcorr_data.wv = color_pipe->trapcorr_data.wv_new; + color_pipe->trapcorr_data.wh = color_pipe->trapcorr_data.wh_new; + + color_pipe->trapcorr_data.img_in = img_out; + color_pipe->trapcorr_data.is_color = is_color; + color_pipe->trapcorr_data.bit_channel = bit_channel; + color_pipe->trapcorr_data.width = width; + color_pipe->trapcorr_data.height = height; + + trapcorr(&(color_pipe->trapcorr_data)); + + img_out = color_pipe->trapcorr_data.img_out; + } +#ifdef DEBUG_PROC_TIME + ts_trapcorr = get_ts(); +#endif // DEBUG_PROC_TIME + + + /* * Return processed image depending on active pipeline stages. */ @@ -692,6 +732,7 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe, printf(" color correction: %lld msec\n", ts_ccm - ts_calib); printf(" sharpening: %lld msec\n", ts_sharp - ts_ccm); printf(" gamma: %lld msec\n", ts_gamma - ts_sharp); + printf(" trapeze correction: %lld msec\n", ts_trapcorr - ts_sharp); #endif // DEBUG_PROC_TIME } @@ -833,6 +874,42 @@ void __stdcall color_pipe_stageconf_gamma(struct color_pipe_t *color_pipe, int e } +/** + * Pipeline stage configuration: Isosceles Trapeze Correction + * + * The vertical and horizontal correction weight are positive + * numbers starting from 0. A weight of 1.0 means no correction. A weight lesser than + * 1.0 means the upper trapeze line is shorter than the lower trapeze line. + * Therfore a weight greater than 1.0 means the upper trapeze line is longer than + * the lower line. + * + * @param color_pipe Pointer to pipeline context + * @param enable not 0: enable, 0: disable + * @param wv vertical weight + * @param wh horizontal weight + */ +void __stdcall color_pipe_stageconf_trapcorr(struct color_pipe_t *color_pipe, int enable, float wv, float wh) { + // paranoia + if(color_pipe == NULL) { + printf("%s: Pipeline pointer is NULL!\n", __func__); + return; + } + + // range check + if(wv < 0.0f) { + wv = 0.0f; + } + if(wh < 0.0f) { + wh = 0.0f; + } + + color_pipe->trapcorr_data.enable = enable; + color_pipe->trapcorr_data.wv_new = wv; + color_pipe->trapcorr_data.wh_new = wh; + color_pipe->trapcorr_data.map_init = 0; +} + + /** * Open color image processing pipeline. * This function allocates memory for various pipe algorithm. The pipeline is set up for a maximum possible image size defined @@ -927,7 +1004,7 @@ int __stdcall color_pipe_open(struct color_pipe_t **color_pipe, const int max_im goto _pipe_open_abort; } data->cam_calib_data.calib_map = do_aligned_alloc(ALIGNMENT_SIZE, - sizeof(struct lense_undistort_coord_t)*max_img_height*max_img_width, + sizeof(struct coord_t)*max_img_height*max_img_width, __func__, __LINE__-1); if(data->cam_calib_data.calib_map == NULL) { goto _pipe_open_abort; @@ -977,6 +1054,19 @@ int __stdcall color_pipe_open(struct color_pipe_t **color_pipe, const int max_im goto _pipe_open_abort; } + // allocate memory for isosceles trapeze correction algorithm + data->trapcorr_data.img_out = do_aligned_alloc(ALIGNMENT_SIZE, max_img_size, __func__, __LINE__-1); + if(data->trapcorr_data.img_out == NULL) { + goto _pipe_open_abort; + } + data->trapcorr_data.map = do_aligned_alloc(ALIGNMENT_SIZE, + sizeof(struct coord_t)*max_img_height*max_img_width, + __func__, __LINE__-1); + if(data->trapcorr_data.map == NULL) { + goto _pipe_open_abort; + } + + // set suitable and valid defaults set_default_value(data); *color_pipe = data; @@ -1022,6 +1112,8 @@ int __stdcall color_pipe_close(struct color_pipe_t *data) { do_aligned_free(data->sharp_data.sharp_mask); do_aligned_free(data->gamma_data.img_gamma); do_aligned_free(data->gamma_data.gamma_table); + do_aligned_free(data->trapcorr_data.img_out); + do_aligned_free(data->trapcorr_data.map); // free various image buffers free(data); diff --git a/color_pipe.h b/color_pipe.h index 2833d0f..b4c50da 100644 --- a/color_pipe.h +++ b/color_pipe.h @@ -2,9 +2,7 @@ * @file color_pipe.h * @brief Color Processing Pipeline Definitions * @author Patrick Roth - roth@stettbacher.ch -* @version 1.0 -* @date 2016-03-01 -* @copyright 2012-2016 Stettbacher Signal Processing AG +* @copyright Stettbacher Signal Processing AG * * @remarks * @@ -35,6 +33,15 @@ #include +/** + * Coordinate definition. + */ +struct coord_t { + int x; ///< x-coordinate of calibrated pixel + int y; ///< y-coordinate of calibrated pixel +}; + + /** * demosaicing algorithm definition */ @@ -55,7 +62,7 @@ struct debayer_data_t { enum enumDataFormat_t format; ///< data format of input image enum enumBayerPattern_t start_pattern; ///< first pixel starts with this bayer pattern enum bayer_alg_t alg; ///< debayer algorithm type - enum bayer_alg_t alg_new; ///< this debayer algorithm type is changed by API call (use double buffering concept) + enum bayer_alg_t alg_new; ///< this debayer algorithm type is changed by API call (use double buffering) }; @@ -71,9 +78,9 @@ struct awb_data_t { int width; ///< image width in number of pixels int16_t *img_yuv; ///< Image buffer holding YUV image. This buffer must be allocated externly. float gray_threshold; ///< gray value threshold - float gray_threshold_new; ///< this gray value threshold is changed by API call (use double buffering concept) + float gray_threshold_new; ///< this gray value threshold is changed by API call (use double buffering) float ctrl_k; ///< controller gain - float ctrl_k_new; ///< this controller gain is changed by API call (use double buffering concept) + float ctrl_k_new; ///< this controller gain is changed by API call (use double buffering) float gain_red; ///< red color gain float gain_blue; ///< blue color gain }; @@ -130,19 +137,6 @@ enum o3000_lenses_t { }; -/** - * Lense undistortion coordinate definition. - * This coordinate pair defines the undistorted pixel location. - * - * NOTE The pixel location may lay between a pixel pair. Therefore this coordinates are - * scaled by a defined factor defined at @ref cam_calib_data_t. - */ -struct lense_undistort_coord_t { - int coord_x; ///< x-coordinate of calibrated pixel - int coord_y; ///< y-coordinate of calibrated pixel -}; - - /** * camera calibration structure * @@ -164,10 +158,23 @@ struct cam_calib_data_t { struct dist_coeff_t dist_coeff; ///< distortion coefficients struct camera_matrix_t camera_matrix; ///< camera matrix enum o3000_lenses_t lense; ///< lense type - enum o3000_lenses_t lense_new; ///< lense type is changed by API call (use double buffering concept) + enum o3000_lenses_t lense_new; ///< lense type is changed by API call (use double buffering) int undistort_map_init; ///< flag indicating if lens undistortion map is initialized - struct lense_undistort_coord_t *calib_map; ///< Lense undistortion map. This buffer must be allocated externly. - int calib_map_scale_fact; ///< Bit shifts applied on undistortion map. + + /** + * Lense undistortion map. + * A coordinate pair defines the undistorted pixel location. It may be shifted (see @ref calib_map_scale_fact). + * + * This buffer must be allocated externly. + */ + struct coord_t *calib_map; + + /** + * Bit shift applied on undistortion map @ref calib_map. + * The pixel location may lay between a pixel pair. Therefore the coordinates defined at the undistortion map + * @ref calib_map are (scaled) shifted by this factor. + */ + int calib_map_scale_fact; }; @@ -218,7 +225,7 @@ struct color_calib_data_t { int height; ///< image height in number of pixels float a[3][3]; ///< 3x3 color correction matrix enum ccm_preset_t ccm; ///< color correction matrix type loaded - enum ccm_preset_t ccm_new; ///< this color correction matrix type is changed by API call (use double buffering concept) + enum ccm_preset_t ccm_new; ///< this color correction matrix type is changed by API call (use double buffering) }; @@ -244,11 +251,11 @@ struct sharp_data_t { int height; ///< image height in number of pixels int16_t *img_yuv; ///< YUV image buffer. This buffer must be allocated externly. float sharp_factor; ///< Sharpening factor: As higher as stronger sharpening is done. - float sharp_factor_new; ///< this sharpening factor is changed by API call (use double buffering concept) + float sharp_factor_new; ///< this sharpening factor is changed by API call (use double buffering) enum sharp_alg_t sharp_alg; ///< sharpening algorithm type - enum sharp_alg_t sharp_alg_new; ///< this algorithm type is changed by API call (use double buffering concept) + enum sharp_alg_t sharp_alg_new; ///< this algorithm type is changed by API call (use double buffering) float local_sens; ///< Sensitivity setting of local sharpening algorithm in per cent. 100 % means everything is sharpened like the global algorithm does - float local_sens_new; ///< this sensitivity setting is changed by API call (use double buffering concept) + float local_sens_new; ///< this sensitivity setting is changed by API call (use double buffering) int16_t *img_yuv_sharp; ///< YUV image buffer holding sharpened Y-channel. This buffer must be allocated externly. int16_t *img_sobel; ///< Sobel image in YUV color space used by local sharpening algorithm. This buffer must be allocated externly. int16_t *img_gauss; ///< Gaussian low-pass filtered image in YUV color space used by local sharpening algorithm. This buffer must be allocated externly. @@ -268,13 +275,48 @@ struct gamma_data_t { int width; ///< image width in number of pixels int height; ///< image height in number of pixels float gamma; ///< gamma coefficient - float gamma_new; ///< this gamma coefficient is changed by API call (use double buffering concept) + float gamma_new; ///< this gamma coefficient is changed by API call (use double buffering) int gamma_table_bitdepth; ///< gamma table is initialized with this bit-depth float gamma_table_init; ///< gamma table is initialized with this coefficient int *gamma_table; ///< Lookup table containg gamma corrected value. This buffer must be allocated externly. }; +/** + * Isosceles trapezoid correction definition + */ +struct trapcorr_data_t { + int enable; ///< flag to enable this algorithm + void *img_out; ///< Isosceles corrected output image. This buffer must be allocated externly. + void *img_in; ///< Input image. + int is_color; ///< Not 0 if it's a color image + int bit_channel; ///< Bits per color channel. + int width; ///< image width in number of pixels + int height; ///< image height in number of pixels + float wv; ///< vertical correction weight in per cent (NOTE not implemented yet!) + float wh; ///< horizontal correction weight in per cent + float wv_new; ///< this vertical correction weight in per cent is changed by API call (use double buffering) (NOTE not implemented yet!) + float wh_new; ///< this horizontal correction weight in per cent is changed by API call (use double buffering) + + int map_init; ///< flag indicating perspective correction map is initialized + + /** + * perspective correction map + * A coordinate pair defines the pixel location to be corrected. It may be shifted (see @ref map_scale_fact). + * + * This buffer must be allocated externly. + */ + struct coord_t *map; + + /** + * Bit shift applied on correction map @ref map. + * The pixel location may lay between a pixel pair. Therefore the coordinates defined at the correction map + * @ref map are (scaled) shifted by this factor. + */ + int map_scale_fact; +}; + + /** * Color pipe definition structure holding all memory data from different pipeline * stages. This structure and image buffers are allocated dynamically. @@ -293,6 +335,7 @@ struct color_pipe_t { struct color_calib_data_t color_calib_data; ///< color calibration data used for color corretion struct sharp_data_t sharp_data; ///< image sharpening data struct gamma_data_t gamma_data; ///< gamma correction data + struct trapcorr_data_t trapcorr_data; ///< isosceles trapezoid correction data }; @@ -312,6 +355,7 @@ void __stdcall color_pipe_stageconf_cam_calib(struct color_pipe_t *color_pipe, i void __stdcall color_pipe_stageconf_color_calib(struct color_pipe_t *color_pipe, int enable, enum ccm_preset_t ccm_preset); void __stdcall color_pipe_stageconf_sharp(struct color_pipe_t *color_pipe, int enable, float factor, enum sharp_alg_t alg, float sens); void __stdcall color_pipe_stageconf_gamma(struct color_pipe_t *color_pipe, int enable, float gamma); +void __stdcall color_pipe_stageconf_trapcorr(struct color_pipe_t *color_pipe, int enable, float wv, float wh); #if defined(__cplusplus) || defined(c_plusplus) } // extern "C" diff --git a/color_pipe_private.h b/color_pipe_private.h index f5a48f8..b2954ac 100644 --- a/color_pipe_private.h +++ b/color_pipe_private.h @@ -47,6 +47,8 @@ int sharpening(struct sharp_data_t *sharp_data); int gamma_corr(struct gamma_data_t *gamma_data); +int trapcorr(struct trapcorr_data_t *trapcorr_data); + #if defined(__cplusplus) || defined(c_plusplus) } // extern "C" #endif 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 +* +* 
+* 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
+*
+* +*/ + +#include +#include +#include + +#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; +} -- cgit v1.2.1