diff options
Diffstat (limited to 'cv/src/cvimgwarp.cpp')
| -rw-r--r-- | cv/src/cvimgwarp.cpp | 2272 |
1 files changed, 2272 insertions, 0 deletions
diff --git a/cv/src/cvimgwarp.cpp b/cv/src/cvimgwarp.cpp new file mode 100644 index 0000000..65c0d8f --- /dev/null +++ b/cv/src/cvimgwarp.cpp @@ -0,0 +1,2272 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// Intel License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of Intel Corporation may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +/* //////////////////////////////////////////////////////////////////// +// +// Geometrical transforms on images and matrices: rotation, zoom etc. +// +// */ + +#include "_cv.h" + + +/************** interpolation constants and tables ***************/ + +#define ICV_WARP_MUL_ONE_8U(x) ((x) << ICV_WARP_SHIFT) +#define ICV_WARP_DESCALE_8U(x) CV_DESCALE((x), ICV_WARP_SHIFT*2) +#define ICV_WARP_CLIP_X(x) ((unsigned)(x) < (unsigned)ssize.width ? \ + (x) : (x) < 0 ? 0 : ssize.width - 1) +#define ICV_WARP_CLIP_Y(y) ((unsigned)(y) < (unsigned)ssize.height ? \ + (y) : (y) < 0 ? 0 : ssize.height - 1) + +float icvLinearCoeffs[(ICV_LINEAR_TAB_SIZE+1)*2]; + +void icvInitLinearCoeffTab() +{ + static int inittab = 0; + if( !inittab ) + { + for( int i = 0; i <= ICV_LINEAR_TAB_SIZE; i++ ) + { + float x = (float)i/ICV_LINEAR_TAB_SIZE; + icvLinearCoeffs[i*2] = x; + icvLinearCoeffs[i*2+1] = 1.f - x; + } + + inittab = 1; + } +} + + +float icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE+1)*2]; + +void icvInitCubicCoeffTab() +{ + static int inittab = 0; + if( !inittab ) + { +#if 0 + // classical Mitchell-Netravali filter + const double B = 1./3; + const double C = 1./3; + const double p0 = (6 - 2*B)/6.; + const double p2 = (-18 + 12*B + 6*C)/6.; + const double p3 = (12 - 9*B - 6*C)/6.; + const double q0 = (8*B + 24*C)/6.; + const double q1 = (-12*B - 48*C)/6.; + const double q2 = (6*B + 30*C)/6.; + const double q3 = (-B - 6*C)/6.; + + #define ICV_CUBIC_1(x) (((x)*p3 + p2)*(x)*(x) + p0) + #define ICV_CUBIC_2(x) ((((x)*q3 + q2)*(x) + q1)*(x) + q0) +#else + // alternative "sharp" filter + const double A = -0.75; + #define ICV_CUBIC_1(x) (((A + 2)*(x) - (A + 3))*(x)*(x) + 1) + #define ICV_CUBIC_2(x) (((A*(x) - 5*A)*(x) + 8*A)*(x) - 4*A) +#endif + for( int i = 0; i <= ICV_CUBIC_TAB_SIZE; i++ ) + { + float x = (float)i/ICV_CUBIC_TAB_SIZE; + icvCubicCoeffs[i*2] = (float)ICV_CUBIC_1(x); + x += 1.f; + icvCubicCoeffs[i*2+1] = (float)ICV_CUBIC_2(x); + } + + inittab = 1; + } +} + + +/****************************************************************************************\ +* Resize * +\****************************************************************************************/ + +static CvStatus CV_STDCALL +icvResize_NN_8u_C1R( const uchar* src, int srcstep, CvSize ssize, + uchar* dst, int dststep, CvSize dsize, int pix_size ) +{ + int* x_ofs = (int*)cvStackAlloc( dsize.width * sizeof(x_ofs[0]) ); + int pix_size4 = pix_size / sizeof(int); + int x, y, t; + + for( x = 0; x < dsize.width; x++ ) + { + t = (ssize.width*x*2 + MIN(ssize.width, dsize.width) - 1)/(dsize.width*2); + t -= t >= ssize.width; + x_ofs[x] = t*pix_size; + } + + for( y = 0; y < dsize.height; y++, dst += dststep ) + { + const uchar* tsrc; + t = (ssize.height*y*2 + MIN(ssize.height, dsize.height) - 1)/(dsize.height*2); + t -= t >= ssize.height; + tsrc = src + srcstep*t; + + switch( pix_size ) + { + case 1: + for( x = 0; x <= dsize.width - 2; x += 2 ) + { + uchar t0 = tsrc[x_ofs[x]]; + uchar t1 = tsrc[x_ofs[x+1]]; + + dst[x] = t0; + dst[x+1] = t1; + } + + for( ; x < dsize.width; x++ ) + dst[x] = tsrc[x_ofs[x]]; + break; + case 2: + for( x = 0; x < dsize.width; x++ ) + *(ushort*)(dst + x*2) = *(ushort*)(tsrc + x_ofs[x]); + break; + case 3: + for( x = 0; x < dsize.width; x++ ) + { + const uchar* _tsrc = tsrc + x_ofs[x]; + dst[x*3] = _tsrc[0]; dst[x*3+1] = _tsrc[1]; dst[x*3+2] = _tsrc[2]; + } + break; + case 4: + for( x = 0; x < dsize.width; x++ ) + *(int*)(dst + x*4) = *(int*)(tsrc + x_ofs[x]); + break; + case 6: + for( x = 0; x < dsize.width; x++ ) + { + const ushort* _tsrc = (const ushort*)(tsrc + x_ofs[x]); + ushort* _tdst = (ushort*)(dst + x*6); + _tdst[0] = _tsrc[0]; _tdst[1] = _tsrc[1]; _tdst[2] = _tsrc[2]; + } + break; + default: + for( x = 0; x < dsize.width; x++ ) + CV_MEMCPY_INT( dst + x*pix_size, tsrc + x_ofs[x], pix_size4 ); + } + } + + return CV_OK; +} + + +typedef struct CvResizeAlpha +{ + int idx; + union + { + float alpha; + int ialpha; + }; +} +CvResizeAlpha; + + +#define ICV_DEF_RESIZE_BILINEAR_FUNC( flavor, arrtype, worktype, alpha_field, \ + mul_one_macro, descale_macro ) \ +static CvStatus CV_STDCALL \ +icvResize_Bilinear_##flavor##_CnR( const arrtype* src, int srcstep, CvSize ssize,\ + arrtype* dst, int dststep, CvSize dsize, \ + int cn, int xmax, \ + const CvResizeAlpha* xofs, \ + const CvResizeAlpha* yofs, \ + worktype* buf0, worktype* buf1 ) \ +{ \ + int prev_sy0 = -1, prev_sy1 = -1; \ + int k, dx, dy; \ + \ + srcstep /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + dsize.width *= cn; \ + xmax *= cn; \ + \ + for( dy = 0; dy < dsize.height; dy++, dst += dststep ) \ + { \ + worktype fy = yofs[dy].alpha_field, *swap_t; \ + int sy0 = yofs[dy].idx, sy1 = sy0 + (fy > 0 && sy0 < ssize.height-1); \ + \ + if( sy0 == prev_sy0 && sy1 == prev_sy1 ) \ + k = 2; \ + else if( sy0 == prev_sy1 ) \ + { \ + CV_SWAP( buf0, buf1, swap_t ); \ + k = 1; \ + } \ + else \ + k = 0; \ + \ + for( ; k < 2; k++ ) \ + { \ + worktype* _buf = k == 0 ? buf0 : buf1; \ + const arrtype* _src; \ + int sy = k == 0 ? sy0 : sy1; \ + if( k == 1 && sy1 == sy0 ) \ + { \ + memcpy( buf1, buf0, dsize.width*sizeof(buf0[0]) ); \ + continue; \ + } \ + \ + _src = src + sy*srcstep; \ + for( dx = 0; dx < xmax; dx++ ) \ + { \ + int sx = xofs[dx].idx; \ + worktype fx = xofs[dx].alpha_field; \ + worktype t = _src[sx]; \ + _buf[dx] = mul_one_macro(t) + fx*(_src[sx+cn] - t); \ + } \ + \ + for( ; dx < dsize.width; dx++ ) \ + _buf[dx] = mul_one_macro(_src[xofs[dx].idx]); \ + } \ + \ + prev_sy0 = sy0; \ + prev_sy1 = sy1; \ + \ + if( sy0 == sy1 ) \ + for( dx = 0; dx < dsize.width; dx++ ) \ + dst[dx] = (arrtype)descale_macro( mul_one_macro(buf0[dx])); \ + else \ + for( dx = 0; dx < dsize.width; dx++ ) \ + dst[dx] = (arrtype)descale_macro( mul_one_macro(buf0[dx]) + \ + fy*(buf1[dx] - buf0[dx])); \ + } \ + \ + return CV_OK; \ +} + + +typedef struct CvDecimateAlpha +{ + int si, di; + float alpha; +} +CvDecimateAlpha; + + +#define ICV_DEF_RESIZE_AREA_FAST_FUNC( flavor, arrtype, worktype, cast_macro ) \ +static CvStatus CV_STDCALL \ +icvResize_AreaFast_##flavor##_CnR( const arrtype* src, int srcstep, CvSize ssize,\ + arrtype* dst, int dststep, CvSize dsize, int cn, \ + const int* ofs, const int* xofs ) \ +{ \ + int dy, dx, k = 0; \ + int scale_x = ssize.width/dsize.width; \ + int scale_y = ssize.height/dsize.height; \ + int area = scale_x*scale_y; \ + float scale = 1.f/(scale_x*scale_y); \ + \ + srcstep /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + dsize.width *= cn; \ + \ + for( dy = 0; dy < dsize.height; dy++, dst += dststep ) \ + for( dx = 0; dx < dsize.width; dx++ ) \ + { \ + const arrtype* _src = src + dy*scale_y*srcstep + xofs[dx]; \ + worktype sum = 0; \ + \ + for( k = 0; k <= area - 4; k += 4 ) \ + sum += _src[ofs[k]] + _src[ofs[k+1]] + \ + _src[ofs[k+2]] + _src[ofs[k+3]]; \ + \ + for( ; k < area; k++ ) \ + sum += _src[ofs[k]]; \ + \ + dst[dx] = (arrtype)cast_macro( sum*scale ); \ + } \ + \ + return CV_OK; \ +} + + +#define ICV_DEF_RESIZE_AREA_FUNC( flavor, arrtype, load_macro, cast_macro ) \ +static CvStatus CV_STDCALL \ +icvResize_Area_##flavor##_CnR( const arrtype* src, int srcstep, CvSize ssize, \ + arrtype* dst, int dststep, CvSize dsize, \ + int cn, const CvDecimateAlpha* xofs, \ + int xofs_count, float* buf, float* sum ) \ +{ \ + int k, sy, dx, cur_dy = 0; \ + float scale_y = (float)ssize.height/dsize.height; \ + \ + srcstep /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + dsize.width *= cn; \ + \ + for( sy = 0; sy < ssize.height; sy++, src += srcstep ) \ + { \ + if( cn == 1 ) \ + for( k = 0; k < xofs_count; k++ ) \ + { \ + int dxn = xofs[k].di; \ + float alpha = xofs[k].alpha; \ + buf[dxn] = buf[dxn] + load_macro(src[xofs[k].si])*alpha; \ + } \ + else if( cn == 2 ) \ + for( k = 0; k < xofs_count; k++ ) \ + { \ + int sxn = xofs[k].si; \ + int dxn = xofs[k].di; \ + float alpha = xofs[k].alpha; \ + float t0 = buf[dxn] + load_macro(src[sxn])*alpha; \ + float t1 = buf[dxn+1] + load_macro(src[sxn+1])*alpha; \ + buf[dxn] = t0; buf[dxn+1] = t1; \ + } \ + else if( cn == 3 ) \ + for( k = 0; k < xofs_count; k++ ) \ + { \ + int sxn = xofs[k].si; \ + int dxn = xofs[k].di; \ + float alpha = xofs[k].alpha; \ + float t0 = buf[dxn] + load_macro(src[sxn])*alpha; \ + float t1 = buf[dxn+1] + load_macro(src[sxn+1])*alpha; \ + float t2 = buf[dxn+2] + load_macro(src[sxn+2])*alpha; \ + buf[dxn] = t0; buf[dxn+1] = t1; buf[dxn+2] = t2; \ + } \ + else \ + for( k = 0; k < xofs_count; k++ ) \ + { \ + int sxn = xofs[k].si; \ + int dxn = xofs[k].di; \ + float alpha = xofs[k].alpha; \ + float t0 = buf[dxn] + load_macro(src[sxn])*alpha; \ + float t1 = buf[dxn+1] + load_macro(src[sxn+1])*alpha; \ + buf[dxn] = t0; buf[dxn+1] = t1; \ + t0 = buf[dxn+2] + load_macro(src[sxn+2])*alpha; \ + t1 = buf[dxn+3] + load_macro(src[sxn+3])*alpha; \ + buf[dxn+2] = t0; buf[dxn+3] = t1; \ + } \ + \ + if( (cur_dy + 1)*scale_y <= sy + 1 || sy == ssize.height - 1 ) \ + { \ + float beta = sy + 1 - (cur_dy+1)*scale_y, beta1; \ + beta = MAX( beta, 0 ); \ + beta1 = 1 - beta; \ + if( fabs(beta) < 1e-3 ) \ + for( dx = 0; dx < dsize.width; dx++ ) \ + { \ + dst[dx] = (arrtype)cast_macro(sum[dx] + buf[dx]); \ + sum[dx] = buf[dx] = 0; \ + } \ + else \ + for( dx = 0; dx < dsize.width; dx++ ) \ + { \ + dst[dx] = (arrtype)cast_macro(sum[dx] + buf[dx]*beta1); \ + sum[dx] = buf[dx]*beta; \ + buf[dx] = 0; \ + } \ + dst += dststep; \ + cur_dy++; \ + } \ + else \ + for( dx = 0; dx < dsize.width; dx += 2 ) \ + { \ + float t0 = sum[dx] + buf[dx]; \ + float t1 = sum[dx+1] + buf[dx+1]; \ + sum[dx] = t0; sum[dx+1] = t1; \ + buf[dx] = buf[dx+1] = 0; \ + } \ + } \ + \ + return CV_OK; \ +} + + +#define ICV_DEF_RESIZE_BICUBIC_FUNC( flavor, arrtype, worktype, load_macro, \ + cast_macro1, cast_macro2 ) \ +static CvStatus CV_STDCALL \ +icvResize_Bicubic_##flavor##_CnR( const arrtype* src, int srcstep, CvSize ssize,\ + arrtype* dst, int dststep, CvSize dsize, \ + int cn, int xmin, int xmax, \ + const CvResizeAlpha* xofs, float** buf ) \ +{ \ + float scale_y = (float)ssize.height/dsize.height; \ + int dx, dy, sx, sy, sy2, ify; \ + int prev_sy2 = -2; \ + \ + xmin *= cn; xmax *= cn; \ + dsize.width *= cn; \ + ssize.width *= cn; \ + srcstep /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + \ + for( dy = 0; dy < dsize.height; dy++, dst += dststep ) \ + { \ + float w0, w1, w2, w3; \ + float fy, x, sum; \ + float *row, *row0, *row1, *row2, *row3; \ + int k1, k = 4; \ + \ + fy = dy*scale_y; \ + sy = cvFloor(fy); \ + fy -= sy; \ + ify = cvRound(fy*ICV_CUBIC_TAB_SIZE); \ + sy2 = sy + 2; \ + \ + if( sy2 > prev_sy2 ) \ + { \ + int delta = prev_sy2 - sy + 2; \ + for( k = 0; k < delta; k++ ) \ + CV_SWAP( buf[k], buf[k+4-delta], row ); \ + } \ + \ + for( sy += k - 1; k < 4; k++, sy++ ) \ + { \ + const arrtype* _src = src + sy*srcstep; \ + \ + row = buf[k]; \ + if( sy < 0 ) \ + continue; \ + if( sy >= ssize.height ) \ + { \ + assert( k > 0 ); \ + memcpy( row, buf[k-1], dsize.width*sizeof(row[0]) ); \ + continue; \ + } \ + \ + for( dx = 0; dx < xmin; dx++ ) \ + { \ + int ifx = xofs[dx].ialpha, sx0 = xofs[dx].idx; \ + sx = sx0 + cn*2; \ + while( sx >= ssize.width ) \ + sx -= cn; \ + x = load_macro(_src[sx]); \ + sum = x*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE - ifx)*2 + 1]; \ + if( (unsigned)(sx = sx0 + cn) < (unsigned)ssize.width ) \ + x = load_macro(_src[sx]); \ + sum += x*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE - ifx)*2]; \ + if( (unsigned)(sx = sx0) < (unsigned)ssize.width ) \ + x = load_macro(_src[sx]); \ + sum += x*icvCubicCoeffs[ifx*2]; \ + if( (unsigned)(sx = sx0 - cn) < (unsigned)ssize.width ) \ + x = load_macro(_src[sx]); \ + row[dx] = sum + x*icvCubicCoeffs[ifx*2 + 1]; \ + } \ + \ + for( ; dx < xmax; dx++ ) \ + { \ + int ifx = xofs[dx].ialpha; \ + int sx0 = xofs[dx].idx; \ + row[dx] = _src[sx0 - cn]*icvCubicCoeffs[ifx*2 + 1] + \ + _src[sx0]*icvCubicCoeffs[ifx*2] + \ + _src[sx0 + cn]*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2] + \ + _src[sx0 + cn*2]*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2+1];\ + } \ + \ + for( ; dx < dsize.width; dx++ ) \ + { \ + int ifx = xofs[dx].ialpha, sx0 = xofs[dx].idx; \ + x = load_macro(_src[sx0 - cn]); \ + sum = x*icvCubicCoeffs[ifx*2 + 1]; \ + if( (unsigned)(sx = sx0) < (unsigned)ssize.width ) \ + x = load_macro(_src[sx]); \ + sum += x*icvCubicCoeffs[ifx*2]; \ + if( (unsigned)(sx = sx0 + cn) < (unsigned)ssize.width ) \ + x = load_macro(_src[sx]); \ + sum += x*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE - ifx)*2]; \ + if( (unsigned)(sx = sx0 + cn*2) < (unsigned)ssize.width ) \ + x = load_macro(_src[sx]); \ + row[dx] = sum + x*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2+1]; \ + } \ + \ + if( sy == 0 ) \ + for( k1 = 0; k1 < k; k1++ ) \ + memcpy( buf[k1], row, dsize.width*sizeof(row[0])); \ + } \ + \ + prev_sy2 = sy2; \ + \ + row0 = buf[0]; row1 = buf[1]; \ + row2 = buf[2]; row3 = buf[3]; \ + \ + w0 = icvCubicCoeffs[ify*2+1]; \ + w1 = icvCubicCoeffs[ify*2]; \ + w2 = icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE - ify)*2]; \ + w3 = icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE - ify)*2 + 1]; \ + \ + for( dx = 0; dx < dsize.width; dx++ ) \ + { \ + worktype val = cast_macro1( row0[dx]*w0 + row1[dx]*w1 + \ + row2[dx]*w2 + row3[dx]*w3 ); \ + dst[dx] = cast_macro2(val); \ + } \ + } \ + \ + return CV_OK; \ +} + + +ICV_DEF_RESIZE_BILINEAR_FUNC( 8u, uchar, int, ialpha, + ICV_WARP_MUL_ONE_8U, ICV_WARP_DESCALE_8U ) +ICV_DEF_RESIZE_BILINEAR_FUNC( 16u, ushort, float, alpha, CV_NOP, cvRound ) +ICV_DEF_RESIZE_BILINEAR_FUNC( 32f, float, float, alpha, CV_NOP, CV_NOP ) + +ICV_DEF_RESIZE_BICUBIC_FUNC( 8u, uchar, int, CV_8TO32F, cvRound, CV_CAST_8U ) +ICV_DEF_RESIZE_BICUBIC_FUNC( 16u, ushort, int, CV_NOP, cvRound, CV_CAST_16U ) +ICV_DEF_RESIZE_BICUBIC_FUNC( 32f, float, float, CV_NOP, CV_NOP, CV_NOP ) + +ICV_DEF_RESIZE_AREA_FAST_FUNC( 8u, uchar, int, cvRound ) +ICV_DEF_RESIZE_AREA_FAST_FUNC( 16u, ushort, int, cvRound ) +ICV_DEF_RESIZE_AREA_FAST_FUNC( 32f, float, float, CV_NOP ) + +ICV_DEF_RESIZE_AREA_FUNC( 8u, uchar, CV_8TO32F, cvRound ) +ICV_DEF_RESIZE_AREA_FUNC( 16u, ushort, CV_NOP, cvRound ) +ICV_DEF_RESIZE_AREA_FUNC( 32f, float, CV_NOP, CV_NOP ) + + +static void icvInitResizeTab( CvFuncTable* bilin_tab, + CvFuncTable* bicube_tab, + CvFuncTable* areafast_tab, + CvFuncTable* area_tab ) +{ + bilin_tab->fn_2d[CV_8U] = (void*)icvResize_Bilinear_8u_CnR; + bilin_tab->fn_2d[CV_16U] = (void*)icvResize_Bilinear_16u_CnR; + bilin_tab->fn_2d[CV_32F] = (void*)icvResize_Bilinear_32f_CnR; + + bicube_tab->fn_2d[CV_8U] = (void*)icvResize_Bicubic_8u_CnR; + bicube_tab->fn_2d[CV_16U] = (void*)icvResize_Bicubic_16u_CnR; + bicube_tab->fn_2d[CV_32F] = (void*)icvResize_Bicubic_32f_CnR; + + areafast_tab->fn_2d[CV_8U] = (void*)icvResize_AreaFast_8u_CnR; + areafast_tab->fn_2d[CV_16U] = (void*)icvResize_AreaFast_16u_CnR; + areafast_tab->fn_2d[CV_32F] = (void*)icvResize_AreaFast_32f_CnR; + + area_tab->fn_2d[CV_8U] = (void*)icvResize_Area_8u_CnR; + area_tab->fn_2d[CV_16U] = (void*)icvResize_Area_16u_CnR; + area_tab->fn_2d[CV_32F] = (void*)icvResize_Area_32f_CnR; +} + + +typedef CvStatus (CV_STDCALL * CvResizeBilinearFunc) + ( const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + int cn, int xmax, const CvResizeAlpha* xofs, + const CvResizeAlpha* yofs, float* buf0, float* buf1 ); + +typedef CvStatus (CV_STDCALL * CvResizeBicubicFunc) + ( const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + int cn, int xmin, int xmax, + const CvResizeAlpha* xofs, float** buf ); + +typedef CvStatus (CV_STDCALL * CvResizeAreaFastFunc) + ( const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + int cn, const int* ofs, const int *xofs ); + +typedef CvStatus (CV_STDCALL * CvResizeAreaFunc) + ( const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + int cn, const CvDecimateAlpha* xofs, + int xofs_count, float* buf, float* sum ); + + +////////////////////////////////// IPP resize functions ////////////////////////////////// + +icvResize_8u_C1R_t icvResize_8u_C1R_p = 0; +icvResize_8u_C3R_t icvResize_8u_C3R_p = 0; +icvResize_8u_C4R_t icvResize_8u_C4R_p = 0; +icvResize_16u_C1R_t icvResize_16u_C1R_p = 0; +icvResize_16u_C3R_t icvResize_16u_C3R_p = 0; +icvResize_16u_C4R_t icvResize_16u_C4R_p = 0; +icvResize_32f_C1R_t icvResize_32f_C1R_p = 0; +icvResize_32f_C3R_t icvResize_32f_C3R_p = 0; +icvResize_32f_C4R_t icvResize_32f_C4R_p = 0; + +typedef CvStatus (CV_STDCALL * CvResizeIPPFunc) +( const void* src, CvSize srcsize, int srcstep, CvRect srcroi, + void* dst, int dststep, CvSize dstroi, + double xfactor, double yfactor, int interpolation ); + +////////////////////////////////////////////////////////////////////////////////////////// + +CV_IMPL void +cvResize( const CvArr* srcarr, CvArr* dstarr, int method ) +{ + static CvFuncTable bilin_tab, bicube_tab, areafast_tab, area_tab; + static int inittab = 0; + void* temp_buf = 0; + + CV_FUNCNAME( "cvResize" ); + + __BEGIN__; + + CvMat srcstub, *src = (CvMat*)srcarr; + CvMat dststub, *dst = (CvMat*)dstarr; + CvSize ssize, dsize; + float scale_x, scale_y; + int k, sx, sy, dx, dy; + int type, depth, cn; + + CV_CALL( src = cvGetMat( srcarr, &srcstub )); + CV_CALL( dst = cvGetMat( dstarr, &dststub )); + + if( CV_ARE_SIZES_EQ( src, dst )) + { + CV_CALL( cvCopy( src, dst )); + EXIT; + } + + if( !CV_ARE_TYPES_EQ( src, dst )) + CV_ERROR( CV_StsUnmatchedFormats, "" ); + + if( !inittab ) + { + icvInitResizeTab( &bilin_tab, &bicube_tab, &areafast_tab, &area_tab ); + inittab = 1; + } + + ssize = cvGetMatSize( src ); + dsize = cvGetMatSize( dst ); + type = CV_MAT_TYPE(src->type); + depth = CV_MAT_DEPTH(type); + cn = CV_MAT_CN(type); + scale_x = (float)ssize.width/dsize.width; + scale_y = (float)ssize.height/dsize.height; + + if( method == CV_INTER_CUBIC && + (MIN(ssize.width, dsize.width) <= 4 || + MIN(ssize.height, dsize.height) <= 4) ) + method = CV_INTER_LINEAR; + + if( icvResize_8u_C1R_p && + MIN(ssize.width, dsize.width) > 4 && + MIN(ssize.height, dsize.height) > 4 ) + { + CvResizeIPPFunc ipp_func = + type == CV_8UC1 ? icvResize_8u_C1R_p : + type == CV_8UC3 ? icvResize_8u_C3R_p : + type == CV_8UC4 ? icvResize_8u_C4R_p : + type == CV_16UC1 ? icvResize_16u_C1R_p : + type == CV_16UC3 ? icvResize_16u_C3R_p : + type == CV_16UC4 ? icvResize_16u_C4R_p : + type == CV_32FC1 ? icvResize_32f_C1R_p : + type == CV_32FC3 ? icvResize_32f_C3R_p : + type == CV_32FC4 ? icvResize_32f_C4R_p : 0; + if( ipp_func && (CV_INTER_NN < method && method < CV_INTER_AREA)) + { + int srcstep = src->step ? src->step : CV_STUB_STEP; + int dststep = dst->step ? dst->step : CV_STUB_STEP; + IPPI_CALL( ipp_func( src->data.ptr, ssize, srcstep, + cvRect(0,0,ssize.width,ssize.height), + dst->data.ptr, dststep, dsize, + (double)dsize.width/ssize.width, + (double)dsize.height/ssize.height, 1 << method )); + EXIT; + } + } + + if( method == CV_INTER_NN ) + { + IPPI_CALL( icvResize_NN_8u_C1R( src->data.ptr, src->step, ssize, + dst->data.ptr, dst->step, dsize, + CV_ELEM_SIZE(src->type))); + } + else if( method == CV_INTER_LINEAR || method == CV_INTER_AREA ) + { + if( method == CV_INTER_AREA && + ssize.width >= dsize.width && ssize.height >= dsize.height ) + { + // "area" method for (scale_x > 1 & scale_y > 1) + int iscale_x = cvRound(scale_x); + int iscale_y = cvRound(scale_y); + + if( fabs(scale_x - iscale_x) < DBL_EPSILON && + fabs(scale_y - iscale_y) < DBL_EPSILON ) + { + int area = iscale_x*iscale_y; + int srcstep = src->step / CV_ELEM_SIZE(depth); + int* ofs = (int*)cvStackAlloc( (area + dsize.width*cn)*sizeof(int) ); + int* xofs = ofs + area; + CvResizeAreaFastFunc func = (CvResizeAreaFastFunc)areafast_tab.fn_2d[depth]; + + if( !func ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + for( sy = 0, k = 0; sy < iscale_y; sy++ ) + for( sx = 0; sx < iscale_x; sx++ ) + ofs[k++] = sy*srcstep + sx*cn; + + for( dx = 0; dx < dsize.width; dx++ ) + { + sx = dx*iscale_x*cn; + for( k = 0; k < cn; k++ ) + xofs[dx*cn + k] = sx + k; + } + + IPPI_CALL( func( src->data.ptr, src->step, ssize, dst->data.ptr, + dst->step, dsize, cn, ofs, xofs )); + } + else + { + int buf_len = dsize.width*cn + 4, buf_size, xofs_count = 0; + float scale = 1.f/(scale_x*scale_y); + float *buf, *sum; + CvDecimateAlpha* xofs; + CvResizeAreaFunc func = (CvResizeAreaFunc)area_tab.fn_2d[depth]; + + if( !func || cn > 4 ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + buf_size = buf_len*2*sizeof(float) + ssize.width*2*sizeof(CvDecimateAlpha); + if( buf_size < CV_MAX_LOCAL_SIZE ) + buf = (float*)cvStackAlloc(buf_size); + else + CV_CALL( temp_buf = buf = (float*)cvAlloc(buf_size)); + sum = buf + buf_len; + xofs = (CvDecimateAlpha*)(sum + buf_len); + + for( dx = 0, k = 0; dx < dsize.width; dx++ ) + { + float fsx1 = dx*scale_x, fsx2 = fsx1 + scale_x; + int sx1 = cvCeil(fsx1), sx2 = cvFloor(fsx2); + + assert( (unsigned)sx1 < (unsigned)ssize.width ); + + if( sx1 > fsx1 ) + { + assert( k < ssize.width*2 ); + xofs[k].di = dx*cn; + xofs[k].si = (sx1-1)*cn; + xofs[k++].alpha = (sx1 - fsx1)*scale; + } + + for( sx = sx1; sx < sx2; sx++ ) + { + assert( k < ssize.width*2 ); + xofs[k].di = dx*cn; + xofs[k].si = sx*cn; + xofs[k++].alpha = scale; + } + + if( fsx2 - sx2 > 1e-3 ) + { + assert( k < ssize.width*2 ); + assert((unsigned)sx2 < (unsigned)ssize.width ); + xofs[k].di = dx*cn; + xofs[k].si = sx2*cn; + xofs[k++].alpha = (fsx2 - sx2)*scale; + } + } + + xofs_count = k; + memset( sum, 0, buf_len*sizeof(float) ); + memset( buf, 0, buf_len*sizeof(float) ); + + IPPI_CALL( func( src->data.ptr, src->step, ssize, dst->data.ptr, + dst->step, dsize, cn, xofs, xofs_count, buf, sum )); + } + } + else // true "area" method for the cases (scale_x > 1 & scale_y < 1) and + // (scale_x < 1 & scale_y > 1) is not implemented. + // instead, it is emulated via some variant of bilinear interpolation. + { + float inv_scale_x = (float)dsize.width/ssize.width; + float inv_scale_y = (float)dsize.height/ssize.height; + int xmax = dsize.width, width = dsize.width*cn, buf_size; + float *buf0, *buf1; + CvResizeAlpha *xofs, *yofs; + int area_mode = method == CV_INTER_AREA; + float fx, fy; + CvResizeBilinearFunc func = (CvResizeBilinearFunc)bilin_tab.fn_2d[depth]; + + if( !func ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + buf_size = width*2*sizeof(float) + (width + dsize.height)*sizeof(CvResizeAlpha); + if( buf_size < CV_MAX_LOCAL_SIZE ) + buf0 = (float*)cvStackAlloc(buf_size); + else + CV_CALL( temp_buf = buf0 = (float*)cvAlloc(buf_size)); + buf1 = buf0 + width; + xofs = (CvResizeAlpha*)(buf1 + width); + yofs = xofs + width; + + for( dx = 0; dx < dsize.width; dx++ ) + { + if( !area_mode ) + { + fx = (float)((dx+0.5)*scale_x - 0.5); + sx = cvFloor(fx); + fx -= sx; + } + else + { + sx = cvFloor(dx*scale_x); + fx = (dx+1) - (sx+1)*inv_scale_x; + fx = fx <= 0 ? 0.f : fx - cvFloor(fx); + } + + if( sx < 0 ) + fx = 0, sx = 0; + + if( sx >= ssize.width-1 ) + { + fx = 0, sx = ssize.width-1; + if( xmax >= dsize.width ) + xmax = dx; + } + + if( depth != CV_8U ) + for( k = 0, sx *= cn; k < cn; k++ ) + xofs[dx*cn + k].idx = sx + k, xofs[dx*cn + k].alpha = fx; + else + for( k = 0, sx *= cn; k < cn; k++ ) + xofs[dx*cn + k].idx = sx + k, + xofs[dx*cn + k].ialpha = CV_FLT_TO_FIX(fx, ICV_WARP_SHIFT); + } + + for( dy = 0; dy < dsize.height; dy++ ) + { + if( !area_mode ) + { + fy = (float)((dy+0.5)*scale_y - 0.5); + sy = cvFloor(fy); + fy -= sy; + if( sy < 0 ) + sy = 0, fy = 0; + } + else + { + sy = cvFloor(dy*scale_y); + fy = (dy+1) - (sy+1)*inv_scale_y; + fy = fy <= 0 ? 0.f : fy - cvFloor(fy); + } + + yofs[dy].idx = sy; + if( depth != CV_8U ) + yofs[dy].alpha = fy; + else + yofs[dy].ialpha = CV_FLT_TO_FIX(fy, ICV_WARP_SHIFT); + } + + IPPI_CALL( func( src->data.ptr, src->step, ssize, dst->data.ptr, + dst->step, dsize, cn, xmax, xofs, yofs, buf0, buf1 )); + } + } + else if( method == CV_INTER_CUBIC ) + { + int width = dsize.width*cn, buf_size; + int xmin = dsize.width, xmax = -1; + CvResizeAlpha* xofs; + float* buf[4]; + CvResizeBicubicFunc func = (CvResizeBicubicFunc)bicube_tab.fn_2d[depth]; + + if( !func ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + buf_size = width*(4*sizeof(float) + sizeof(xofs[0])); + if( buf_size < CV_MAX_LOCAL_SIZE ) + buf[0] = (float*)cvStackAlloc(buf_size); + else + CV_CALL( temp_buf = buf[0] = (float*)cvAlloc(buf_size)); + + for( k = 1; k < 4; k++ ) + buf[k] = buf[k-1] + width; + xofs = (CvResizeAlpha*)(buf[3] + width); + + icvInitCubicCoeffTab(); + + for( dx = 0; dx < dsize.width; dx++ ) + { + float fx = dx*scale_x; + sx = cvFloor(fx); + fx -= sx; + int ifx = cvRound(fx*ICV_CUBIC_TAB_SIZE); + if( sx-1 >= 0 && xmin > dx ) + xmin = dx; + if( sx+2 < ssize.width ) + xmax = dx + 1; + + // at least one of 4 points should be within the image - to + // be able to set other points to the same value. see the loops + // for( dx = 0; dx < xmin; dx++ ) ... and for( ; dx < width; dx++ ) ... + if( sx < -2 ) + sx = -2; + else if( sx > ssize.width ) + sx = ssize.width; + + for( k = 0; k < cn; k++ ) + { + xofs[dx*cn + k].idx = sx*cn + k; + xofs[dx*cn + k].ialpha = ifx; + } + } + + IPPI_CALL( func( src->data.ptr, src->step, ssize, dst->data.ptr, + dst->step, dsize, cn, xmin, xmax, xofs, buf )); + } + else + CV_ERROR( CV_StsBadFlag, "Unknown/unsupported interpolation method" ); + + __END__; + + cvFree( &temp_buf ); +} + + +/****************************************************************************************\ +* WarpAffine * +\****************************************************************************************/ + +#define ICV_DEF_WARP_AFFINE_BILINEAR_FUNC( flavor, arrtype, worktype, \ + scale_alpha_macro, mul_one_macro, descale_macro, cast_macro ) \ +static CvStatus CV_STDCALL \ +icvWarpAffine_Bilinear_##flavor##_CnR( \ + const arrtype* src, int step, CvSize ssize, \ + arrtype* dst, int dststep, CvSize dsize, \ + const double* matrix, int cn, \ + const arrtype* fillval, const int* ofs ) \ +{ \ + int x, y, k; \ + double A12 = matrix[1], b1 = matrix[2]; \ + double A22 = matrix[4], b2 = matrix[5]; \ + \ + step /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + \ + for( y = 0; y < dsize.height; y++, dst += dststep ) \ + { \ + int xs = CV_FLT_TO_FIX( A12*y + b1, ICV_WARP_SHIFT ); \ + int ys = CV_FLT_TO_FIX( A22*y + b2, ICV_WARP_SHIFT ); \ + \ + for( x = 0; x < dsize.width; x++ ) \ + { \ + int ixs = xs + ofs[x*2]; \ + int iys = ys + ofs[x*2+1]; \ + worktype a = scale_alpha_macro( ixs & ICV_WARP_MASK ); \ + worktype b = scale_alpha_macro( iys & ICV_WARP_MASK ); \ + worktype p0, p1; \ + ixs >>= ICV_WARP_SHIFT; \ + iys >>= ICV_WARP_SHIFT; \ + \ + if( (unsigned)ixs < (unsigned)(ssize.width - 1) && \ + (unsigned)iys < (unsigned)(ssize.height - 1) ) \ + { \ + const arrtype* ptr = src + step*iys + ixs*cn; \ + \ + for( k = 0; k < cn; k++ ) \ + { \ + p0 = mul_one_macro(ptr[k]) + \ + a * (ptr[k+cn] - ptr[k]); \ + p1 = mul_one_macro(ptr[k+step]) + \ + a * (ptr[k+cn+step] - ptr[k+step]); \ + p0 = descale_macro(mul_one_macro(p0) + b*(p1 - p0)); \ + dst[x*cn+k] = (arrtype)cast_macro(p0); \ + } \ + } \ + else if( (unsigned)(ixs+1) < (unsigned)(ssize.width+1) && \ + (unsigned)(iys+1) < (unsigned)(ssize.height+1)) \ + { \ + int x0 = ICV_WARP_CLIP_X( ixs ); \ + int y0 = ICV_WARP_CLIP_Y( iys ); \ + int x1 = ICV_WARP_CLIP_X( ixs + 1 ); \ + int y1 = ICV_WARP_CLIP_Y( iys + 1 ); \ + const arrtype* ptr0, *ptr1, *ptr2, *ptr3; \ + \ + ptr0 = src + y0*step + x0*cn; \ + ptr1 = src + y0*step + x1*cn; \ + ptr2 = src + y1*step + x0*cn; \ + ptr3 = src + y1*step + x1*cn; \ + \ + for( k = 0; k < cn; k++ ) \ + { \ + p0 = mul_one_macro(ptr0[k]) + a * (ptr1[k] - ptr0[k]); \ + p1 = mul_one_macro(ptr2[k]) + a * (ptr3[k] - ptr2[k]); \ + p0 = descale_macro( mul_one_macro(p0) + b*(p1 - p0) ); \ + dst[x*cn+k] = (arrtype)cast_macro(p0); \ + } \ + } \ + else if( fillval ) \ + for( k = 0; k < cn; k++ ) \ + dst[x*cn+k] = fillval[k]; \ + } \ + } \ + \ + return CV_OK; \ +} + + +#define ICV_WARP_SCALE_ALPHA(x) ((x)*(1./(ICV_WARP_MASK+1))) + +ICV_DEF_WARP_AFFINE_BILINEAR_FUNC( 8u, uchar, int, CV_NOP, ICV_WARP_MUL_ONE_8U, + ICV_WARP_DESCALE_8U, CV_NOP ) +//ICV_DEF_WARP_AFFINE_BILINEAR_FUNC( 8u, uchar, double, ICV_WARP_SCALE_ALPHA, CV_NOP, +// CV_NOP, ICV_WARP_CAST_8U ) +ICV_DEF_WARP_AFFINE_BILINEAR_FUNC( 16u, ushort, double, ICV_WARP_SCALE_ALPHA, CV_NOP, + CV_NOP, cvRound ) +ICV_DEF_WARP_AFFINE_BILINEAR_FUNC( 32f, float, double, ICV_WARP_SCALE_ALPHA, CV_NOP, + CV_NOP, CV_NOP ) + + +typedef CvStatus (CV_STDCALL * CvWarpAffineFunc)( + const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + const double* matrix, int cn, + const void* fillval, const int* ofs ); + +static void icvInitWarpAffineTab( CvFuncTable* bilin_tab ) +{ + bilin_tab->fn_2d[CV_8U] = (void*)icvWarpAffine_Bilinear_8u_CnR; + bilin_tab->fn_2d[CV_16U] = (void*)icvWarpAffine_Bilinear_16u_CnR; + bilin_tab->fn_2d[CV_32F] = (void*)icvWarpAffine_Bilinear_32f_CnR; +} + + +/////////////////////////////// IPP warpaffine functions ///////////////////////////////// + +icvWarpAffineBack_8u_C1R_t icvWarpAffineBack_8u_C1R_p = 0; +icvWarpAffineBack_8u_C3R_t icvWarpAffineBack_8u_C3R_p = 0; +icvWarpAffineBack_8u_C4R_t icvWarpAffineBack_8u_C4R_p = 0; +icvWarpAffineBack_32f_C1R_t icvWarpAffineBack_32f_C1R_p = 0; +icvWarpAffineBack_32f_C3R_t icvWarpAffineBack_32f_C3R_p = 0; +icvWarpAffineBack_32f_C4R_t icvWarpAffineBack_32f_C4R_p = 0; + +typedef CvStatus (CV_STDCALL * CvWarpAffineBackIPPFunc) +( const void* src, CvSize srcsize, int srcstep, CvRect srcroi, + void* dst, int dststep, CvRect dstroi, + const double* coeffs, int interpolation ); + +////////////////////////////////////////////////////////////////////////////////////////// + +CV_IMPL void +cvWarpAffine( const CvArr* srcarr, CvArr* dstarr, const CvMat* matrix, + int flags, CvScalar fillval ) +{ + static CvFuncTable bilin_tab; + static int inittab = 0; + + CV_FUNCNAME( "cvWarpAffine" ); + + __BEGIN__; + + CvMat srcstub, *src = (CvMat*)srcarr; + CvMat dststub, *dst = (CvMat*)dstarr; + int k, type, depth, cn, *ofs = 0; + double src_matrix[6], dst_matrix[6]; + double fillbuf[4]; + int method = flags & 3; + CvMat srcAb = cvMat( 2, 3, CV_64F, src_matrix ), + dstAb = cvMat( 2, 3, CV_64F, dst_matrix ), + A, b, invA, invAb; + CvWarpAffineFunc func; + CvSize ssize, dsize; + + if( !inittab ) + { + icvInitWarpAffineTab( &bilin_tab ); + inittab = 1; + } + + CV_CALL( src = cvGetMat( srcarr, &srcstub )); + CV_CALL( dst = cvGetMat( dstarr, &dststub )); + + if( !CV_ARE_TYPES_EQ( src, dst )) + CV_ERROR( CV_StsUnmatchedFormats, "" ); + + if( !CV_IS_MAT(matrix) || CV_MAT_CN(matrix->type) != 1 || + CV_MAT_DEPTH(matrix->type) < CV_32F || matrix->rows != 2 || matrix->cols != 3 ) + CV_ERROR( CV_StsBadArg, + "Transformation matrix should be 2x3 floating-point single-channel matrix" ); + + if( flags & CV_WARP_INVERSE_MAP ) + cvConvertScale( matrix, &dstAb ); + else + { + // [R|t] -> [R^-1 | -(R^-1)*t] + cvConvertScale( matrix, &srcAb ); + cvGetCols( &srcAb, &A, 0, 2 ); + cvGetCol( &srcAb, &b, 2 ); + cvGetCols( &dstAb, &invA, 0, 2 ); + cvGetCol( &dstAb, &invAb, 2 ); + cvInvert( &A, &invA, CV_SVD ); + cvGEMM( &invA, &b, -1, 0, 0, &invAb ); + } + + type = CV_MAT_TYPE(src->type); + depth = CV_MAT_DEPTH(type); + cn = CV_MAT_CN(type); + if( cn > 4 ) + CV_ERROR( CV_BadNumChannels, "" ); + + ssize = cvGetMatSize(src); + dsize = cvGetMatSize(dst); + + if( icvWarpAffineBack_8u_C1R_p && MIN( ssize.width, dsize.width ) >= 4 && + MIN( ssize.height, dsize.height ) >= 4 ) + { + CvWarpAffineBackIPPFunc ipp_func = + type == CV_8UC1 ? icvWarpAffineBack_8u_C1R_p : + type == CV_8UC3 ? icvWarpAffineBack_8u_C3R_p : + type == CV_8UC4 ? icvWarpAffineBack_8u_C4R_p : + type == CV_32FC1 ? icvWarpAffineBack_32f_C1R_p : + type == CV_32FC3 ? icvWarpAffineBack_32f_C3R_p : + type == CV_32FC4 ? icvWarpAffineBack_32f_C4R_p : 0; + + if( ipp_func && CV_INTER_NN <= method && method <= CV_INTER_AREA ) + { + int srcstep = src->step ? src->step : CV_STUB_STEP; + int dststep = dst->step ? dst->step : CV_STUB_STEP; + CvRect srcroi = {0, 0, ssize.width, ssize.height}; + CvRect dstroi = {0, 0, dsize.width, dsize.height}; + + // this is not the most efficient way to fill outliers + if( flags & CV_WARP_FILL_OUTLIERS ) + cvSet( dst, fillval ); + + if( ipp_func( src->data.ptr, ssize, srcstep, srcroi, + dst->data.ptr, dststep, dstroi, + dstAb.data.db, 1 << method ) >= 0 ) + EXIT; + } + } + + cvScalarToRawData( &fillval, fillbuf, CV_MAT_TYPE(src->type), 0 ); + ofs = (int*)cvStackAlloc( dst->cols*2*sizeof(ofs[0]) ); + for( k = 0; k < dst->cols; k++ ) + { + ofs[2*k] = CV_FLT_TO_FIX( dst_matrix[0]*k, ICV_WARP_SHIFT ); + ofs[2*k+1] = CV_FLT_TO_FIX( dst_matrix[3]*k, ICV_WARP_SHIFT ); + } + + /*if( method == CV_INTER_LINEAR )*/ + { + func = (CvWarpAffineFunc)bilin_tab.fn_2d[depth]; + if( !func ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + IPPI_CALL( func( src->data.ptr, src->step, ssize, dst->data.ptr, + dst->step, dsize, dst_matrix, cn, + flags & CV_WARP_FILL_OUTLIERS ? fillbuf : 0, ofs )); + } + + __END__; +} + + +CV_IMPL CvMat* +cv2DRotationMatrix( CvPoint2D32f center, double angle, + double scale, CvMat* matrix ) +{ + CV_FUNCNAME( "cvGetRotationMatrix" ); + + __BEGIN__; + + double m[2][3]; + CvMat M = cvMat( 2, 3, CV_64FC1, m ); + double alpha, beta; + + if( !matrix ) + CV_ERROR( CV_StsNullPtr, "" ); + + angle *= CV_PI/180; + alpha = cos(angle)*scale; + beta = sin(angle)*scale; + + m[0][0] = alpha; + m[0][1] = beta; + m[0][2] = (1-alpha)*center.x - beta*center.y; + m[1][0] = -beta; + m[1][1] = alpha; + m[1][2] = beta*center.x + (1-alpha)*center.y; + + cvConvert( &M, matrix ); + + __END__; + + return matrix; +} + + +/****************************************************************************************\ +* WarpPerspective * +\****************************************************************************************/ + +#define ICV_DEF_WARP_PERSPECTIVE_BILINEAR_FUNC( flavor, arrtype, load_macro, cast_macro )\ +static CvStatus CV_STDCALL \ +icvWarpPerspective_Bilinear_##flavor##_CnR( \ + const arrtype* src, int step, CvSize ssize, \ + arrtype* dst, int dststep, CvSize dsize, \ + const double* matrix, int cn, \ + const arrtype* fillval ) \ +{ \ + int x, y, k; \ + float A11 = (float)matrix[0], A12 = (float)matrix[1], A13 = (float)matrix[2];\ + float A21 = (float)matrix[3], A22 = (float)matrix[4], A23 = (float)matrix[5];\ + float A31 = (float)matrix[6], A32 = (float)matrix[7], A33 = (float)matrix[8];\ + \ + step /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + \ + for( y = 0; y < dsize.height; y++, dst += dststep ) \ + { \ + float xs0 = A12*y + A13; \ + float ys0 = A22*y + A23; \ + float ws = A32*y + A33; \ + \ + for( x = 0; x < dsize.width; x++, xs0 += A11, ys0 += A21, ws += A31 )\ + { \ + float inv_ws = 1.f/ws; \ + float xs = xs0*inv_ws; \ + float ys = ys0*inv_ws; \ + int ixs = cvFloor(xs); \ + int iys = cvFloor(ys); \ + float a = xs - ixs; \ + float b = ys - iys; \ + float p0, p1; \ + \ + if( (unsigned)ixs < (unsigned)(ssize.width - 1) && \ + (unsigned)iys < (unsigned)(ssize.height - 1) ) \ + { \ + const arrtype* ptr = src + step*iys + ixs*cn; \ + \ + for( k = 0; k < cn; k++ ) \ + { \ + p0 = load_macro(ptr[k]) + \ + a * (load_macro(ptr[k+cn]) - load_macro(ptr[k])); \ + p1 = load_macro(ptr[k+step]) + \ + a * (load_macro(ptr[k+cn+step]) - \ + load_macro(ptr[k+step])); \ + dst[x*cn+k] = (arrtype)cast_macro(p0 + b*(p1 - p0)); \ + } \ + } \ + else if( (unsigned)(ixs+1) < (unsigned)(ssize.width+1) && \ + (unsigned)(iys+1) < (unsigned)(ssize.height+1)) \ + { \ + int x0 = ICV_WARP_CLIP_X( ixs ); \ + int y0 = ICV_WARP_CLIP_Y( iys ); \ + int x1 = ICV_WARP_CLIP_X( ixs + 1 ); \ + int y1 = ICV_WARP_CLIP_Y( iys + 1 ); \ + const arrtype* ptr0, *ptr1, *ptr2, *ptr3; \ + \ + ptr0 = src + y0*step + x0*cn; \ + ptr1 = src + y0*step + x1*cn; \ + ptr2 = src + y1*step + x0*cn; \ + ptr3 = src + y1*step + x1*cn; \ + \ + for( k = 0; k < cn; k++ ) \ + { \ + p0 = load_macro(ptr0[k]) + \ + a * (load_macro(ptr1[k]) - load_macro(ptr0[k])); \ + p1 = load_macro(ptr2[k]) + \ + a * (load_macro(ptr3[k]) - load_macro(ptr2[k])); \ + dst[x*cn+k] = (arrtype)cast_macro(p0 + b*(p1 - p0)); \ + } \ + } \ + else if( fillval ) \ + for( k = 0; k < cn; k++ ) \ + dst[x*cn+k] = fillval[k]; \ + } \ + } \ + \ + return CV_OK; \ +} + + +#define ICV_WARP_SCALE_ALPHA(x) ((x)*(1./(ICV_WARP_MASK+1))) + +ICV_DEF_WARP_PERSPECTIVE_BILINEAR_FUNC( 8u, uchar, CV_8TO32F, cvRound ) +ICV_DEF_WARP_PERSPECTIVE_BILINEAR_FUNC( 16u, ushort, CV_NOP, cvRound ) +ICV_DEF_WARP_PERSPECTIVE_BILINEAR_FUNC( 32f, float, CV_NOP, CV_NOP ) + +typedef CvStatus (CV_STDCALL * CvWarpPerspectiveFunc)( + const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + const double* matrix, int cn, const void* fillval ); + +static void icvInitWarpPerspectiveTab( CvFuncTable* bilin_tab ) +{ + bilin_tab->fn_2d[CV_8U] = (void*)icvWarpPerspective_Bilinear_8u_CnR; + bilin_tab->fn_2d[CV_16U] = (void*)icvWarpPerspective_Bilinear_16u_CnR; + bilin_tab->fn_2d[CV_32F] = (void*)icvWarpPerspective_Bilinear_32f_CnR; +} + + +/////////////////////////// IPP warpperspective functions //////////////////////////////// + +icvWarpPerspectiveBack_8u_C1R_t icvWarpPerspectiveBack_8u_C1R_p = 0; +icvWarpPerspectiveBack_8u_C3R_t icvWarpPerspectiveBack_8u_C3R_p = 0; +icvWarpPerspectiveBack_8u_C4R_t icvWarpPerspectiveBack_8u_C4R_p = 0; +icvWarpPerspectiveBack_32f_C1R_t icvWarpPerspectiveBack_32f_C1R_p = 0; +icvWarpPerspectiveBack_32f_C3R_t icvWarpPerspectiveBack_32f_C3R_p = 0; +icvWarpPerspectiveBack_32f_C4R_t icvWarpPerspectiveBack_32f_C4R_p = 0; + +icvWarpPerspective_8u_C1R_t icvWarpPerspective_8u_C1R_p = 0; +icvWarpPerspective_8u_C3R_t icvWarpPerspective_8u_C3R_p = 0; +icvWarpPerspective_8u_C4R_t icvWarpPerspective_8u_C4R_p = 0; +icvWarpPerspective_32f_C1R_t icvWarpPerspective_32f_C1R_p = 0; +icvWarpPerspective_32f_C3R_t icvWarpPerspective_32f_C3R_p = 0; +icvWarpPerspective_32f_C4R_t icvWarpPerspective_32f_C4R_p = 0; + +typedef CvStatus (CV_STDCALL * CvWarpPerspectiveBackIPPFunc) +( const void* src, CvSize srcsize, int srcstep, CvRect srcroi, + void* dst, int dststep, CvRect dstroi, + const double* coeffs, int interpolation ); + +////////////////////////////////////////////////////////////////////////////////////////// + +CV_IMPL void +cvWarpPerspective( const CvArr* srcarr, CvArr* dstarr, + const CvMat* matrix, int flags, CvScalar fillval ) +{ + static CvFuncTable bilin_tab; + static int inittab = 0; + + CV_FUNCNAME( "cvWarpPerspective" ); + + __BEGIN__; + + CvMat srcstub, *src = (CvMat*)srcarr; + CvMat dststub, *dst = (CvMat*)dstarr; + int type, depth, cn; + int method = flags & 3; + double src_matrix[9], dst_matrix[9]; + double fillbuf[4]; + CvMat A = cvMat( 3, 3, CV_64F, src_matrix ), + invA = cvMat( 3, 3, CV_64F, dst_matrix ); + CvWarpPerspectiveFunc func; + CvSize ssize, dsize; + + if( method == CV_INTER_NN || method == CV_INTER_AREA ) + method = CV_INTER_LINEAR; + + if( !inittab ) + { + icvInitWarpPerspectiveTab( &bilin_tab ); + inittab = 1; + } + + CV_CALL( src = cvGetMat( srcarr, &srcstub )); + CV_CALL( dst = cvGetMat( dstarr, &dststub )); + + if( !CV_ARE_TYPES_EQ( src, dst )) + CV_ERROR( CV_StsUnmatchedFormats, "" ); + + if( !CV_IS_MAT(matrix) || CV_MAT_CN(matrix->type) != 1 || + CV_MAT_DEPTH(matrix->type) < CV_32F || matrix->rows != 3 || matrix->cols != 3 ) + CV_ERROR( CV_StsBadArg, + "Transformation matrix should be 3x3 floating-point single-channel matrix" ); + + if( flags & CV_WARP_INVERSE_MAP ) + cvConvertScale( matrix, &invA ); + else + { + cvConvertScale( matrix, &A ); + cvInvert( &A, &invA, CV_SVD ); + } + + type = CV_MAT_TYPE(src->type); + depth = CV_MAT_DEPTH(type); + cn = CV_MAT_CN(type); + if( cn > 4 ) + CV_ERROR( CV_BadNumChannels, "" ); + + ssize = cvGetMatSize(src); + dsize = cvGetMatSize(dst); + + if( icvWarpPerspectiveBack_8u_C1R_p ) + { + CvWarpPerspectiveBackIPPFunc ipp_func = + type == CV_8UC1 ? icvWarpPerspectiveBack_8u_C1R_p : + type == CV_8UC3 ? icvWarpPerspectiveBack_8u_C3R_p : + type == CV_8UC4 ? icvWarpPerspectiveBack_8u_C4R_p : + type == CV_32FC1 ? icvWarpPerspectiveBack_32f_C1R_p : + type == CV_32FC3 ? icvWarpPerspectiveBack_32f_C3R_p : + type == CV_32FC4 ? icvWarpPerspectiveBack_32f_C4R_p : 0; + + if( ipp_func && CV_INTER_NN <= method && method <= CV_INTER_AREA && + MIN(ssize.width,ssize.height) >= 4 && MIN(dsize.width,dsize.height) >= 4 ) + { + int srcstep = src->step ? src->step : CV_STUB_STEP; + int dststep = dst->step ? dst->step : CV_STUB_STEP; + CvStatus status; + CvRect srcroi = {0, 0, ssize.width, ssize.height}; + CvRect dstroi = {0, 0, dsize.width, dsize.height}; + + // this is not the most efficient way to fill outliers + if( flags & CV_WARP_FILL_OUTLIERS ) + cvSet( dst, fillval ); + + status = ipp_func( src->data.ptr, ssize, srcstep, srcroi, + dst->data.ptr, dststep, dstroi, + invA.data.db, 1 << method ); + if( status >= 0 ) + EXIT; + + ipp_func = type == CV_8UC1 ? icvWarpPerspective_8u_C1R_p : + type == CV_8UC3 ? icvWarpPerspective_8u_C3R_p : + type == CV_8UC4 ? icvWarpPerspective_8u_C4R_p : + type == CV_32FC1 ? icvWarpPerspective_32f_C1R_p : + type == CV_32FC3 ? icvWarpPerspective_32f_C3R_p : + type == CV_32FC4 ? icvWarpPerspective_32f_C4R_p : 0; + + if( ipp_func ) + { + if( flags & CV_WARP_INVERSE_MAP ) + cvInvert( &invA, &A, CV_SVD ); + + status = ipp_func( src->data.ptr, ssize, srcstep, srcroi, + dst->data.ptr, dststep, dstroi, + A.data.db, 1 << method ); + if( status >= 0 ) + EXIT; + } + } + } + + cvScalarToRawData( &fillval, fillbuf, CV_MAT_TYPE(src->type), 0 ); + + /*if( method == CV_INTER_LINEAR )*/ + { + func = (CvWarpPerspectiveFunc)bilin_tab.fn_2d[depth]; + if( !func ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + IPPI_CALL( func( src->data.ptr, src->step, ssize, dst->data.ptr, + dst->step, dsize, dst_matrix, cn, + flags & CV_WARP_FILL_OUTLIERS ? fillbuf : 0 )); + } + + __END__; +} + + +/* Calculates coefficients of perspective transformation + * which maps (xi,yi) to (ui,vi), (i=1,2,3,4): + * + * c00*xi + c01*yi + c02 + * ui = --------------------- + * c20*xi + c21*yi + c22 + * + * c10*xi + c11*yi + c12 + * vi = --------------------- + * c20*xi + c21*yi + c22 + * + * Coefficients are calculated by solving linear system: + * / x0 y0 1 0 0 0 -x0*u0 -y0*u0 \ /c00\ /u0\ + * | x1 y1 1 0 0 0 -x1*u1 -y1*u1 | |c01| |u1| + * | x2 y2 1 0 0 0 -x2*u2 -y2*u2 | |c02| |u2| + * | x3 y3 1 0 0 0 -x3*u3 -y3*u3 |.|c10|=|u3|, + * | 0 0 0 x0 y0 1 -x0*v0 -y0*v0 | |c11| |v0| + * | 0 0 0 x1 y1 1 -x1*v1 -y1*v1 | |c12| |v1| + * | 0 0 0 x2 y2 1 -x2*v2 -y2*v2 | |c20| |v2| + * \ 0 0 0 x3 y3 1 -x3*v3 -y3*v3 / \c21/ \v3/ + * + * where: + * cij - matrix coefficients, c22 = 1 + */ +CV_IMPL CvMat* +cvGetPerspectiveTransform( const CvPoint2D32f* src, + const CvPoint2D32f* dst, + CvMat* matrix ) +{ + CV_FUNCNAME( "cvGetPerspectiveTransform" ); + + __BEGIN__; + + double a[8][8]; + double b[8], x[9]; + + CvMat A = cvMat( 8, 8, CV_64FC1, a ); + CvMat B = cvMat( 8, 1, CV_64FC1, b ); + CvMat X = cvMat( 8, 1, CV_64FC1, x ); + + int i; + + if( !src || !dst || !matrix ) + CV_ERROR( CV_StsNullPtr, "" ); + + for( i = 0; i < 4; ++i ) + { + a[i][0] = a[i+4][3] = src[i].x; + a[i][1] = a[i+4][4] = src[i].y; + a[i][2] = a[i+4][5] = 1; + a[i][3] = a[i][4] = a[i][5] = + a[i+4][0] = a[i+4][1] = a[i+4][2] = 0; + a[i][6] = -src[i].x*dst[i].x; + a[i][7] = -src[i].y*dst[i].x; + a[i+4][6] = -src[i].x*dst[i].y; + a[i+4][7] = -src[i].y*dst[i].y; + b[i] = dst[i].x; + b[i+4] = dst[i].y; + } + + cvSolve( &A, &B, &X, CV_SVD ); + x[8] = 1; + + X = cvMat( 3, 3, CV_64FC1, x ); + cvConvert( &X, matrix ); + + __END__; + + return matrix; +} + +/* Calculates coefficients of affine transformation + * which maps (xi,yi) to (ui,vi), (i=1,2,3): + * + * ui = c00*xi + c01*yi + c02 + * + * vi = c10*xi + c11*yi + c12 + * + * Coefficients are calculated by solving linear system: + * / x0 y0 1 0 0 0 \ /c00\ /u0\ + * | x1 y1 1 0 0 0 | |c01| |u1| + * | x2 y2 1 0 0 0 | |c02| |u2| + * | 0 0 0 x0 y0 1 | |c10| |v0| + * | 0 0 0 x1 y1 1 | |c11| |v1| + * \ 0 0 0 x2 y2 1 / |c12| |v2| + * + * where: + * cij - matrix coefficients + */ +CV_IMPL CvMat* +cvGetAffineTransform( const CvPoint2D32f * src, const CvPoint2D32f * dst, CvMat * map_matrix ) +{ + CV_FUNCNAME( "cvGetAffineTransform" ); + + __BEGIN__; + + CvMat mA, mX, mB; + double A[6*6]; + double B[6]; + double x[6]; + int i; + + cvInitMatHeader(&mA, 6, 6, CV_64F, A); + cvInitMatHeader(&mB, 6, 1, CV_64F, B); + cvInitMatHeader(&mX, 6, 1, CV_64F, x); + + if( !src || !dst || !map_matrix ) + CV_ERROR( CV_StsNullPtr, "" ); + + for( i = 0; i < 3; i++ ) + { + int j = i*12; + int k = i*12+6; + A[j] = A[k+3] = src[i].x; + A[j+1] = A[k+4] = src[i].y; + A[j+2] = A[k+5] = 1; + A[j+3] = A[j+4] = A[j+5] = 0; + A[k] = A[k+1] = A[k+2] = 0; + B[i*2] = dst[i].x; + B[i*2+1] = dst[i].y; + } + cvSolve(&mA, &mB, &mX); + + mX = cvMat( 2, 3, CV_64FC1, x ); + cvConvert( &mX, map_matrix ); + + __END__; + return map_matrix; +} + +/****************************************************************************************\ +* Generic Geometric Transformation: Remap * +\****************************************************************************************/ + +#define ICV_DEF_REMAP_BILINEAR_FUNC( flavor, arrtype, load_macro, cast_macro ) \ +static CvStatus CV_STDCALL \ +icvRemap_Bilinear_##flavor##_CnR( const arrtype* src, int srcstep, CvSize ssize,\ + arrtype* dst, int dststep, CvSize dsize, \ + const float* mapx, int mxstep, \ + const float* mapy, int mystep, \ + int cn, const arrtype* fillval ) \ +{ \ + int i, j, k; \ + ssize.width--; \ + ssize.height--; \ + \ + srcstep /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + mxstep /= sizeof(mapx[0]); \ + mystep /= sizeof(mapy[0]); \ + \ + for( i = 0; i < dsize.height; i++, dst += dststep, \ + mapx += mxstep, mapy += mystep ) \ + { \ + for( j = 0; j < dsize.width; j++ ) \ + { \ + float _x = mapx[j], _y = mapy[j]; \ + int ix = cvFloor(_x), iy = cvFloor(_y); \ + \ + if( (unsigned)ix < (unsigned)ssize.width && \ + (unsigned)iy < (unsigned)ssize.height ) \ + { \ + const arrtype* s = src + iy*srcstep + ix*cn; \ + _x -= ix; _y -= iy; \ + for( k = 0; k < cn; k++, s++ ) \ + { \ + float t0 = load_macro(s[0]), t1 = load_macro(s[srcstep]); \ + t0 += _x*(load_macro(s[cn]) - t0); \ + t1 += _x*(load_macro(s[srcstep + cn]) - t1); \ + dst[j*cn + k] = (arrtype)cast_macro(t0 + _y*(t1 - t0)); \ + } \ + } \ + else if( fillval ) \ + for( k = 0; k < cn; k++ ) \ + dst[j*cn + k] = fillval[k]; \ + } \ + } \ + \ + return CV_OK; \ +} + + +#define ICV_DEF_REMAP_BICUBIC_FUNC( flavor, arrtype, worktype, \ + load_macro, cast_macro1, cast_macro2 ) \ +static CvStatus CV_STDCALL \ +icvRemap_Bicubic_##flavor##_CnR( const arrtype* src, int srcstep, CvSize ssize, \ + arrtype* dst, int dststep, CvSize dsize, \ + const float* mapx, int mxstep, \ + const float* mapy, int mystep, \ + int cn, const arrtype* fillval ) \ +{ \ + int i, j, k; \ + ssize.width = MAX( ssize.width - 3, 0 ); \ + ssize.height = MAX( ssize.height - 3, 0 ); \ + \ + srcstep /= sizeof(src[0]); \ + dststep /= sizeof(dst[0]); \ + mxstep /= sizeof(mapx[0]); \ + mystep /= sizeof(mapy[0]); \ + \ + for( i = 0; i < dsize.height; i++, dst += dststep, \ + mapx += mxstep, mapy += mystep ) \ + { \ + for( j = 0; j < dsize.width; j++ ) \ + { \ + int ix = cvRound(mapx[j]*(1 << ICV_WARP_SHIFT)); \ + int iy = cvRound(mapy[j]*(1 << ICV_WARP_SHIFT)); \ + int ifx = ix & ICV_WARP_MASK; \ + int ify = iy & ICV_WARP_MASK; \ + ix >>= ICV_WARP_SHIFT; \ + iy >>= ICV_WARP_SHIFT; \ + \ + if( (unsigned)(ix-1) < (unsigned)ssize.width && \ + (unsigned)(iy-1) < (unsigned)ssize.height ) \ + { \ + for( k = 0; k < cn; k++ ) \ + { \ + const arrtype* s = src + (iy-1)*srcstep + ix*cn + k; \ + \ + float t0 = load_macro(s[-cn])*icvCubicCoeffs[ifx*2 + 1] + \ + load_macro(s[0])*icvCubicCoeffs[ifx*2] + \ + load_macro(s[cn])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2] +\ + load_macro(s[cn*2])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2+1];\ + \ + s += srcstep; \ + \ + float t1 = load_macro(s[-cn])*icvCubicCoeffs[ifx*2 + 1] + \ + load_macro(s[0])*icvCubicCoeffs[ifx*2] + \ + load_macro(s[cn])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2] +\ + load_macro(s[cn*2])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2+1];\ + \ + s += srcstep; \ + \ + float t2 = load_macro(s[-cn])*icvCubicCoeffs[ifx*2 + 1] + \ + load_macro(s[0])*icvCubicCoeffs[ifx*2] + \ + load_macro(s[cn])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2] +\ + load_macro(s[cn*2])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2+1];\ + \ + s += srcstep; \ + \ + float t3 = load_macro(s[-cn])*icvCubicCoeffs[ifx*2 + 1] + \ + load_macro(s[0])*icvCubicCoeffs[ifx*2] + \ + load_macro(s[cn])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2] +\ + load_macro(s[cn*2])*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ifx)*2+1];\ + \ + worktype t = cast_macro1( t0*icvCubicCoeffs[ify*2 + 1] + \ + t1*icvCubicCoeffs[ify*2] + \ + t2*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ify)*2] + \ + t3*icvCubicCoeffs[(ICV_CUBIC_TAB_SIZE-ify)*2+1] );\ + \ + dst[j*cn + k] = cast_macro2(t); \ + } \ + } \ + else if( fillval ) \ + for( k = 0; k < cn; k++ ) \ + dst[j*cn + k] = fillval[k]; \ + } \ + } \ + \ + return CV_OK; \ +} + + +ICV_DEF_REMAP_BILINEAR_FUNC( 8u, uchar, CV_8TO32F, cvRound ) +ICV_DEF_REMAP_BILINEAR_FUNC( 16u, ushort, CV_NOP, cvRound ) +ICV_DEF_REMAP_BILINEAR_FUNC( 32f, float, CV_NOP, CV_NOP ) + +ICV_DEF_REMAP_BICUBIC_FUNC( 8u, uchar, int, CV_8TO32F, cvRound, CV_FAST_CAST_8U ) +ICV_DEF_REMAP_BICUBIC_FUNC( 16u, ushort, int, CV_NOP, cvRound, CV_CAST_16U ) +ICV_DEF_REMAP_BICUBIC_FUNC( 32f, float, float, CV_NOP, CV_NOP, CV_NOP ) + +typedef CvStatus (CV_STDCALL * CvRemapFunc)( + const void* src, int srcstep, CvSize ssize, + void* dst, int dststep, CvSize dsize, + const float* mapx, int mxstep, + const float* mapy, int mystep, + int cn, const void* fillval ); + +static void icvInitRemapTab( CvFuncTable* bilinear_tab, CvFuncTable* bicubic_tab ) +{ + bilinear_tab->fn_2d[CV_8U] = (void*)icvRemap_Bilinear_8u_CnR; + bilinear_tab->fn_2d[CV_16U] = (void*)icvRemap_Bilinear_16u_CnR; + bilinear_tab->fn_2d[CV_32F] = (void*)icvRemap_Bilinear_32f_CnR; + + bicubic_tab->fn_2d[CV_8U] = (void*)icvRemap_Bicubic_8u_CnR; + bicubic_tab->fn_2d[CV_16U] = (void*)icvRemap_Bicubic_16u_CnR; + bicubic_tab->fn_2d[CV_32F] = (void*)icvRemap_Bicubic_32f_CnR; +} + + +/******************** IPP remap functions *********************/ + +typedef CvStatus (CV_STDCALL * CvRemapIPPFunc)( + const void* src, CvSize srcsize, int srcstep, CvRect srcroi, + const float* xmap, int xmapstep, const float* ymap, int ymapstep, + void* dst, int dststep, CvSize dstsize, int interpolation ); + +icvRemap_8u_C1R_t icvRemap_8u_C1R_p = 0; +icvRemap_8u_C3R_t icvRemap_8u_C3R_p = 0; +icvRemap_8u_C4R_t icvRemap_8u_C4R_p = 0; + +icvRemap_32f_C1R_t icvRemap_32f_C1R_p = 0; +icvRemap_32f_C3R_t icvRemap_32f_C3R_p = 0; +icvRemap_32f_C4R_t icvRemap_32f_C4R_p = 0; + +/**************************************************************/ + +#define CV_REMAP_SHIFT 5 +#define CV_REMAP_MASK ((1 << CV_REMAP_SHIFT) - 1) + +#if CV_SSE2 && defined(__GNUC__) +#define align(x) __attribute__ ((aligned (x))) +#elif CV_SSE2 && (defined(__ICL) || defined _MSC_VER && _MSC_VER >= 1300) +#define align(x) __declspec(align(x)) +#else +#define align(x) +#endif + +static void icvRemapFixedPt_8u( const CvMat* src, CvMat* dst, + const CvMat* xymap, const CvMat* amap, const uchar* fillval ) +{ + const int TABSZ = 1 << (CV_REMAP_SHIFT*2); + static ushort align(8) atab[TABSZ][4]; + static int inittab = 0; + + int x, y, cols = src->cols, rows = src->rows; + const uchar* sptr0 = src->data.ptr; + int sstep = src->step; + uchar fv0 = fillval[0], fv1 = fillval[1], fv2 = fillval[2], fv3 = fillval[3]; + int cn = CV_MAT_CN(src->type); +#if CV_SSE2 + const uchar* sptr1 = sptr0 + sstep; + __m128i br = _mm_set1_epi32((cols-2) + ((rows-2)<<16)); + __m128i xy2ofs = _mm_set1_epi32(1 + (sstep << 16)); + __m128i z = _mm_setzero_si128(); + int align(16) iofs0[4], iofs1[4]; +#endif + + if( !inittab ) + { + for( y = 0; y <= CV_REMAP_MASK; y++ ) + for( x = 0; x <= CV_REMAP_MASK; x++ ) + { + int k = (y << CV_REMAP_SHIFT) + x; + atab[k][0] = (ushort)((CV_REMAP_MASK+1 - y)*(CV_REMAP_MASK+1 - x)); + atab[k][1] = (ushort)((CV_REMAP_MASK+1 - y)*x); + atab[k][2] = (ushort)(y*(CV_REMAP_MASK+1 - x)); + atab[k][3] = (ushort)(y*x); + } + inittab = 1; + } + + for( y = 0; y < rows; y++ ) + { + const short* xy = (const short*)(xymap->data.ptr + xymap->step*y); + const ushort* alpha = (const ushort*)(amap->data.ptr + amap->step*y); + uchar* dptr = (uchar*)(dst->data.ptr + dst->step*y); + int x = 0; + + if( cn == 1 ) + { + #if CV_SSE2 + for( ; x <= cols - 8; x += 8 ) + { + __m128i xy0 = _mm_load_si128( (const __m128i*)(xy + x*2)); + __m128i xy1 = _mm_load_si128( (const __m128i*)(xy + x*2 + 8)); + // 0|0|0|0|... <= x0|y0|x1|y1|... < cols-1|rows-1|cols-1|rows-1|... ? + __m128i mask0 = _mm_cmpeq_epi32(_mm_or_si128(_mm_cmpgt_epi16(z, xy0), + _mm_cmpgt_epi16(xy0,br)), z); + __m128i mask1 = _mm_cmpeq_epi32(_mm_or_si128(_mm_cmpgt_epi16(z, xy1), + _mm_cmpgt_epi16(xy1,br)), z); + __m128i ofs0 = _mm_and_si128(_mm_madd_epi16( xy0, xy2ofs ), mask0 ); + __m128i ofs1 = _mm_and_si128(_mm_madd_epi16( xy1, xy2ofs ), mask1 ); + unsigned i0, i1; + __m128i v0, v1, v2, v3, a0, a1, b0, b1; + _mm_store_si128( (__m128i*)iofs0, ofs0 ); + _mm_store_si128( (__m128i*)iofs1, ofs1 ); + i0 = *(ushort*)(sptr0 + iofs0[0]) + (*(ushort*)(sptr0 + iofs0[1]) << 16); + i1 = *(ushort*)(sptr0 + iofs0[2]) + (*(ushort*)(sptr0 + iofs0[3]) << 16); + v0 = _mm_unpacklo_epi32(_mm_cvtsi32_si128(i0), _mm_cvtsi32_si128(i1)); + i0 = *(ushort*)(sptr1 + iofs0[0]) + (*(ushort*)(sptr1 + iofs0[1]) << 16); + i1 = *(ushort*)(sptr1 + iofs0[2]) + (*(ushort*)(sptr1 + iofs0[3]) << 16); + v1 = _mm_unpacklo_epi32(_mm_cvtsi32_si128(i0), _mm_cvtsi32_si128(i1)); + v0 = _mm_unpacklo_epi8(v0, z); + v1 = _mm_unpacklo_epi8(v1, z); + + a0 = _mm_unpacklo_epi32(_mm_loadl_epi64((__m128i*)atab[alpha[x]]), + _mm_loadl_epi64((__m128i*)atab[alpha[x+1]])); + a1 = _mm_unpacklo_epi32(_mm_loadl_epi64((__m128i*)atab[alpha[x+2]]), + _mm_loadl_epi64((__m128i*)atab[alpha[x+3]])); + b0 = _mm_unpacklo_epi64(a0, a1); + b1 = _mm_unpackhi_epi64(a0, a1); + v0 = _mm_madd_epi16(v0, b0); + v1 = _mm_madd_epi16(v1, b1); + v0 = _mm_and_si128(_mm_add_epi32(v0, v1), mask0); + + i0 = *(ushort*)(sptr0 + iofs1[0]) + (*(ushort*)(sptr0 + iofs1[1]) << 16); + i1 = *(ushort*)(sptr0 + iofs1[2]) + (*(ushort*)(sptr0 + iofs1[3]) << 16); + v2 = _mm_unpacklo_epi32(_mm_cvtsi32_si128(i0), _mm_cvtsi32_si128(i1)); + i0 = *(ushort*)(sptr1 + iofs1[0]) + (*(ushort*)(sptr1 + iofs1[1]) << 16); + i1 = *(ushort*)(sptr1 + iofs1[2]) + (*(ushort*)(sptr1 + iofs1[3]) << 16); + v3 = _mm_unpacklo_epi32(_mm_cvtsi32_si128(i0), _mm_cvtsi32_si128(i1)); + v2 = _mm_unpacklo_epi8(v2, z); + v3 = _mm_unpacklo_epi8(v3, z); + + a0 = _mm_unpacklo_epi32(_mm_loadl_epi64((__m128i*)atab[alpha[x+4]]), + _mm_loadl_epi64((__m128i*)atab[alpha[x+5]])); + a1 = _mm_unpacklo_epi32(_mm_loadl_epi64((__m128i*)atab[alpha[x+6]]), + _mm_loadl_epi64((__m128i*)atab[alpha[x+7]])); + b0 = _mm_unpacklo_epi64(a0, a1); + b1 = _mm_unpackhi_epi64(a0, a1); + v2 = _mm_madd_epi16(v2, b0); + v3 = _mm_madd_epi16(v3, b1); + v2 = _mm_and_si128(_mm_add_epi32(v2, v3), mask1); + + v0 = _mm_srai_epi32(v0, CV_REMAP_SHIFT*2); + v2 = _mm_srai_epi32(v2, CV_REMAP_SHIFT*2); + v0 = _mm_packus_epi16(_mm_packs_epi32(v0, v2), z); + _mm_storel_epi64( (__m128i*)(dptr + x), v0 ); + } + #endif + + for( ; x < cols; x++ ) + { + int xi = xy[x*2], yi = xy[x*2+1]; + if( (unsigned)yi >= (unsigned)(rows - 1) || + (unsigned)xi >= (unsigned)(cols - 1)) + { + dptr[x] = fv0; + } + else + { + const uchar* sptr = sptr0 + sstep*yi + xi; + const ushort* a = atab[alpha[x]]; + dptr[x] = (uchar)((sptr[0]*a[0] + sptr[1]*a[1] + sptr[sstep]*a[2] + + sptr[sstep+1]*a[3])>>CV_REMAP_SHIFT*2); + } + } + } + else if( cn == 3 ) + { + for( ; x < cols; x++ ) + { + int xi = xy[x*2], yi = xy[x*2+1]; + if( (unsigned)yi >= (unsigned)(rows - 1) || + (unsigned)xi >= (unsigned)(cols - 1)) + { + dptr[x*3] = fv0; dptr[x*3+1] = fv1; dptr[x*3+2] = fv2; + } + else + { + const uchar* sptr = sptr0 + sstep*yi + xi*3; + const ushort* a = atab[alpha[x]]; + int v0, v1, v2; + v0 = (sptr[0]*a[0] + sptr[3]*a[1] + + sptr[sstep]*a[2] + sptr[sstep+3]*a[3])>>CV_REMAP_SHIFT*2; + v1 = (sptr[1]*a[0] + sptr[4]*a[1] + + sptr[sstep+1]*a[2] + sptr[sstep+4]*a[3])>>CV_REMAP_SHIFT*2; + v2 = (sptr[2]*a[0] + sptr[5]*a[1] + + sptr[sstep+2]*a[2] + sptr[sstep+5]*a[3])>>CV_REMAP_SHIFT*2; + dptr[x*3] = (uchar)v0; dptr[x*3+1] = (uchar)v1; dptr[x*3+2] = (uchar)v2; + } + } + } + else + { + assert( cn == 4 ); + for( ; x < cols; x++ ) + { + int xi = xy[x*2], yi = xy[x*2+1]; + if( (unsigned)yi >= (unsigned)(rows - 1) || + (unsigned)xi >= (unsigned)(cols - 1)) + { + dptr[x*4] = fv0; dptr[x*4+1] = fv1; + dptr[x*4+2] = fv2; dptr[x*4+3] = fv3; + } + else + { + const uchar* sptr = sptr0 + sstep*yi + xi*3; + const ushort* a = atab[alpha[x]]; + int v0, v1; + v0 = (sptr[0]*a[0] + sptr[4]*a[1] + + sptr[sstep]*a[2] + sptr[sstep+3]*a[3])>>CV_REMAP_SHIFT*2; + v1 = (sptr[1]*a[0] + sptr[5]*a[1] + + sptr[sstep+1]*a[2] + sptr[sstep+5]*a[3])>>CV_REMAP_SHIFT*2; + dptr[x*4] = (uchar)v0; dptr[x*4+1] = (uchar)v1; + v0 = (sptr[2]*a[0] + sptr[6]*a[1] + + sptr[sstep+2]*a[2] + sptr[sstep+6]*a[3])>>CV_REMAP_SHIFT*2; + v1 = (sptr[3]*a[0] + sptr[7]*a[1] + + sptr[sstep+3]*a[2] + sptr[sstep+7]*a[3])>>CV_REMAP_SHIFT*2; + dptr[x*4+2] = (uchar)v0; dptr[x*4+3] = (uchar)v1; + } + } + } + } +} + + +CV_IMPL void +cvRemap( const CvArr* srcarr, CvArr* dstarr, + const CvArr* _mapx, const CvArr* _mapy, + int flags, CvScalar fillval ) +{ + static CvFuncTable bilinear_tab; + static CvFuncTable bicubic_tab; + static int inittab = 0; + + CV_FUNCNAME( "cvRemap" ); + + __BEGIN__; + + CvMat srcstub, *src = (CvMat*)srcarr; + CvMat dststub, *dst = (CvMat*)dstarr; + CvMat mxstub, *mapx = (CvMat*)_mapx; + CvMat mystub, *mapy = (CvMat*)_mapy; + int type, depth, cn; + bool fltremap; + int method = flags & 3; + double fillbuf[4]; + CvSize ssize, dsize; + + if( !inittab ) + { + icvInitRemapTab( &bilinear_tab, &bicubic_tab ); + icvInitLinearCoeffTab(); + icvInitCubicCoeffTab(); + inittab = 1; + } + + CV_CALL( src = cvGetMat( srcarr, &srcstub )); + CV_CALL( dst = cvGetMat( dstarr, &dststub )); + CV_CALL( mapx = cvGetMat( mapx, &mxstub )); + CV_CALL( mapy = cvGetMat( mapy, &mystub )); + + if( !CV_ARE_TYPES_EQ( src, dst )) + CV_ERROR( CV_StsUnmatchedFormats, "" ); + + if( CV_MAT_TYPE(mapx->type) == CV_16SC1 && CV_MAT_TYPE(mapy->type) == CV_16SC2 ) + { + CvMat* temp; + CV_SWAP(mapx, mapy, temp); + } + + if( (CV_MAT_TYPE(mapx->type) != CV_32FC1 || CV_MAT_TYPE(mapy->type) != CV_32FC1) && + (CV_MAT_TYPE(mapx->type) != CV_16SC2 || CV_MAT_TYPE(mapy->type) != CV_16SC1)) + CV_ERROR( CV_StsUnmatchedFormats, "Either both map arrays must have 32fC1 type, " + "or one of them must be 16sC2 and the other one must be 16sC1" ); + + if( !CV_ARE_SIZES_EQ( mapx, mapy ) || !CV_ARE_SIZES_EQ( mapx, dst )) + CV_ERROR( CV_StsUnmatchedSizes, + "Both map arrays and the destination array must have the same size" ); + + fltremap = CV_MAT_TYPE(mapx->type) == CV_32FC1; + type = CV_MAT_TYPE(src->type); + depth = CV_MAT_DEPTH(type); + cn = CV_MAT_CN(type); + if( cn > 4 ) + CV_ERROR( CV_BadNumChannels, "" ); + + ssize = cvGetMatSize(src); + dsize = cvGetMatSize(dst); + + cvScalarToRawData( &fillval, fillbuf, CV_MAT_TYPE(src->type), 0 ); + + if( !fltremap ) + { + if( CV_MAT_TYPE(src->type) != CV_8UC1 && CV_MAT_TYPE(src->type) != CV_8UC3 && + CV_MAT_TYPE(src->type) != CV_8UC4 ) + CV_ERROR( CV_StsUnsupportedFormat, + "Only 8-bit input/output is supported by the fixed-point variant of cvRemap" ); + icvRemapFixedPt_8u( src, dst, mapx, mapy, (uchar*)fillbuf ); + EXIT; + } + + if( icvRemap_8u_C1R_p ) + { + CvRemapIPPFunc ipp_func = + type == CV_8UC1 ? icvRemap_8u_C1R_p : + type == CV_8UC3 ? icvRemap_8u_C3R_p : + type == CV_8UC4 ? icvRemap_8u_C4R_p : + type == CV_32FC1 ? icvRemap_32f_C1R_p : + type == CV_32FC3 ? icvRemap_32f_C3R_p : + type == CV_32FC4 ? icvRemap_32f_C4R_p : 0; + + if( ipp_func ) + { + int srcstep = src->step ? src->step : CV_STUB_STEP; + int dststep = dst->step ? dst->step : CV_STUB_STEP; + int mxstep = mapx->step ? mapx->step : CV_STUB_STEP; + int mystep = mapy->step ? mapy->step : CV_STUB_STEP; + CvStatus status; + CvRect srcroi = {0, 0, ssize.width, ssize.height}; + + // this is not the most efficient way to fill outliers + if( flags & CV_WARP_FILL_OUTLIERS ) + cvSet( dst, fillval ); + + status = ipp_func( src->data.ptr, ssize, srcstep, srcroi, + mapx->data.fl, mxstep, mapy->data.fl, mystep, + dst->data.ptr, dststep, dsize, + 1 << (method == CV_INTER_NN || method == CV_INTER_LINEAR || + method == CV_INTER_CUBIC ? method : CV_INTER_LINEAR) ); + if( status >= 0 ) + EXIT; + } + } + + { + CvRemapFunc func = method == CV_INTER_CUBIC ? + (CvRemapFunc)bicubic_tab.fn_2d[depth] : + (CvRemapFunc)bilinear_tab.fn_2d[depth]; + + if( !func ) + CV_ERROR( CV_StsUnsupportedFormat, "" ); + + func( src->data.ptr, src->step, ssize, dst->data.ptr, dst->step, dsize, + mapx->data.fl, mapx->step, mapy->data.fl, mapy->step, + cn, flags & CV_WARP_FILL_OUTLIERS ? fillbuf : 0 ); + } + + __END__; +} + +CV_IMPL void +cvConvertMaps( const CvArr* arrx, const CvArr* arry, + CvArr* arrxy, CvArr* arra ) +{ + CV_FUNCNAME( "cvConvertMaps" ); + + __BEGIN__; + + CvMat xstub, *mapx = cvGetMat( arrx, &xstub ); + CvMat ystub, *mapy = cvGetMat( arry, &ystub ); + CvMat xystub, *mapxy = cvGetMat( arrxy, &xystub ); + CvMat astub, *mapa = cvGetMat( arra, &astub ); + int x, y, cols = mapx->cols, rows = mapx->rows; + + CV_ASSERT( CV_ARE_SIZES_EQ(mapx, mapy) && CV_ARE_TYPES_EQ(mapx, mapy) && + CV_MAT_TYPE(mapx->type) == CV_32FC1 && + CV_ARE_SIZES_EQ(mapxy, mapx) && CV_ARE_SIZES_EQ(mapxy, mapa) && + CV_MAT_TYPE(mapxy->type) == CV_16SC2 && + CV_MAT_TYPE(mapa->type) == CV_16SC1 ); + + for( y = 0; y < rows; y++ ) + { + const float* xrow = (const float*)(mapx->data.ptr + mapx->step*y); + const float* yrow = (const float*)(mapy->data.ptr + mapy->step*y); + short* xy = (short*)(mapxy->data.ptr + mapxy->step*y); + short* alpha = (short*)(mapa->data.ptr + mapa->step*y); + + for( x = 0; x < cols; x++ ) + { + int xi = cvRound(xrow[x]*(1 << CV_REMAP_SHIFT)); + int yi = cvRound(yrow[x]*(1 << CV_REMAP_SHIFT)); + xy[x*2] = (short)(xi >> CV_REMAP_SHIFT); + xy[x*2+1] = (short)(yi >> CV_REMAP_SHIFT); + alpha[x] = (short)((xi & CV_REMAP_MASK) + ((yi & CV_REMAP_MASK)<<CV_REMAP_SHIFT)); + } + } + + __END__; +} + + +/****************************************************************************************\ +* Log-Polar Transform * +\****************************************************************************************/ + +/* now it is done via Remap; more correct implementation should use + some super-sampling technique outside of the "fovea" circle */ +CV_IMPL void +cvLogPolar( const CvArr* srcarr, CvArr* dstarr, + CvPoint2D32f center, double M, int flags ) +{ + CvMat* mapx = 0; + CvMat* mapy = 0; + double* exp_tab = 0; + float* buf = 0; + + CV_FUNCNAME( "cvLogPolar" ); + + __BEGIN__; + + CvMat srcstub, *src = (CvMat*)srcarr; + CvMat dststub, *dst = (CvMat*)dstarr; + CvSize ssize, dsize; + + CV_CALL( src = cvGetMat( srcarr, &srcstub )); + CV_CALL( dst = cvGetMat( dstarr, &dststub )); + + if( !CV_ARE_TYPES_EQ( src, dst )) + CV_ERROR( CV_StsUnmatchedFormats, "" ); + + if( M <= 0 ) + CV_ERROR( CV_StsOutOfRange, "M should be >0" ); + + ssize = cvGetMatSize(src); + dsize = cvGetMatSize(dst); + + CV_CALL( mapx = cvCreateMat( dsize.height, dsize.width, CV_32F )); + CV_CALL( mapy = cvCreateMat( dsize.height, dsize.width, CV_32F )); + + if( !(flags & CV_WARP_INVERSE_MAP) ) + { + int phi, rho; + + CV_CALL( exp_tab = (double*)cvAlloc( dsize.width*sizeof(exp_tab[0])) ); + + for( rho = 0; rho < dst->width; rho++ ) + exp_tab[rho] = exp(rho/M); + + for( phi = 0; phi < dsize.height; phi++ ) + { + double cp = cos(phi*2*CV_PI/dsize.height); + double sp = sin(phi*2*CV_PI/dsize.height); + float* mx = (float*)(mapx->data.ptr + phi*mapx->step); + float* my = (float*)(mapy->data.ptr + phi*mapy->step); + + for( rho = 0; rho < dsize.width; rho++ ) + { + double r = exp_tab[rho]; + double x = r*cp + center.x; + double y = r*sp + center.y; + + mx[rho] = (float)x; + my[rho] = (float)y; + } + } + } + else + { + int x, y; + CvMat bufx, bufy, bufp, bufa; + double ascale = (ssize.width-1)/(2*CV_PI); + + CV_CALL( buf = (float*)cvAlloc( 4*dsize.width*sizeof(buf[0]) )); + + bufx = cvMat( 1, dsize.width, CV_32F, buf ); + bufy = cvMat( 1, dsize.width, CV_32F, buf + dsize.width ); + bufp = cvMat( 1, dsize.width, CV_32F, buf + dsize.width*2 ); + bufa = cvMat( 1, dsize.width, CV_32F, buf + dsize.width*3 ); + + for( x = 0; x < dsize.width; x++ ) + bufx.data.fl[x] = (float)x - center.x; + + for( y = 0; y < dsize.height; y++ ) + { + float* mx = (float*)(mapx->data.ptr + y*mapx->step); + float* my = (float*)(mapy->data.ptr + y*mapy->step); + + for( x = 0; x < dsize.width; x++ ) + bufy.data.fl[x] = (float)y - center.y; + +#if 1 + cvCartToPolar( &bufx, &bufy, &bufp, &bufa ); + + for( x = 0; x < dsize.width; x++ ) + bufp.data.fl[x] += 1.f; + + cvLog( &bufp, &bufp ); + + for( x = 0; x < dsize.width; x++ ) + { + double rho = bufp.data.fl[x]*M; + double phi = bufa.data.fl[x]*ascale; + + mx[x] = (float)rho; + my[x] = (float)phi; + } +#else + for( x = 0; x < dsize.width; x++ ) + { + double xx = bufx.data.fl[x]; + double yy = bufy.data.fl[x]; + + double p = log(sqrt(xx*xx + yy*yy) + 1.)*M; + double a = atan2(yy,xx); + if( a < 0 ) + a = 2*CV_PI + a; + a *= ascale; + + mx[x] = (float)p; + my[x] = (float)a; + } +#endif + } + } + + cvRemap( src, dst, mapx, mapy, flags, cvScalarAll(0) ); + + __END__; + + cvFree( &exp_tab ); + cvFree( &buf ); + cvReleaseMat( &mapx ); + cvReleaseMat( &mapy ); +} + +/* End of file. */ |