git.maemo.org Git - opencv/blob - cv/src/cvsamplers.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cv.h"
  43
  44 /**************************************************************************************\
  45 *                                   line samplers                                      *
  46 \**************************************************************************************/
  47
  48 CV_IMPL int
  49 cvSampleLine( const void* img, CvPoint pt1, CvPoint pt2,
  50               void* _buffer, int connectivity )
  51 {
  52     int count = -1;
  53
  54     CV_FUNCNAME( "cvSampleLine" );
  55
  56     __BEGIN__;
  57
  58     int i, coi = 0, pix_size;
  59     CvMat stub, *mat = (CvMat*)img;
  60     CvLineIterator iterator;
  61     uchar* buffer = (uchar*)_buffer;
  62
  63     CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
  64
  65     if( coi != 0 )
  66         CV_ERROR( CV_BadCOI, "" );
  67
  68     if( !buffer )
  69         CV_ERROR( CV_StsNullPtr, "" );
  70
  71     CV_CALL( count = cvInitLineIterator( mat, pt1, pt2, &iterator, connectivity ));
  72
  73     pix_size = CV_ELEM_SIZE(mat->type);
  74     for( i = 0; i < count; i++ )
  75     {
  76         CV_MEMCPY_AUTO( buffer, iterator.ptr, pix_size );
  77         buffer += pix_size;
  78         CV_NEXT_LINE_POINT( iterator );
  79     }
  80
  81     __END__;
  82
  83     return count;
  84 }
  85
  86
  87 static const void*
  88 icvAdjustRect( const void* srcptr, int src_step, int pix_size,
  89                CvSize src_size, CvSize win_size,
  90                CvPoint ip, CvRect* pRect )
  91 {
  92     CvRect rect;
  93     const char* src = (const char*)srcptr;
  94
  95     if( ip.x >= 0 )
  96     {
  97         src += ip.x*pix_size;
  98         rect.x = 0;
  99     }
 100     else
 101     {
 102         rect.x = -ip.x;
 103         if( rect.x > win_size.width )
 104             rect.x = win_size.width;
 105     }
 106
 107     if( ip.x + win_size.width < src_size.width )
 108         rect.width = win_size.width;
 109     else
 110     {
 111         rect.width = src_size.width - ip.x - 1;
 112         if( rect.width < 0 )
 113         {
 114             src += rect.width*pix_size;
 115             rect.width = 0;
 116         }
 117         assert( rect.width <= win_size.width );
 118     }
 119
 120     if( ip.y >= 0 )
 121     {
 122         src += ip.y * src_step;
 123         rect.y = 0;
 124     }
 125     else
 126         rect.y = -ip.y;
 127
 128     if( ip.y + win_size.height < src_size.height )
 129         rect.height = win_size.height;
 130     else
 131     {
 132         rect.height = src_size.height - ip.y - 1;
 133         if( rect.height < 0 )
 134         {
 135             src += rect.height*src_step;
 136             rect.height = 0;
 137         }
 138     }
 139
 140     *pRect = rect;
 141     return src - rect.x*pix_size;
 142 }
 143
 144
 145 #define  ICV_DEF_GET_RECT_SUB_PIX_FUNC( flavor, srctype, dsttype, worktype, \
 146                                         cast_macro, scale_macro, cast_macro2 )\
 147 CvStatus CV_STDCALL icvGetRectSubPix_##flavor##_C1R                         \
 148 ( const srctype* src, int src_step, CvSize src_size,                        \
 149   dsttype* dst, int dst_step, CvSize win_size, CvPoint2D32f center )        \
 150 {                                                                           \
 151     CvPoint ip;                                                             \
 152     worktype  a11, a12, a21, a22, b1, b2;                                   \
 153     float a, b;                                                             \
 154     int i, j;                                                               \
 155                                                                             \
 156     center.x -= (win_size.width-1)*0.5f;                                    \
 157     center.y -= (win_size.height-1)*0.5f;                                   \
 158                                                                             \
 159     ip.x = cvFloor( center.x );                                             \
 160     ip.y = cvFloor( center.y );                                             \
 161                                                                             \
 162     a = center.x - ip.x;                                                    \
 163     b = center.y - ip.y;                                                    \
 164     a11 = scale_macro((1.f-a)*(1.f-b));                                     \
 165     a12 = scale_macro(a*(1.f-b));                                           \
 166     a21 = scale_macro((1.f-a)*b);                                           \
 167     a22 = scale_macro(a*b);                                                 \
 168     b1 = scale_macro(1.f - b);                                              \
 169     b2 = scale_macro(b);                                                    \
 170                                                                             \
 171     src_step /= sizeof(src[0]);                                             \
 172     dst_step /= sizeof(dst[0]);                                             \
 173                                                                             \
 174     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&              \
 175         0 <= ip.y && ip.y + win_size.height < src_size.height )             \
 176     {                                                                       \
 177         /* extracted rectangle is totally inside the image */               \
 178         src += ip.y * src_step + ip.x;                                      \
 179                                                                             \
 180         if( icvCopySubpix_##flavor##_C1R_p &&                               \
 181             icvCopySubpix_##flavor##_C1R_p( src, src_step*sizeof(src[0]),   \
 182                                             dst, dst_step*sizeof(dst[0]),   \
 183                                             win_size, a, b ) >= 0 )         \
 184             return CV_OK;                                                   \
 185                                                                             \
 186         for( i = 0; i < win_size.height; i++, src += src_step,              \
 187                                               dst += dst_step )             \
 188         {                                                                   \
 189             for( j = 0; j <= win_size.width - 2; j += 2 )                   \
 190             {                                                               \
 191                 worktype s0 = cast_macro(src[j])*a11 +                      \
 192                               cast_macro(src[j+1])*a12 +                    \
 193                               cast_macro(src[j+src_step])*a21 +             \
 194                               cast_macro(src[j+src_step+1])*a22;            \
 195                 worktype s1 = cast_macro(src[j+1])*a11 +                    \
 196                               cast_macro(src[j+2])*a12 +                    \
 197                               cast_macro(src[j+src_step+1])*a21 +           \
 198                               cast_macro(src[j+src_step+2])*a22;            \
 199                                                                             \
 200                 dst[j] = (dsttype)cast_macro2(s0);                          \
 201                 dst[j+1] = (dsttype)cast_macro2(s1);                        \
 202             }                                                               \
 203                                                                             \
 204             for( ; j < win_size.width; j++ )                                \
 205             {                                                               \
 206                 worktype s0 = cast_macro(src[j])*a11 +                      \
 207                               cast_macro(src[j+1])*a12 +                    \
 208                               cast_macro(src[j+src_step])*a21 +             \
 209                               cast_macro(src[j+src_step+1])*a22;            \
 210                                                                             \
 211                 dst[j] = (dsttype)cast_macro2(s0);                          \
 212             }                                                               \
 213         }                                                                   \
 214     }                                                                       \
 215     else                                                                    \
 216     {                                                                       \
 217         CvRect r;                                                           \
 218                                                                             \
 219         src = (const srctype*)icvAdjustRect( src, src_step*sizeof(*src),    \
 220                                sizeof(*src), src_size, win_size,ip, &r);    \
 221                                                                             \
 222         for( i = 0; i < win_size.height; i++, dst += dst_step )             \
 223         {                                                                   \
 224             const srctype *src2 = src + src_step;                           \
 225                                                                             \
 226             if( i < r.y || i >= r.height )                                  \
 227                 src2 -= src_step;                                           \
 228                                                                             \
 229             for( j = 0; j < r.x; j++ )                                      \
 230             {                                                               \
 231                 worktype s0 = cast_macro(src[r.x])*b1 +                     \
 232                               cast_macro(src2[r.x])*b2;                     \
 233                                                                             \
 234                 dst[j] = (dsttype)cast_macro2(s0);                          \
 235             }                                                               \
 236                                                                             \
 237             for( ; j < r.width; j++ )                                       \
 238             {                                                               \
 239                 worktype s0 = cast_macro(src[j])*a11 +                      \
 240                               cast_macro(src[j+1])*a12 +                    \
 241                               cast_macro(src2[j])*a21 +                     \
 242                               cast_macro(src2[j+1])*a22;                    \
 243                                                                             \
 244                 dst[j] = (dsttype)cast_macro2(s0);                          \
 245             }                                                               \
 246                                                                             \
 247             for( ; j < win_size.width; j++ )                                \
 248             {                                                               \
 249                 worktype s0 = cast_macro(src[r.width])*b1 +                 \
 250                               cast_macro(src2[r.width])*b2;                 \
 251                                                                             \
 252                 dst[j] = (dsttype)cast_macro2(s0);                          \
 253             }                                                               \
 254                                                                             \
 255             if( i < r.height )                                              \
 256                 src = src2;                                                 \
 257         }                                                                   \
 258     }                                                                       \
 259                                                                             \
 260     return CV_OK;                                                           \
 261 }
 262
 263
 264 #define  ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( flavor, srctype, dsttype, worktype, \
 265                                         cast_macro, scale_macro, mul_macro )\
 266 static CvStatus CV_STDCALL icvGetRectSubPix_##flavor##_C3R                  \
 267 ( const srctype* src, int src_step, CvSize src_size,                        \
 268   dsttype* dst, int dst_step, CvSize win_size, CvPoint2D32f center )        \
 269 {                                                                           \
 270     CvPoint ip;                                                             \
 271     worktype a, b;                                                          \
 272     int i, j;                                                               \
 273                                                                             \
 274     center.x -= (win_size.width-1)*0.5f;                                    \
 275     center.y -= (win_size.height-1)*0.5f;                                   \
 276                                                                             \
 277     ip.x = cvFloor( center.x );                                             \
 278     ip.y = cvFloor( center.y );                                             \
 279                                                                             \
 280     a = scale_macro( center.x - ip.x );                                     \
 281     b = scale_macro( center.y - ip.y );                                     \
 282                                                                             \
 283     src_step /= sizeof( src[0] );                                           \
 284     dst_step /= sizeof( dst[0] );                                           \
 285                                                                             \
 286     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&              \
 287         0 <= ip.y && ip.y + win_size.height < src_size.height )             \
 288     {                                                                       \
 289         /* extracted rectangle is totally inside the image */               \
 290         src += ip.y * src_step + ip.x*3;                                    \
 291                                                                             \
 292         for( i = 0; i < win_size.height; i++, src += src_step,              \
 293                                               dst += dst_step )             \
 294         {                                                                   \
 295             for( j = 0; j < win_size.width; j++ )                           \
 296             {                                                               \
 297                 worktype s0 = cast_macro(src[j*3]);                         \
 298                 worktype s1 = cast_macro(src[j*3 + src_step]);              \
 299                 s0 += mul_macro( a, (cast_macro(src[j*3+3]) - s0));         \
 300                 s1 += mul_macro( a, (cast_macro(src[j*3+3+src_step]) - s1));\
 301                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
 302                                                                             \
 303                 s0 = cast_macro(src[j*3+1]);                                \
 304                 s1 = cast_macro(src[j*3+1 + src_step]);                     \
 305                 s0 += mul_macro( a, (cast_macro(src[j*3+4]) - s0));         \
 306                 s1 += mul_macro( a, (cast_macro(src[j*3+4+src_step]) - s1));\
 307                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 308                                                                             \
 309                 s0 = cast_macro(src[j*3+2]);                                \
 310                 s1 = cast_macro(src[j*3+2 + src_step]);                     \
 311                 s0 += mul_macro( a, (cast_macro(src[j*3+5]) - s0));         \
 312                 s1 += mul_macro( a, (cast_macro(src[j*3+5+src_step]) - s1));\
 313                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 314             }                                                               \
 315         }                                                                   \
 316     }                                                                       \
 317     else                                                                    \
 318     {                                                                       \
 319         CvRect r;                                                           \
 320                                                                             \
 321         src = (const srctype*)icvAdjustRect( src, src_step*sizeof(*src),    \
 322                              sizeof(*src)*3, src_size, win_size, ip, &r );  \
 323                                                                             \
 324         for( i = 0; i < win_size.height; i++, dst += dst_step )             \
 325         {                                                                   \
 326             const srctype *src2 = src + src_step;                           \
 327                                                                             \
 328             if( i < r.y || i >= r.height )                                  \
 329                 src2 -= src_step;                                           \
 330                                                                             \
 331             for( j = 0; j < r.x; j++ )                                      \
 332             {                                                               \
 333                 worktype s0 = cast_macro(src[r.x*3]);                       \
 334                 worktype s1 = cast_macro(src2[r.x*3]);                      \
 335                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
 336                                                                             \
 337                 s0 = cast_macro(src[r.x*3+1]);                              \
 338                 s1 = cast_macro(src2[r.x*3+1]);                             \
 339                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 340                                                                             \
 341                 s0 = cast_macro(src[r.x*3+2]);                              \
 342                 s1 = cast_macro(src2[r.x*3+2]);                             \
 343                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 344             }                                                               \
 345                                                                             \
 346             for( ; j < r.width; j++ )                                       \
 347             {                                                               \
 348                 worktype s0 = cast_macro(src[j*3]);                         \
 349                 worktype s1 = cast_macro(src2[j*3]);                        \
 350                 s0 += mul_macro( a, (cast_macro(src[j*3 + 3]) - s0));       \
 351                 s1 += mul_macro( a, (cast_macro(src2[j*3 + 3]) - s1));      \
 352                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
 353                                                                             \
 354                 s0 = cast_macro(src[j*3+1]);                                \
 355                 s1 = cast_macro(src2[j*3+1]);                               \
 356                 s0 += mul_macro( a, (cast_macro(src[j*3 + 4]) - s0));       \
 357                 s1 += mul_macro( a, (cast_macro(src2[j*3 + 4]) - s1));      \
 358                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 359                                                                             \
 360                 s0 = cast_macro(src[j*3+2]);                                \
 361                 s1 = cast_macro(src2[j*3+2]);                               \
 362                 s0 += mul_macro( a, (cast_macro(src[j*3 + 5]) - s0));       \
 363                 s1 += mul_macro( a, (cast_macro(src2[j*3 + 5]) - s1));      \
 364                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 365             }                                                               \
 366                                                                             \
 367             for( ; j < win_size.width; j++ )                                \
 368             {                                                               \
 369                 worktype s0 = cast_macro(src[r.width*3]);                   \
 370                 worktype s1 = cast_macro(src2[r.width*3]);                  \
 371                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
 372                                                                             \
 373                 s0 = cast_macro(src[r.width*3+1]);                          \
 374                 s1 = cast_macro(src2[r.width*3+1]);                         \
 375                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 376                                                                             \
 377                 s0 = cast_macro(src[r.width*3+2]);                          \
 378                 s1 = cast_macro(src2[r.width*3+2]);                         \
 379                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
 380             }                                                               \
 381                                                                             \
 382             if( i < r.height )                                              \
 383                 src = src2;                                                 \
 384         }                                                                   \
 385     }                                                                       \
 386                                                                             \
 387     return CV_OK;                                                           \
 388 }
 389
 390
 391
 392 CvStatus CV_STDCALL icvGetRectSubPix_8u32f_C1R
 393 ( const uchar* src, int src_step, CvSize src_size,
 394   float* dst, int dst_step, CvSize win_size, CvPoint2D32f center )
 395 {
 396     CvPoint ip;
 397     float  a12, a22, b1, b2;
 398     float a, b;
 399     double s = 0;
 400     int i, j;
 401
 402     center.x -= (win_size.width-1)*0.5f;
 403     center.y -= (win_size.height-1)*0.5f;
 404
 405     ip.x = cvFloor( center.x );
 406     ip.y = cvFloor( center.y );
 407
 408     if( win_size.width <= 0 || win_size.height <= 0 )
 409         return CV_BADRANGE_ERR;
 410
 411     a = center.x - ip.x;
 412     b = center.y - ip.y;
 413     a = MAX(a,0.0001f);
 414     a12 = a*(1.f-b);
 415     a22 = a*b;
 416     b1 = 1.f - b;
 417     b2 = b;
 418     s = (1. - a)/a;
 419
 420     src_step /= sizeof(src[0]);
 421     dst_step /= sizeof(dst[0]);
 422
 423     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&
 424         0 <= ip.y && ip.y + win_size.height < src_size.height )
 425     {
 426         // extracted rectangle is totally inside the image
 427         src += ip.y * src_step + ip.x;
 428
 429 #if 0
 430         if( icvCopySubpix_8u32f_C1R_p &&
 431             icvCopySubpix_8u32f_C1R_p( src, src_step, dst,
 432                 dst_step*sizeof(dst[0]), win_size, a, b ) >= 0 )
 433             return CV_OK;
 434 #endif
 435
 436         for( ; win_size.height--; src += src_step, dst += dst_step )
 437         {
 438             float prev = (1 - a)*(b1*CV_8TO32F(src[0]) + b2*CV_8TO32F(src[src_step]));
 439             for( j = 0; j < win_size.width; j++ )
 440             {
 441                 float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src[j+1+src_step]);
 442                 dst[j] = prev + t;
 443                 prev = (float)(t*s);
 444             }
 445         }
 446     }
 447     else
 448     {
 449         CvRect r;
 450
 451         src = (const uchar*)icvAdjustRect( src, src_step*sizeof(*src),
 452                                sizeof(*src), src_size, win_size,ip, &r);
 453
 454         for( i = 0; i < win_size.height; i++, dst += dst_step )
 455         {
 456             const uchar *src2 = src + src_step;
 457
 458             if( i < r.y || i >= r.height )
 459                 src2 -= src_step;
 460
 461             for( j = 0; j < r.x; j++ )
 462             {
 463                 float s0 = CV_8TO32F(src[r.x])*b1 +
 464                            CV_8TO32F(src2[r.x])*b2;
 465
 466                 dst[j] = (float)(s0);
 467             }
 468
 469             if( j < r.width )
 470             {
 471                 float prev = (1 - a)*(b1*CV_8TO32F(src[j]) + b2*CV_8TO32F(src2[j]));
 472
 473                 for( ; j < r.width; j++ )
 474                 {
 475                     float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src2[j+1]);
 476                     dst[j] = prev + t;
 477                     prev = (float)(t*s);
 478                 }
 479             }
 480
 481             for( ; j < win_size.width; j++ )
 482             {
 483                 float s0 = CV_8TO32F(src[r.width])*b1 +
 484                            CV_8TO32F(src2[r.width])*b2;
 485
 486                 dst[j] = (float)(s0);
 487             }
 488
 489             if( i < r.height )
 490                 src = src2;
 491         }
 492     }
 493
 494     return CV_OK;
 495 }
 496
 497
 498
 499 #define ICV_SHIFT             16
 500 #define ICV_SCALE(x)          cvRound((x)*(1 << ICV_SHIFT))
 501 #define ICV_MUL_SCALE(x,y)    (((x)*(y) + (1 << (ICV_SHIFT-1))) >> ICV_SHIFT)
 502 #define ICV_DESCALE(x)        (((x)+(1 << (ICV_SHIFT-1))) >> ICV_SHIFT)
 503
 504 icvCopySubpix_8u_C1R_t icvCopySubpix_8u_C1R_p = 0;
 505 icvCopySubpix_8u32f_C1R_t icvCopySubpix_8u32f_C1R_p = 0;
 506 icvCopySubpix_32f_C1R_t icvCopySubpix_32f_C1R_p = 0;
 507
 508 ICV_DEF_GET_RECT_SUB_PIX_FUNC( 8u, uchar, uchar, int, CV_NOP, ICV_SCALE, ICV_DESCALE )
 509 //ICV_DEF_GET_RECT_SUB_PIX_FUNC( 8u32f, uchar, float, float, CV_8TO32F, CV_NOP, CV_NOP )
 510 ICV_DEF_GET_RECT_SUB_PIX_FUNC( 32f, float, float, float, CV_NOP, CV_NOP, CV_NOP )
 511
 512 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 8u, uchar, uchar, int, CV_NOP, ICV_SCALE, ICV_MUL_SCALE )
 513 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 8u32f, uchar, float, float, CV_8TO32F, CV_NOP, CV_MUL )
 514 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 32f, float, float, float, CV_NOP, CV_NOP, CV_MUL )
 515
 516
 517 #define  ICV_DEF_INIT_SUBPIX_TAB( FUNCNAME, FLAG )                  \
 518 static void icvInit##FUNCNAME##FLAG##Table( CvFuncTable* tab )      \
 519 {                                                                   \
 520     tab->fn_2d[CV_8U] = (void*)icv##FUNCNAME##_8u_##FLAG;           \
 521     tab->fn_2d[CV_32F] = (void*)icv##FUNCNAME##_32f_##FLAG;         \
 522                                                                     \
 523     tab->fn_2d[1] = (void*)icv##FUNCNAME##_8u32f_##FLAG;            \
 524 }
 525
 526
 527 ICV_DEF_INIT_SUBPIX_TAB( GetRectSubPix, C1R )
 528 ICV_DEF_INIT_SUBPIX_TAB( GetRectSubPix, C3R )
 529
 530 typedef CvStatus (CV_STDCALL *CvGetRectSubPixFunc)( const void* src, int src_step,
 531                                                     CvSize src_size, void* dst,
 532                                                     int dst_step, CvSize win_size,
 533                                                     CvPoint2D32f center );
 534
 535 CV_IMPL void
 536 cvGetRectSubPix( const void* srcarr, void* dstarr, CvPoint2D32f center )
 537 {
 538     static CvFuncTable gr_tab[2];
 539     static int inittab = 0;
 540     CV_FUNCNAME( "cvGetRectSubPix" );
 541
 542     __BEGIN__;
 543
 544     CvMat srcstub, *src = (CvMat*)srcarr;
 545     CvMat dststub, *dst = (CvMat*)dstarr;
 546     CvSize src_size, dst_size;
 547     CvGetRectSubPixFunc func;
 548     int cn, src_step, dst_step;
 549
 550     if( !inittab )
 551     {
 552         icvInitGetRectSubPixC1RTable( gr_tab + 0 );
 553         icvInitGetRectSubPixC3RTable( gr_tab + 1 );
 554         inittab = 1;
 555     }
 556
 557     if( !CV_IS_MAT(src))
 558         CV_CALL( src = cvGetMat( src, &srcstub ));
 559
 560     if( !CV_IS_MAT(dst))
 561         CV_CALL( dst = cvGetMat( dst, &dststub ));
 562
 563     cn = CV_MAT_CN( src->type );
 564
 565     if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
 566         CV_ERROR( CV_StsUnsupportedFormat, "" );
 567
 568     src_size = cvGetMatSize( src );
 569     dst_size = cvGetMatSize( dst );
 570     src_step = src->step ? src->step : CV_STUB_STEP;
 571     dst_step = dst->step ? dst->step : CV_STUB_STEP;
 572
 573     if( dst_size.width > src_size.width || dst_size.height > src_size.height )
 574         CV_ERROR( CV_StsBadSize, "destination ROI must be smaller than source ROI" );
 575
 576     if( CV_ARE_DEPTHS_EQ( src, dst ))
 577     {
 578         func = (CvGetRectSubPixFunc)(gr_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
 579     }
 580     else
 581     {
 582         if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
 583             CV_ERROR( CV_StsUnsupportedFormat, "" );
 584
 585         func = (CvGetRectSubPixFunc)(gr_tab[cn != 1].fn_2d[1]);
 586     }
 587
 588     if( !func )
 589         CV_ERROR( CV_StsUnsupportedFormat, "" );
 590
 591     IPPI_CALL( func( src->data.ptr, src_step, src_size,
 592                      dst->data.ptr, dst_step, dst_size, center ));
 593
 594     __END__;
 595 }
 596
 597
 598 #define ICV_32F8U(x)  ((uchar)cvRound(x))
 599
 600 #define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( flavor, srctype, dsttype,      \
 601                                              worktype, cast_macro, cvt )    \
 602 CvStatus CV_STDCALL                                                         \
 603 icvGetQuadrangleSubPix_##flavor##_C1R                                       \
 604 ( const srctype * src, int src_step, CvSize src_size,                       \
 605   dsttype *dst, int dst_step, CvSize win_size, const float *matrix )        \
 606 {                                                                           \
 607     int x, y;                                                               \
 608     double dx = (win_size.width - 1)*0.5;                                   \
 609     double dy = (win_size.height - 1)*0.5;                                  \
 610     double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \
 611     double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \
 612                                                                             \
 613     src_step /= sizeof(srctype);                                            \
 614     dst_step /= sizeof(dsttype);                                            \
 615                                                                             \
 616     for( y = 0; y < win_size.height; y++, dst += dst_step )                 \
 617     {                                                                       \
 618         double xs = A12*y + A13;                                            \
 619         double ys = A22*y + A23;                                            \
 620         double xe = A11*(win_size.width-1) + A12*y + A13;                   \
 621         double ye = A21*(win_size.width-1) + A22*y + A23;                   \
 622                                                                             \
 623         if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) &&   \
 624             (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) &&  \
 625             (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) &&   \
 626             (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3))    \
 627         {                                                                   \
 628             for( x = 0; x < win_size.width; x++ )                           \
 629             {                                                               \
 630                 int ixs = cvFloor( xs );                                    \
 631                 int iys = cvFloor( ys );                                    \
 632                 const srctype *ptr = src + src_step*iys + ixs;              \
 633                 double a = xs - ixs, b = ys - iys, a1 = 1.f - a;            \
 634                 worktype p0 = cvt(ptr[0])*a1 + cvt(ptr[1])*a;               \
 635                 worktype p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+1])*a;\
 636                 xs += A11;                                                  \
 637                 ys += A21;                                                  \
 638                                                                             \
 639                 dst[x] = cast_macro(p0 + b * (p1 - p0));                    \
 640             }                                                               \
 641         }                                                                   \
 642         else                                                                \
 643         {                                                                   \
 644             for( x = 0; x < win_size.width; x++ )                           \
 645             {                                                               \
 646                 int ixs = cvFloor( xs ), iys = cvFloor( ys );               \
 647                 double a = xs - ixs, b = ys - iys, a1 = 1.f - a;            \
 648                 const srctype *ptr0, *ptr1;                                 \
 649                 worktype p0, p1;                                            \
 650                 xs += A11; ys += A21;                                       \
 651                                                                             \
 652                 if( (unsigned)iys < (unsigned)(src_size.height-1) )         \
 653                     ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step;      \
 654                 else                                                        \
 655                     ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \
 656                                                                             \
 657                 if( (unsigned)ixs < (unsigned)(src_size.width-1) )          \
 658                 {                                                           \
 659                     p0 = cvt(ptr0[ixs])*a1 + cvt(ptr0[ixs+1])*a;            \
 660                     p1 = cvt(ptr1[ixs])*a1 + cvt(ptr1[ixs+1])*a;            \
 661                 }                                                           \
 662                 else                                                        \
 663                 {                                                           \
 664                     ixs = ixs < 0 ? 0 : src_size.width - 1;                 \
 665                     p0 = cvt(ptr0[ixs]); p1 = cvt(ptr1[ixs]);               \
 666                 }                                                           \
 667                 dst[x] = cast_macro(p0 + b * (p1 - p0));                    \
 668             }                                                               \
 669         }                                                                   \
 670     }                                                                       \
 671                                                                             \
 672     return CV_OK;                                                           \
 673 }
 674
 675
 676 #define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( flavor, srctype, dsttype,   \
 677                                                 worktype, cast_macro, cvt ) \
 678 static CvStatus CV_STDCALL                                                  \
 679 icvGetQuadrangleSubPix_##flavor##_C3R                                       \
 680 ( const srctype * src, int src_step, CvSize src_size,                       \
 681   dsttype *dst, int dst_step, CvSize win_size, const float *matrix )        \
 682 {                                                                           \
 683     int x, y;                                                               \
 684     double dx = (win_size.width - 1)*0.5;                                   \
 685     double dy = (win_size.height - 1)*0.5;                                  \
 686     double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \
 687     double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \
 688                                                                             \
 689     src_step /= sizeof(srctype);                                            \
 690     dst_step /= sizeof(dsttype);                                            \
 691                                                                             \
 692     for( y = 0; y < win_size.height; y++, dst += dst_step )                 \
 693     {                                                                       \
 694         double xs = A12*y + A13;                                            \
 695         double ys = A22*y + A23;                                            \
 696         double xe = A11*(win_size.width-1) + A12*y + A13;                   \
 697         double ye = A21*(win_size.width-1) + A22*y + A23;                   \
 698                                                                             \
 699         if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) &&   \
 700             (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) &&  \
 701             (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) &&   \
 702             (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3))    \
 703         {                                                                   \
 704             for( x = 0; x < win_size.width; x++ )                           \
 705             {                                                               \
 706                 int ixs = cvFloor( xs );                                    \
 707                 int iys = cvFloor( ys );                                    \
 708                 const srctype *ptr = src + src_step*iys + ixs*3;            \
 709                 double a = xs - ixs, b = ys - iys, a1 = 1.f - a;            \
 710                 worktype p0, p1;                                            \
 711                 xs += A11;                                                  \
 712                 ys += A21;                                                  \
 713                                                                             \
 714                 p0 = cvt(ptr[0])*a1 + cvt(ptr[3])*a;                        \
 715                 p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+3])*a;        \
 716                 dst[x*3] = cast_macro(p0 + b * (p1 - p0));                  \
 717                                                                             \
 718                 p0 = cvt(ptr[1])*a1 + cvt(ptr[4])*a;                        \
 719                 p1 = cvt(ptr[src_step+1])*a1 + cvt(ptr[src_step+4])*a;      \
 720                 dst[x*3+1] = cast_macro(p0 + b * (p1 - p0));                \
 721                                                                             \
 722                 p0 = cvt(ptr[2])*a1 + cvt(ptr[5])*a;                        \
 723                 p1 = cvt(ptr[src_step+2])*a1 + cvt(ptr[src_step+5])*a;      \
 724                 dst[x*3+2] = cast_macro(p0 + b * (p1 - p0));                \
 725             }                                                               \
 726         }                                                                   \
 727         else                                                                \
 728         {                                                                   \
 729             for( x = 0; x < win_size.width; x++ )                           \
 730             {                                                               \
 731                 int ixs = cvFloor(xs), iys = cvFloor(ys);                   \
 732                 double a = xs - ixs, b = ys - iys;                          \
 733                 const srctype *ptr0, *ptr1;                                 \
 734                 xs += A11; ys += A21;                                       \
 735                                                                             \
 736                 if( (unsigned)iys < (unsigned)(src_size.height-1) )         \
 737                     ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step;      \
 738                 else                                                        \
 739                     ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \
 740                                                                             \
 741                 if( (unsigned)ixs < (unsigned)(src_size.width - 1) )        \
 742                 {                                                           \
 743                     double a1 = 1.f - a;                                    \
 744                     worktype p0, p1;                                        \
 745                     ptr0 += ixs*3; ptr1 += ixs*3;                           \
 746                     p0 = cvt(ptr0[0])*a1 + cvt(ptr0[3])*a;                  \
 747                     p1 = cvt(ptr1[0])*a1 + cvt(ptr1[3])*a;                  \
 748                     dst[x*3] = cast_macro(p0 + b * (p1 - p0));              \
 749                                                                             \
 750                     p0 = cvt(ptr0[1])*a1 + cvt(ptr0[4])*a;                  \
 751                     p1 = cvt(ptr1[1])*a1 + cvt(ptr1[4])*a;                  \
 752                     dst[x*3+1] = cast_macro(p0 + b * (p1 - p0));            \
 753                                                                             \
 754                     p0 = cvt(ptr0[2])*a1 + cvt(ptr0[5])*a;                  \
 755                     p1 = cvt(ptr1[2])*a1 + cvt(ptr1[5])*a;                  \
 756                     dst[x*3+2] = cast_macro(p0 + b * (p1 - p0));            \
 757                 }                                                           \
 758                 else                                                        \
 759                 {                                                           \
 760                     double b1 = 1.f - b;                                    \
 761                     ixs = ixs < 0 ? 0 : src_size.width - 1;                 \
 762                     ptr0 += ixs*3; ptr1 += ixs*3;                           \
 763                                                                             \
 764                     dst[x*3] = cast_macro(cvt(ptr0[0])*b1 + cvt(ptr1[0])*b);\
 765                     dst[x*3+1]=cast_macro(cvt(ptr0[1])*b1 + cvt(ptr1[1])*b);\
 766                     dst[x*3+2]=cast_macro(cvt(ptr0[2])*b1 + cvt(ptr1[2])*b);\
 767                 }                                                           \
 768             }                                                               \
 769         }                                                                   \
 770     }                                                                       \
 771                                                                             \
 772     return CV_OK;                                                           \
 773 }
 774
 775
 776 /*#define srctype uchar
 777 #define dsttype uchar
 778 #define worktype float
 779 #define cvt CV_8TO32F
 780 #define cast_macro ICV_32F8U
 781
 782 #undef srctype
 783 #undef dsttype
 784 #undef worktype
 785 #undef cvt
 786 #undef cast_macro*/
 787
 788 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u, uchar, uchar, double, ICV_32F8U, CV_8TO32F )
 789 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 32f, float, float, double, CV_CAST_32F, CV_NOP )
 790 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F )
 791
 792 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 8u, uchar, uchar, double, ICV_32F8U, CV_8TO32F )
 793 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 32f, float, float, double, CV_CAST_32F, CV_NOP )
 794 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F )
 795
 796 ICV_DEF_INIT_SUBPIX_TAB( GetQuadrangleSubPix, C1R )
 797 ICV_DEF_INIT_SUBPIX_TAB( GetQuadrangleSubPix, C3R )
 798
 799 typedef CvStatus (CV_STDCALL *CvGetQuadrangleSubPixFunc)(
 800                                          const void* src, int src_step,
 801                                          CvSize src_size, void* dst,
 802                                          int dst_step, CvSize win_size,
 803                                          const float* matrix );
 804
 805 CV_IMPL void
 806 cvGetQuadrangleSubPix( const void* srcarr, void* dstarr, const CvMat* mat )
 807 {
 808     static  CvFuncTable  gq_tab[2];
 809     static  int inittab = 0;
 810     CV_FUNCNAME( "cvGetQuadrangleSubPix" );
 811
 812     __BEGIN__;
 813
 814     CvMat srcstub, *src = (CvMat*)srcarr;
 815     CvMat dststub, *dst = (CvMat*)dstarr;
 816     CvSize src_size, dst_size;
 817     CvGetQuadrangleSubPixFunc func;
 818     float m[6];
 819     int k, cn;
 820
 821     if( !inittab )
 822     {
 823         icvInitGetQuadrangleSubPixC1RTable( gq_tab + 0 );
 824         icvInitGetQuadrangleSubPixC3RTable( gq_tab + 1 );
 825         inittab = 1;
 826     }
 827
 828     if( !CV_IS_MAT(src))
 829         CV_CALL( src = cvGetMat( src, &srcstub ));
 830
 831     if( !CV_IS_MAT(dst))
 832         CV_CALL( dst = cvGetMat( dst, &dststub ));
 833
 834     if( !CV_IS_MAT(mat))
 835         CV_ERROR( CV_StsBadArg, "map matrix is not valid" );
 836
 837     cn = CV_MAT_CN( src->type );
 838
 839     if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
 840         CV_ERROR( CV_StsUnsupportedFormat, "" );
 841
 842     src_size = cvGetMatSize( src );
 843     dst_size = cvGetMatSize( dst );
 844
 845     /*if( dst_size.width > src_size.width || dst_size.height > src_size.height )
 846         CV_ERROR( CV_StsBadSize, "destination ROI must not be larger than source ROI" );*/
 847
 848     if( mat->rows != 2 || mat->cols != 3 )
 849         CV_ERROR( CV_StsBadArg,
 850         "Transformation matrix must be 2x3" );
 851
 852     if( CV_MAT_TYPE( mat->type ) == CV_32FC1 )
 853     {
 854         for( k = 0; k < 3; k++ )
 855         {
 856             m[k] = mat->data.fl[k];
 857             m[3 + k] = ((float*)(mat->data.ptr + mat->step))[k];
 858         }
 859     }
 860     else if( CV_MAT_TYPE( mat->type ) == CV_64FC1 )
 861     {
 862         for( k = 0; k < 3; k++ )
 863         {
 864             m[k] = (float)mat->data.db[k];
 865             m[3 + k] = (float)((double*)(mat->data.ptr + mat->step))[k];
 866         }
 867     }
 868     else
 869         CV_ERROR( CV_StsUnsupportedFormat,
 870             "The transformation matrix should have 32fC1 or 64fC1 type" );
 871
 872     if( CV_ARE_DEPTHS_EQ( src, dst ))
 873     {
 874         func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
 875     }
 876     else
 877     {
 878         if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
 879             CV_ERROR( CV_StsUnsupportedFormat, "" );
 880
 881         func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[1]);
 882     }
 883
 884     if( !func )
 885         CV_ERROR( CV_StsUnsupportedFormat, "" );
 886
 887     IPPI_CALL( func( src->data.ptr, src->step, src_size,
 888                      dst->data.ptr, dst->step, dst_size, m ));
 889
 890     __END__;
 891 }
 892
 893
 894 /* End of file. */