samplers.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                           License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
00015 // Third party copyrights are property of their respective owners.
00016 //
00017 // Redistribution and use in source and binary forms, with or without modification,
00018 // are permitted provided that the following conditions are met:
00019 //
00020 //   * Redistribution's of source code must retain the above copyright notice,
00021 //     this list of conditions and the following disclaimer.
00022 //
00023 //   * Redistribution's in binary form must reproduce the above copyright notice,
00024 //     this list of conditions and the following disclaimer in the documentation
00025 //     and/or other materials provided with the distribution.
00026 //
00027 //   * The name of the copyright holders may not be used to endorse or promote products
00028 //     derived from this software without specific prior written permission.
00029 //
00030 // This software is provided by the copyright holders and contributors "as is" and
00031 // any express or implied warranties, including, but not limited to, the implied
00032 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00033 // In no event shall the Intel Corporation or contributors be liable for any direct,
00034 // indirect, incidental, special, exemplary, or consequential damages
00035 // (including, but not limited to, procurement of substitute goods or services;
00036 // loss of use, data, or profits; or business interruption) however caused
00037 // and on any theory of liability, whether in contract, strict liability,
00038 // or tort (including negligence or otherwise) arising in any way out of
00039 // the use of this software, even if advised of the possibility of such damage.
00040 //
00041 //M*/
00042 
00043 #include "precomp.hpp"
00044 
00045 namespace cv
00046 {
00047 
00048 static const uchar*
00049 adjustRect( const uchar* src, size_t src_step, int pix_size,
00050            Size src_size, Size win_size,
00051            Point ip, Rect* pRect )
00052 {
00053     Rect rect;
00054 
00055     if( ip.x >= 0 )
00056     {
00057         src += ip.x*pix_size;
00058         rect.x = 0;
00059     }
00060     else
00061     {
00062         rect.x = -ip.x;
00063         if( rect.x > win_size.width )
00064             rect.x = win_size.width;
00065     }
00066 
00067     if( ip.x < src_size.width - win_size.width )
00068         rect.width = win_size.width;
00069     else
00070     {
00071         rect.width = src_size.width - ip.x - 1;
00072         if( rect.width < 0 )
00073         {
00074             src += rect.width*pix_size;
00075             rect.width = 0;
00076         }
00077         assert( rect.width <= win_size.width );
00078     }
00079 
00080     if( ip.y >= 0 )
00081     {
00082         src += ip.y * src_step;
00083         rect.y = 0;
00084     }
00085     else
00086         rect.y = -ip.y;
00087 
00088     if( ip.y < src_size.height - win_size.height )
00089         rect.height = win_size.height;
00090     else
00091     {
00092         rect.height = src_size.height - ip.y - 1;
00093         if( rect.height < 0 )
00094         {
00095             src += rect.height*src_step;
00096             rect.height = 0;
00097         }
00098     }
00099 
00100     *pRect = rect;
00101     return src - rect.x*pix_size;
00102 }
00103 
00104 
00105 enum { SUBPIX_SHIFT=16 };
00106 
00107 struct scale_fixpt
00108 {
00109     int operator()(float a) const { return cvRound(a*(1 << SUBPIX_SHIFT)); }
00110 };
00111 
00112 struct cast_8u
00113 {
00114     uchar operator()(int a) const { return (uchar)((a + (1 << (SUBPIX_SHIFT-1))) >> SUBPIX_SHIFT); }
00115 };
00116 
00117 struct cast_flt_8u
00118 {
00119     uchar operator()(float a) const { return (uchar)cvRound(a); }
00120 };
00121 
00122 template<typename _Tp>
00123 struct nop
00124 {
00125     _Tp operator()(_Tp a) const { return a; }
00126 };
00127 
00128 
00129 template<typename _Tp, typename _DTp, typename _WTp, class ScaleOp, class CastOp>
00130 void getRectSubPix_Cn_(const _Tp* src, size_t src_step, Size src_size,
00131                        _DTp* dst, size_t dst_step, Size win_size, Point2f center, int cn )
00132 {
00133     ScaleOp scale_op;
00134     CastOp cast_op;
00135     Point ip;
00136     _WTp a11, a12, a21, a22, b1, b2;
00137     float a, b;
00138     int i, j, c;
00139 
00140     center.x -= (win_size.width-1)*0.5f;
00141     center.y -= (win_size.height-1)*0.5f;
00142 
00143     ip.x = cvFloor( center.x );
00144     ip.y = cvFloor( center.y );
00145 
00146     a = center.x - ip.x;
00147     b = center.y - ip.y;
00148     a11 = scale_op((1.f-a)*(1.f-b));
00149     a12 = scale_op(a*(1.f-b));
00150     a21 = scale_op((1.f-a)*b);
00151     a22 = scale_op(a*b);
00152     b1 = scale_op(1.f - b);
00153     b2 = scale_op(b);
00154 
00155     src_step /= sizeof(src[0]);
00156     dst_step /= sizeof(dst[0]);
00157 
00158     if( 0 <= ip.x && ip.x < src_size.width - win_size.width &&
00159        0 <= ip.y && ip.y < src_size.height - win_size.height)
00160     {
00161         // extracted rectangle is totally inside the image
00162         src += ip.y * src_step + ip.x*cn;
00163         win_size.width *= cn;
00164 
00165         for( i = 0; i < win_size.height; i++, src += src_step, dst += dst_step )
00166         {
00167             for( j = 0; j <= win_size.width - 2; j += 2 )
00168             {
00169                 _WTp s0 = src[j]*a11 + src[j+cn]*a12 + src[j+src_step]*a21 + src[j+src_step+cn]*a22;
00170                 _WTp s1 = src[j+1]*a11 + src[j+cn+1]*a12 + src[j+src_step+1]*a21 + src[j+src_step+cn+1]*a22;
00171                 dst[j] = cast_op(s0);
00172                 dst[j+1] = cast_op(s1);
00173             }
00174 
00175             for( ; j < win_size.width; j++ )
00176             {
00177                 _WTp s0 = src[j]*a11 + src[j+cn]*a12 + src[j+src_step]*a21 + src[j+src_step+cn]*a22;
00178                 dst[j] = cast_op(s0);
00179             }
00180         }
00181     }
00182     else
00183     {
00184         Rect r;
00185         src = (const _Tp*)adjustRect( (const uchar*)src, src_step*sizeof(*src),
00186                                      sizeof(*src)*cn, src_size, win_size, ip, &r);
00187 
00188         for( i = 0; i < win_size.height; i++, dst += dst_step )
00189         {
00190             const _Tp *src2 = src + src_step;
00191             _WTp s0;
00192 
00193             if( i < r.y || i >= r.height )
00194                 src2 -= src_step;
00195 
00196             for( c = 0; c < cn; c++ )
00197             {
00198                 s0 = src[r.x*cn + c]*b1 + src2[r.x*cn + c]*b2;
00199                 for( j = 0; j < r.x; j++ )
00200                     dst[j*cn + c] = cast_op(s0);
00201                 s0 = src[r.width*cn + c]*b1 + src2[r.width*cn + c]*b2;
00202                 for( j = r.width; j < win_size.width; j++ )
00203                     dst[j*cn + c] = cast_op(s0);
00204             }
00205 
00206             for( j = r.x*cn; j < r.width*cn; j++ )
00207             {
00208                 s0 = src[j]*a11 + src[j+cn]*a12 + src2[j]*a21 + src2[j+cn]*a22;
00209                 dst[j] = cast_op(s0);
00210             }
00211 
00212             if( i < r.height )
00213                 src = src2;
00214         }
00215     }
00216 }
00217 
00218 
00219 static void getRectSubPix_8u32f
00220 ( const uchar* src, size_t src_step, Size src_size,
00221  float* dst, size_t dst_step, Size win_size, Point2f center0, int cn )
00222 {
00223     Point2f center = center0;
00224     Point ip;
00225 
00226     center.x -= (win_size.width-1)*0.5f;
00227     center.y -= (win_size.height-1)*0.5f;
00228 
00229     ip.x = cvFloor( center.x );
00230     ip.y = cvFloor( center.y );
00231 
00232     if( cn == 1 &&
00233        0 <= ip.x && ip.x + win_size.width < src_size.width &&
00234        0 <= ip.y && ip.y + win_size.height < src_size.height &&
00235        win_size.width > 0 && win_size.height > 0 )
00236     {
00237         float a = center.x - ip.x;
00238         float b = center.y - ip.y;
00239         a = MAX(a,0.0001f);
00240         float a12 = a*(1.f-b);
00241         float a22 = a*b;
00242         float b1 = 1.f - b;
00243         float b2 = b;
00244         double s = (1. - a)/a;
00245 
00246         src_step /= sizeof(src[0]);
00247         dst_step /= sizeof(dst[0]);
00248 
00249         // extracted rectangle is totally inside the image
00250         src += ip.y * src_step + ip.x;
00251 
00252         for( ; win_size.height--; src += src_step, dst += dst_step )
00253         {
00254             float prev = (1 - a)*(b1*src[0] + b2*src[src_step]);
00255             for( int j = 0; j < win_size.width; j++ )
00256             {
00257                 float t = a12*src[j+1] + a22*src[j+1+src_step];
00258                 dst[j] = prev + t;
00259                 prev = (float)(t*s);
00260             }
00261         }
00262     }
00263     else
00264     {
00265         getRectSubPix_Cn_<uchar, float, float, nop<float>, nop<float> >
00266         (src, src_step, src_size, dst, dst_step, win_size, center0, cn );
00267     }
00268 }
00269 
00270 static void
00271 getQuadrangleSubPix_8u32f_CnR( const uchar* src, size_t src_step, Size src_size,
00272                                float* dst, size_t dst_step, Size win_size,
00273                                const double *matrix, int cn )
00274 {
00275     int x, y, k;
00276     double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2];
00277     double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5];
00278 
00279     src_step /= sizeof(src[0]);
00280     dst_step /= sizeof(dst[0]);
00281 
00282     for( y = 0; y < win_size.height; y++, dst += dst_step )
00283     {
00284         double xs = A12*y + A13;
00285         double ys = A22*y + A23;
00286         double xe = A11*(win_size.width-1) + A12*y + A13;
00287         double ye = A21*(win_size.width-1) + A22*y + A23;
00288 
00289         if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) &&
00290             (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) &&
00291             (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) &&
00292             (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3))
00293         {
00294             for( x = 0; x < win_size.width; x++ )
00295             {
00296                 int ixs = cvFloor( xs );
00297                 int iys = cvFloor( ys );
00298                 const uchar *ptr = src + src_step*iys;
00299                 float a = (float)(xs - ixs), b = (float)(ys - iys), a1 = 1.f - a, b1 = 1.f - b;
00300                 float w00 = a1*b1, w01 = a*b1, w10 = a1*b, w11 = a*b;
00301                 xs += A11;
00302                 ys += A21;
00303 
00304                 if( cn == 1 )
00305                 {
00306                     ptr += ixs;
00307                     dst[x] = ptr[0]*w00 + ptr[1]*w01 + ptr[src_step]*w10 + ptr[src_step+1]*w11;
00308                 }
00309                 else if( cn == 3 )
00310                 {
00311                     ptr += ixs*3;
00312                     float t0 = ptr[0]*w00 + ptr[3]*w01 + ptr[src_step]*w10 + ptr[src_step+3]*w11;
00313                     float t1 = ptr[1]*w00 + ptr[4]*w01 + ptr[src_step+1]*w10 + ptr[src_step+4]*w11;
00314                     float t2 = ptr[2]*w00 + ptr[5]*w01 + ptr[src_step+2]*w10 + ptr[src_step+5]*w11;
00315 
00316                     dst[x*3] = t0;
00317                     dst[x*3+1] = t1;
00318                     dst[x*3+2] = t2;
00319                 }
00320                 else
00321                 {
00322                     ptr += ixs*cn;
00323                     for( k = 0; k < cn; k++ )
00324                         dst[x*cn+k] = ptr[k]*w00 + ptr[k+cn]*w01 +
00325                                     ptr[src_step+k]*w10 + ptr[src_step+k+cn]*w11;
00326                 }
00327             }
00328         }
00329         else
00330         {
00331             for( x = 0; x < win_size.width; x++ )
00332             {
00333                 int ixs = cvFloor( xs ), iys = cvFloor( ys );
00334                 float a = (float)(xs - ixs), b = (float)(ys - iys), a1 = 1.f - a, b1 = 1.f - b;
00335                 float w00 = a1*b1, w01 = a*b1, w10 = a1*b, w11 = a*b;
00336                 const uchar *ptr0, *ptr1;
00337                 xs += A11; ys += A21;
00338 
00339                 if( (unsigned)iys < (unsigned)(src_size.height-1) )
00340                     ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step;
00341                 else
00342                     ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step;
00343 
00344                 if( (unsigned)ixs < (unsigned)(src_size.width-1) )
00345                 {
00346                     ptr0 += ixs*cn; ptr1 += ixs*cn;
00347                     for( k = 0; k < cn; k++ )
00348                         dst[x*cn + k] = ptr0[k]*w00 + ptr0[k+cn]*w01 + ptr1[k]*w10 + ptr1[k+cn]*w11;
00349                 }
00350                 else
00351                 {
00352                     ixs = ixs < 0 ? 0 : src_size.width - 1;
00353                     ptr0 += ixs*cn; ptr1 += ixs*cn;
00354                     for( k = 0; k < cn; k++ )
00355                         dst[x*cn + k] = ptr0[k]*b1 + ptr1[k]*b;
00356                 }
00357             }
00358         }
00359     }
00360 }
00361 
00362 }
00363 
00364 
00365 void cv::getRectSubPix( InputArray _image, Size patchSize, Point2f  center,
00366                        OutputArray _patch, int patchType )
00367 {
00368     Mat image = _image.getMat();
00369     int depth = image.depth(), cn = image.channels();
00370     int ddepth = patchType < 0 ? depth : CV_MAT_DEPTH(patchType);
00371 
00372     CV_Assert( cn == 1 || cn == 3 );
00373 
00374     _patch.create(patchSize, CV_MAKETYPE(ddepth, cn));
00375     Mat patch = _patch.getMat();
00376 
00377 #if defined (HAVE_IPP) && (IPP_VERSION_X100 >= 700)
00378     CV_IPP_CHECK()
00379     {
00380         typedef IppStatus (CV_STDCALL *ippiGetRectSubPixFunc)( const void* src, int src_step,
00381                                                                 IppiSize src_size, void* dst,
00382                                                                 int dst_step, IppiSize win_size,
00383                                                                 IppiPoint_32f center,
00384                                                                 IppiPoint* minpt, IppiPoint* maxpt );
00385 
00386         IppiPoint minpt={0,0}, maxpt={0,0};
00387         IppiPoint_32f icenter = {center.x, center.y};
00388         IppiSize src_size={image.cols, image.rows}, win_size={patch.cols, patch.rows};
00389         int srctype = image.type();
00390         ippiGetRectSubPixFunc ippfunc =
00391             srctype == CV_8UC1 && ddepth == CV_8U ? (ippiGetRectSubPixFunc)ippiCopySubpixIntersect_8u_C1R :
00392             srctype == CV_8UC1 && ddepth == CV_32F ? (ippiGetRectSubPixFunc)ippiCopySubpixIntersect_8u32f_C1R :
00393             srctype == CV_32FC1 && ddepth == CV_32F ? (ippiGetRectSubPixFunc)ippiCopySubpixIntersect_32f_C1R : 0;
00394 
00395         if( ippfunc)
00396         {
00397             if (ippfunc(image.ptr(), (int)image.step, src_size, patch.ptr(),
00398                         (int)patch.step, win_size, icenter, &minpt, &maxpt) >= 0 )
00399             {
00400                 CV_IMPL_ADD(CV_IMPL_IPP);
00401                 return;
00402             }
00403             setIppErrorStatus();
00404         }
00405     }
00406 #endif
00407 
00408     if( depth == CV_8U && ddepth == CV_8U )
00409         getRectSubPix_Cn_<uchar, uchar, int, scale_fixpt, cast_8u>
00410         (image.ptr(), image.step, image.size(), patch.ptr(), patch.step, patch.size(), center, cn);
00411     else if( depth == CV_8U && ddepth == CV_32F )
00412         getRectSubPix_8u32f
00413         (image.ptr(), image.step, image.size(), patch.ptr<float>(), patch.step, patch.size(), center, cn);
00414     else if( depth == CV_32F && ddepth == CV_32F )
00415         getRectSubPix_Cn_<float, float, float, nop<float>, nop<float> >
00416         (image.ptr<float>(), image.step, image.size(), patch.ptr<float>(), patch.step, patch.size(), center, cn);
00417     else
00418         CV_Error( CV_StsUnsupportedFormat, "Unsupported combination of input and output formats");
00419 }
00420 
00421 
00422 CV_IMPL void
00423 cvGetRectSubPix( const void* srcarr, void* dstarr, CvPoint2D32f center )
00424 {
00425     cv::Mat src = cv::cvarrToMat(srcarr);
00426     const cv::Mat dst = cv::cvarrToMat(dstarr);
00427     CV_Assert( src.channels() == dst.channels() );
00428 
00429     cv::getRectSubPix(src, dst.size(), center, dst, dst.type());
00430 }
00431 
00432 
00433 CV_IMPL void
00434 cvGetQuadrangleSubPix( const void* srcarr, void* dstarr, const CvMat* mat )
00435 {
00436     const cv::Mat src = cv::cvarrToMat(srcarr), m = cv::cvarrToMat(mat);
00437     cv::Mat dst = cv::cvarrToMat(dstarr);
00438 
00439     CV_Assert( src.channels() == dst.channels() );
00440 
00441     cv::Size win_size = dst.size();
00442     double matrix[6];
00443     cv::Mat M(2, 3, CV_64F, matrix);
00444     m.convertTo(M, CV_64F);
00445     double dx = (win_size.width - 1)*0.5;
00446     double dy = (win_size.height - 1)*0.5;
00447     matrix[2] -= matrix[0]*dx + matrix[1]*dy;
00448     matrix[5] -= matrix[3]*dx + matrix[4]*dy;
00449 
00450     if( src.depth() == CV_8U && dst.depth() == CV_32F )
00451         cv::getQuadrangleSubPix_8u32f_CnR( src.ptr(), src.step, src.size(),
00452                                            dst.ptr<float>(), dst.step, dst.size(),
00453                                            matrix, src.channels());
00454     else
00455     {
00456         CV_Assert( src.depth() == dst.depth() );
00457         cv::warpAffine(src, dst, M, dst.size(),
00458                        cv::INTER_LINEAR + cv::WARP_INVERSE_MAP,
00459                        cv::BORDER_REPLICATE);
00460     }
00461 }
00462 
00463 
00464 CV_IMPL int
00465 cvSampleLine( const void* _img, CvPoint pt1, CvPoint pt2,
00466               void* _buffer, int connectivity )
00467 {
00468     cv::Mat img = cv::cvarrToMat(_img);
00469     cv::LineIterator li(img, pt1, pt2, connectivity, false);
00470     uchar* buffer = (uchar*)_buffer;
00471     size_t pixsize = img.elemSize();
00472 
00473     if( !buffer )
00474         CV_Error( CV_StsNullPtr, "" );
00475 
00476     for( int i = 0; i < li.count; i++, ++li )
00477     {
00478         for( size_t k = 0; k < pixsize; k++ )
00479             *buffer++ = li.ptr[k];
00480     }
00481 
00482     return li.count;
00483 }
00484 
00485 
00486 /* End of file. */
00487