aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--camera_calib.c12
-rw-r--r--color_pipe.c116
-rw-r--r--color_pipe.h98
-rw-r--r--color_pipe_private.h2
-rw-r--r--trapcorr.c314
5 files changed, 497 insertions, 45 deletions
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
/*
@@ -676,6 +681,41 @@ void __stdcall color_pipe_process(struct color_pipe_t *__restrict__ color_pipe,
/*
+ * 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.
*/
color_pipe->img_out = img_out;
@@ -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
}
@@ -834,6 +875,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
* by the height, width and bitdepth per color channel.
@@ -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
*
@@ -36,6 +34,15 @@
/**
+ * Coordinate definition.
+ */
+struct coord_t {
+ int x; ///< x-coordinate of calibrated pixel
+ int y; ///< y-coordinate of calibrated pixel
+};
+
+
+/**
* demosaicing algorithm definition
*/
enum bayer_alg_t {
@@ -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
};
@@ -131,19 +138,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
*
* The total width @ref tot_width and height @ref tot_height define the image size take during calibration processs.
@@ -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,7 +275,7 @@ 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.
@@ -276,6 +283,41 @@ struct gamma_data_t {
/**
+ * 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
+*
+* <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;
+}