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

   1 /*M///////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                           License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
  15 // Third party copyrights are property of their respective owners.\r
  16 //\r
  17 // Redistribution and use in source and binary forms, with or without modification,\r
  18 // are permitted provided that the following conditions are met:\r
  19 //\r
  20 //   * Redistribution's of source code must retain the above copyright notice,\r
  21 //     this list of conditions and the following disclaimer.\r
  22 //\r
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  24 //     this list of conditions and the following disclaimer in the documentation\r
  25 //     and/or other materials provided with the distribution.\r
  26 //\r
  27 //   * The name of the copyright holders may not be used to endorse or promote products\r
  28 //     derived from this software without specific prior written permission.\r
  29 //\r
  30 // This software is provided by the copyright holders and contributors "as is" and\r
  31 // any express or implied warranties, including, but not limited to, the implied\r
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  34 // indirect, incidental, special, exemplary, or consequential damages\r
  35 // (including, but not limited to, procurement of substitute goods or services;\r
  36 // loss of use, data, or profits; or business interruption) however caused\r
  37 // and on any theory of liability, whether in contract, strict liability,\r
  38 // or tort (including negligence or otherwise) arising in any way out of\r
  39 // the use of this software, even if advised of the possibility of such damage.\r
  40 //\r
  41 //M*/\r
  42 \r
  43 #include "_cv.h"\r
  44 \r
  45 /*\r
  46  * This file includes the code, contributed by Simon Perreault\r
  47  * (the function icvMedianBlur_8u_O1)\r
  48  *\r
  49  * Constant-time median filtering -- http://nomis80.org/ctmf.html\r
  50  * Copyright (C) 2006 Simon Perreault\r
  51  *\r
  52  * Contact:\r
  53  *  Laboratoire de vision et systemes numeriques\r
  54  *  Pavillon Adrien-Pouliot\r
  55  *  Universite Laval\r
  56  *  Sainte-Foy, Quebec, Canada\r
  57  *  G1K 7P4\r
  58  *\r
  59  *  perreaul@gel.ulaval.ca\r
  60  */\r
  61 \r
  62 namespace cv\r
  63 {\r
  64 \r
  65 /****************************************************************************************\\r
  66                                          Box Filter\r
  67 \****************************************************************************************/\r
  68 \r
  69 template<typename T, typename ST> struct RowSum : public BaseRowFilter\r
  70 {\r
  71     RowSum( int _ksize, int _anchor )\r
  72     {\r
  73         ksize = _ksize;\r
  74         anchor = _anchor;\r
  75     }\r
  76     \r
  77     void operator()(const uchar* src, uchar* dst, int width, int cn)\r
  78     {\r
  79         const T* S = (const T*)src;\r
  80         ST* D = (ST*)dst;\r
  81         int i = 0, k, ksz_cn = ksize*cn;\r
  82         \r
  83         width = (width - 1)*cn;\r
  84         for( k = 0; k < cn; k++, S++, D++ )\r
  85         {\r
  86             ST s = 0;\r
  87             for( i = 0; i < ksz_cn; i += cn )\r
  88                 s += S[i];\r
  89             D[0] = s;\r
  90             for( i = 0; i < width; i += cn )\r
  91             {\r
  92                 s += S[i + ksz_cn] - S[i];\r
  93                 D[i+cn] = s;\r
  94             }\r
  95         }\r
  96     }\r
  97 };\r
  98 \r
  99 \r
 100 template<typename ST, typename T> struct ColumnSum : public BaseColumnFilter\r
 101 {\r
 102     ColumnSum( int _ksize, int _anchor, double _scale )\r
 103     {\r
 104         ksize = _ksize;\r
 105         anchor = _anchor;\r
 106         scale = _scale;\r
 107         sumCount = 0;\r
 108     }\r
 109 \r
 110     void reset() { sumCount = 0; }\r
 111     \r
 112     void operator()(const uchar** src, uchar* dst, int dststep, int count, int width)\r
 113     {\r
 114         int i;\r
 115         ST* SUM;\r
 116         bool haveScale = scale != 1;\r
 117         double _scale = scale;\r
 118 \r
 119         if( width != (int)sum.size() )\r
 120         {\r
 121             sum.resize(width);\r
 122             sumCount = 0;\r
 123         }\r
 124 \r
 125         SUM = &sum[0];\r
 126         if( sumCount == 0 )\r
 127         {\r
 128             for( i = 0; i < width; i++ )\r
 129                 SUM[i] = 0;\r
 130             for( ; sumCount < ksize - 1; sumCount++, src++ )\r
 131             {\r
 132                 const ST* Sp = (const ST*)src[0];\r
 133                 for( i = 0; i <= width - 2; i += 2 )\r
 134                 {\r
 135                     ST s0 = SUM[i] + Sp[i], s1 = SUM[i+1] + Sp[i+1];\r
 136                     SUM[i] = s0; SUM[i+1] = s1;\r
 137                 }\r
 138 \r
 139                 for( ; i < width; i++ )\r
 140                     SUM[i] += Sp[i];\r
 141             }\r
 142         }\r
 143         else\r
 144         {\r
 145             CV_Assert( sumCount == ksize-1 );\r
 146             src += ksize-1;\r
 147         }\r
 148 \r
 149         for( ; count--; src++ )\r
 150         {\r
 151             const ST* Sp = (const ST*)src[0];\r
 152             const ST* Sm = (const ST*)src[1-ksize];\r
 153             T* D = (T*)dst;\r
 154             if( haveScale )\r
 155             {\r
 156                 for( i = 0; i <= width - 2; i += 2 )\r
 157                 {\r
 158                     ST s0 = SUM[i] + Sp[i], s1 = SUM[i+1] + Sp[i+1];\r
 159                     D[i] = saturate_cast<T>(s0*_scale);\r
 160                     D[i+1] = saturate_cast<T>(s1*_scale);\r
 161                     s0 -= Sm[i]; s1 -= Sm[i+1];\r
 162                     SUM[i] = s0; SUM[i+1] = s1;\r
 163                 }\r
 164 \r
 165                 for( ; i < width; i++ )\r
 166                 {\r
 167                     ST s0 = SUM[i] + Sp[i];\r
 168                     D[i] = saturate_cast<T>(s0*_scale);\r
 169                     SUM[i] = s0 - Sm[i];\r
 170                 }\r
 171             }\r
 172             else\r
 173             {\r
 174                 for( i = 0; i <= width - 2; i += 2 )\r
 175                 {\r
 176                     ST s0 = SUM[i] + Sp[i], s1 = SUM[i+1] + Sp[i+1];\r
 177                     D[i] = saturate_cast<T>(s0);\r
 178                     D[i+1] = saturate_cast<T>(s1);\r
 179                     s0 -= Sm[i]; s1 -= Sm[i+1];\r
 180                     SUM[i] = s0; SUM[i+1] = s1;\r
 181                 }\r
 182 \r
 183                 for( ; i < width; i++ )\r
 184                 {\r
 185                     ST s0 = SUM[i] + Sp[i];\r
 186                     D[i] = saturate_cast<T>(s0);\r
 187                     SUM[i] = s0 - Sm[i];\r
 188                 }\r
 189             }\r
 190             dst += dststep;\r
 191         }\r
 192     }\r
 193 \r
 194     double scale;\r
 195     int sumCount;\r
 196     vector<ST> sum;\r
 197 };\r
 198 \r
 199 \r
 200 Ptr<BaseRowFilter> getRowSumFilter(int srcType, int sumType, int ksize, int anchor)\r
 201 {\r
 202     int sdepth = CV_MAT_DEPTH(srcType), ddepth = CV_MAT_DEPTH(sumType);\r
 203     CV_Assert( CV_MAT_CN(sumType) == CV_MAT_CN(srcType) );\r
 204 \r
 205     if( anchor < 0 )\r
 206         anchor = ksize/2;\r
 207 \r
 208     if( sdepth == CV_8U && ddepth == CV_32S )\r
 209         return Ptr<BaseRowFilter>(new RowSum<uchar, int>(ksize, anchor));\r
 210     if( sdepth == CV_8U && ddepth == CV_64F )\r
 211         return Ptr<BaseRowFilter>(new RowSum<uchar, double>(ksize, anchor));\r
 212     if( sdepth == CV_16U && ddepth == CV_32S )\r
 213         return Ptr<BaseRowFilter>(new RowSum<ushort, int>(ksize, anchor));\r
 214     if( sdepth == CV_16U && ddepth == CV_64F )\r
 215         return Ptr<BaseRowFilter>(new RowSum<ushort, double>(ksize, anchor));\r
 216     if( sdepth == CV_16S && ddepth == CV_32S )\r
 217         return Ptr<BaseRowFilter>(new RowSum<short, int>(ksize, anchor));\r
 218     if( sdepth == CV_32S && ddepth == CV_32S )\r
 219         return Ptr<BaseRowFilter>(new RowSum<int, int>(ksize, anchor));\r
 220     if( sdepth == CV_16S && ddepth == CV_64F )\r
 221         return Ptr<BaseRowFilter>(new RowSum<short, double>(ksize, anchor));\r
 222     if( sdepth == CV_32F && ddepth == CV_64F )\r
 223         return Ptr<BaseRowFilter>(new RowSum<float, double>(ksize, anchor));\r
 224     if( sdepth == CV_64F && ddepth == CV_64F )\r
 225         return Ptr<BaseRowFilter>(new RowSum<double, double>(ksize, anchor));\r
 226 \r
 227     CV_Error_( CV_StsNotImplemented,\r
 228         ("Unsupported combination of source format (=%d), and buffer format (=%d)",\r
 229         srcType, sumType));\r
 230 \r
 231     return Ptr<BaseRowFilter>(0);\r
 232 }\r
 233 \r
 234 \r
 235 Ptr<BaseColumnFilter> getColumnSumFilter(int sumType, int dstType, int ksize,\r
 236                                          int anchor, double scale)\r
 237 {\r
 238     int sdepth = CV_MAT_DEPTH(sumType), ddepth = CV_MAT_DEPTH(dstType);\r
 239     CV_Assert( CV_MAT_CN(sumType) == CV_MAT_CN(dstType) );\r
 240 \r
 241     if( anchor < 0 )\r
 242         anchor = ksize/2;\r
 243 \r
 244     if( ddepth == CV_8U && sdepth == CV_32S )\r
 245         return Ptr<BaseColumnFilter>(new ColumnSum<int, uchar>(ksize, anchor, scale));\r
 246     if( ddepth == CV_8U && sdepth == CV_64F )\r
 247         return Ptr<BaseColumnFilter>(new ColumnSum<double, uchar>(ksize, anchor, scale));\r
 248     if( ddepth == CV_16U && sdepth == CV_32S )\r
 249         return Ptr<BaseColumnFilter>(new ColumnSum<int, ushort>(ksize, anchor, scale));\r
 250     if( ddepth == CV_16U && sdepth == CV_64F )\r
 251         return Ptr<BaseColumnFilter>(new ColumnSum<double, ushort>(ksize, anchor, scale));\r
 252     if( ddepth == CV_16S && sdepth == CV_32S )\r
 253         return Ptr<BaseColumnFilter>(new ColumnSum<int, short>(ksize, anchor, scale));\r
 254     if( ddepth == CV_16S && sdepth == CV_64F )\r
 255         return Ptr<BaseColumnFilter>(new ColumnSum<double, short>(ksize, anchor, scale));\r
 256     if( ddepth == CV_32S && sdepth == CV_32S )\r
 257         return Ptr<BaseColumnFilter>(new ColumnSum<int, int>(ksize, anchor, scale));\r
 258     if( ddepth == CV_32F && sdepth == CV_32S )\r
 259         return Ptr<BaseColumnFilter>(new ColumnSum<int, float>(ksize, anchor, scale));\r
 260     if( ddepth == CV_32F && sdepth == CV_64F )\r
 261         return Ptr<BaseColumnFilter>(new ColumnSum<double, float>(ksize, anchor, scale));\r
 262     if( ddepth == CV_64F && sdepth == CV_32S )\r
 263         return Ptr<BaseColumnFilter>(new ColumnSum<int, double>(ksize, anchor, scale));\r
 264     if( ddepth == CV_64F && sdepth == CV_64F )\r
 265         return Ptr<BaseColumnFilter>(new ColumnSum<double, double>(ksize, anchor, scale));\r
 266 \r
 267     CV_Error_( CV_StsNotImplemented,\r
 268         ("Unsupported combination of sum format (=%d), and destination format (=%d)",\r
 269         sumType, dstType));\r
 270 \r
 271     return Ptr<BaseColumnFilter>(0);\r
 272 }\r
 273 \r
 274 \r
 275 Ptr<FilterEngine> createBoxFilter( int srcType, int dstType, Size ksize,\r
 276                     Point anchor, bool normalize, int borderType )\r
 277 {\r
 278     int sdepth = CV_MAT_DEPTH(srcType);\r
 279     int cn = CV_MAT_CN(srcType), sumType = CV_64F;\r
 280     if( sdepth < CV_32S && (!normalize ||\r
 281         ksize.width*ksize.height <= (sdepth == CV_8U ? (1<<23) :\r
 282             sdepth == CV_16U ? (1 << 15) : (1 << 16))) )\r
 283         sumType = CV_32S;\r
 284     sumType = CV_MAKETYPE( sumType, cn );\r
 285 \r
 286     Ptr<BaseRowFilter> rowFilter = getRowSumFilter(srcType, sumType, ksize.width, anchor.x );\r
 287     Ptr<BaseColumnFilter> columnFilter = getColumnSumFilter(sumType,\r
 288         dstType, ksize.height, anchor.y, normalize ? 1./(ksize.width*ksize.height) : 1);\r
 289 \r
 290     return Ptr<FilterEngine>(new FilterEngine(Ptr<BaseFilter>(0), rowFilter, columnFilter,\r
 291            srcType, dstType, sumType, borderType ));\r
 292 }\r
 293 \r
 294 \r
 295 void boxFilter( const Mat& src, Mat& dst, int ddepth,\r
 296                 Size ksize, Point anchor,\r
 297                 bool normalize, int borderType )\r
 298 {\r
 299     int sdepth = src.depth(), cn = src.channels();\r
 300     if( ddepth < 0 )\r
 301         ddepth = sdepth;\r
 302     dst.create( src.size(), CV_MAKETYPE(ddepth, cn) );\r
 303     if( borderType != BORDER_CONSTANT && normalize )\r
 304     {\r
 305         if( src.rows == 1 )\r
 306             ksize.height = 1;\r
 307         if( src.cols == 1 )\r
 308             ksize.width = 1;\r
 309     }\r
 310     Ptr<FilterEngine> f = createBoxFilter( src.type(), dst.type(),\r
 311                         ksize, anchor, normalize, borderType );\r
 312     f->apply( src, dst );\r
 313 }\r
 314 \r
 315 /****************************************************************************************\\r
 316                                      Gaussian Blur\r
 317 \****************************************************************************************/\r
 318 \r
 319 Mat getGaussianKernel( int n, double sigma, int ktype )\r
 320 {\r
 321     const int SMALL_GAUSSIAN_SIZE = 7;\r
 322     static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] =\r
 323     {\r
 324         {1.f},\r
 325         {0.25f, 0.5f, 0.25f},\r
 326         {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f},\r
 327         {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f}\r
 328     };\r
 329 \r
 330     const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ?\r
 331         small_gaussian_tab[n>>1] : 0;\r
 332 \r
 333     CV_Assert( ktype == CV_32F || ktype == CV_64F );\r
 334     Mat kernel(n, 1, ktype);\r
 335     float* cf = (float*)kernel.data;\r
 336     double* cd = (double*)kernel.data;\r
 337 \r
 338     double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8;\r
 339     double scale2X = -0.5/(sigmaX*sigmaX);\r
 340     double sum = 0;\r
 341 \r
 342     int i;\r
 343     for( i = 0; i < n; i++ )\r
 344     {\r
 345         double x = i - (n-1)*0.5;\r
 346         double t = fixed_kernel ? (double)fixed_kernel[i] : std::exp(scale2X*x*x);\r
 347         if( ktype == CV_32F )\r
 348         {\r
 349             cf[i] = (float)t;\r
 350             sum += cf[i];\r
 351         }\r
 352         else\r
 353         {\r
 354             cd[i] = t;\r
 355             sum += cd[i];\r
 356         }\r
 357     }\r
 358 \r
 359     sum = 1./sum;\r
 360     for( i = 0; i < n; i++ )\r
 361     {\r
 362         if( ktype == CV_32F )\r
 363             cf[i] = (float)(cf[i]*sum);\r
 364         else\r
 365             cd[i] *= sum;\r
 366     }\r
 367 \r
 368     return kernel;\r
 369 }\r
 370 \r
 371 \r
 372 Ptr<FilterEngine> createGaussianFilter( int type, Size ksize,\r
 373                                         double sigma1, double sigma2,\r
 374                                         int borderType )\r
 375 {\r
 376     int depth = CV_MAT_DEPTH(type);\r
 377     if( sigma2 <= 0 )\r
 378         sigma2 = sigma1;\r
 379 \r
 380     // automatic detection of kernel size from sigma\r
 381     if( ksize.width <= 0 && sigma1 > 0 )\r
 382         ksize.width = cvRound(sigma1*(depth == CV_8U ? 3 : 4)*2 + 1)|1;\r
 383     if( ksize.height <= 0 && sigma2 > 0 )\r
 384         ksize.height = cvRound(sigma2*(depth == CV_8U ? 3 : 4)*2 + 1)|1;\r
 385 \r
 386     CV_Assert( ksize.width > 0 && ksize.width % 2 == 1 &&\r
 387         ksize.height > 0 && ksize.height % 2 == 1 );\r
 388 \r
 389     sigma1 = std::max( sigma1, 0. );\r
 390     sigma2 = std::max( sigma2, 0. );\r
 391 \r
 392     Mat kx = getGaussianKernel( ksize.width, sigma1, std::max(depth, CV_32F) );\r
 393     Mat ky;\r
 394     if( ksize.height == ksize.width && std::abs(sigma1 - sigma2) < DBL_EPSILON )\r
 395         ky = kx;\r
 396     else\r
 397         ky = getGaussianKernel( ksize.height, sigma2, std::max(depth, CV_32F) );\r
 398 \r
 399     return createSeparableLinearFilter( type, type, kx, ky, Point(-1,-1), 0, borderType );\r
 400 }\r
 401 \r
 402 \r
 403 void GaussianBlur( const Mat& src, Mat& dst, Size ksize,\r
 404                    double sigma1, double sigma2,\r
 405                    int borderType )\r
 406 {\r
 407     if( ksize.width == 1 && ksize.height == 1 )\r
 408     {\r
 409         src.copyTo(dst);\r
 410         return;\r
 411     }\r
 412 \r
 413     dst.create( src.size(), src.type() );\r
 414     if( borderType != BORDER_CONSTANT )\r
 415     {\r
 416         if( src.rows == 1 )\r
 417             ksize.height = 1;\r
 418         if( src.cols == 1 )\r
 419             ksize.width = 1;\r
 420     }\r
 421     Ptr<FilterEngine> f = createGaussianFilter( src.type(), ksize, sigma1, sigma2, borderType );\r
 422     f->apply( src, dst );\r
 423 }\r
 424 \r
 425 \r
 426 /****************************************************************************************\\r
 427                                       Median Filter\r
 428 \****************************************************************************************/\r
 429 \r
 430 //#undef CV_SSE2\r
 431 \r
 432 //#if defined __VEC__ || defined __ALTIVEC__\r
 433 //#define CV_ALTIVEC 1\r
 434 //#endif\r
 435 \r
 436 #undef CV_ALTIVEC\r
 437 \r
 438 #if CV_ALTIVEC\r
 439 #include <altivec.h>\r
 440 #undef bool\r
 441 #endif\r
 442 \r
 443 #if _MSC_VER >= 1200\r
 444 #pragma warning( disable: 4244 )\r
 445 #endif\r
 446 \r
 447 typedef ushort HT;\r
 448 \r
 449 /**\r
 450  * This structure represents a two-tier histogram. The first tier (known as the\r
 451  * "coarse" level) is 4 bit wide and the second tier (known as the "fine" level)\r
 452  * is 8 bit wide. Pixels inserted in the fine level also get inserted into the\r
 453  * coarse bucket designated by the 4 MSBs of the fine bucket value.\r
 454  *\r
 455  * The structure is aligned on 16 bits, which is a prerequisite for SIMD\r
 456  * instructions. Each bucket is 16 bit wide, which means that extra care must be\r
 457  * taken to prevent overflow.\r
 458  */\r
 459 typedef struct\r
 460 {\r
 461     HT coarse[16];\r
 462     HT fine[16][16];\r
 463 } Histogram;\r
 464 \r
 465 \r
 466 #if CV_SSE2 || defined __MMX__ || CV_ALTIVEC\r
 467 #define MEDIAN_HAVE_SIMD 1\r
 468 #else\r
 469 #define MEDIAN_HAVE_SIMD 0\r
 470 #endif\r
 471 \r
 472 /**\r
 473  * histogram_add - adds histograms x and y.\r
 474  * histogram_sub - subtracts histogram x from y.\r
 475  */\r
 476 #if CV_SSE2\r
 477 static inline void histogram_add( const HT x[16], HT y[16] )\r
 478 {\r
 479     const __m128i* rx = (const __m128i*)x;\r
 480     __m128i* ry = (__m128i*)y;\r
 481     __m128i r0 = _mm_add_epi16(_mm_load_si128(ry+0),_mm_load_si128(rx+0));\r
 482     __m128i r1 = _mm_add_epi16(_mm_load_si128(ry+1),_mm_load_si128(rx+1));\r
 483     _mm_store_si128(ry+0, r0);\r
 484     _mm_store_si128(ry+1, r1);\r
 485 }\r
 486 \r
 487 static inline void histogram_sub( const HT x[16], HT y[16] )\r
 488 {\r
 489     const __m128i* rx = (const __m128i*)x;\r
 490     __m128i* ry = (__m128i*)y;\r
 491     __m128i r0 = _mm_sub_epi16(_mm_load_si128(ry+0),_mm_load_si128(rx+0));\r
 492     __m128i r1 = _mm_sub_epi16(_mm_load_si128(ry+1),_mm_load_si128(rx+1));\r
 493     _mm_store_si128(ry+0, r0);\r
 494     _mm_store_si128(ry+1, r1);\r
 495 }\r
 496 #elif defined(__MMX__)\r
 497 static inline void histogram_add( const HT x[16], HT y[16] )\r
 498 {\r
 499     *(__m64*) &y[0]  = _mm_add_pi16( *(__m64*) &y[0],  *(__m64*) &x[0]  );\r
 500     *(__m64*) &y[4]  = _mm_add_pi16( *(__m64*) &y[4],  *(__m64*) &x[4]  );\r
 501     *(__m64*) &y[8]  = _mm_add_pi16( *(__m64*) &y[8],  *(__m64*) &x[8]  );\r
 502     *(__m64*) &y[12] = _mm_add_pi16( *(__m64*) &y[12], *(__m64*) &x[12] );\r
 503 }\r
 504 \r
 505 static inline void histogram_sub( const HT x[16], HT y[16] )\r
 506 {\r
 507     *(__m64*) &y[0]  = _mm_sub_pi16( *(__m64*) &y[0],  *(__m64*) &x[0]  );\r
 508     *(__m64*) &y[4]  = _mm_sub_pi16( *(__m64*) &y[4],  *(__m64*) &x[4]  );\r
 509     *(__m64*) &y[8]  = _mm_sub_pi16( *(__m64*) &y[8],  *(__m64*) &x[8]  );\r
 510     *(__m64*) &y[12] = _mm_sub_pi16( *(__m64*) &y[12], *(__m64*) &x[12] );\r
 511 }\r
 512 #elif CV_ALTIVEC\r
 513 static inline void histogram_add( const HT x[16], HT y[16] )\r
 514 {\r
 515     *(vector HT*) &y[0] = vec_add( *(vector HT*) &y[0], *(vector HT*) &x[0] );\r
 516     *(vector HT*) &y[8] = vec_add( *(vector HT*) &y[8], *(vector HT*) &x[8] );\r
 517 }\r
 518 \r
 519 static inline void histogram_sub( const HT x[16], HT y[16] )\r
 520 {\r
 521     *(vector HT*) &y[0] = vec_sub( *(vector HT*) &y[0], *(vector HT*) &x[0] );\r
 522     *(vector HT*) &y[8] = vec_sub( *(vector HT*) &y[8], *(vector HT*) &x[8] );\r
 523 }\r
 524 #else\r
 525 static inline void histogram_add( const HT x[16], HT y[16] )\r
 526 {\r
 527     int i;\r
 528     for( i = 0; i < 16; ++i )\r
 529         y[i] = (HT)(y[i] + x[i]);\r
 530 }\r
 531 \r
 532 static inline void histogram_sub( const HT x[16], HT y[16] )\r
 533 {\r
 534     int i;\r
 535     for( i = 0; i < 16; ++i )\r
 536         y[i] = (HT)(y[i] - x[i]);\r
 537 }\r
 538 #endif\r
 539 \r
 540 static inline void histogram_muladd( int a, const HT x[16],\r
 541         HT y[16] )\r
 542 {\r
 543     for( int i = 0; i < 16; ++i )\r
 544         y[i] = (HT)(y[i] + a * x[i]);\r
 545 }\r
 546 \r
 547 static void\r
 548 medianBlur_8u_O1( const Mat& _src, Mat& _dst, int ksize )\r
 549 {\r
 550 /**\r
 551  * HOP is short for Histogram OPeration. This macro makes an operation \a op on\r
 552  * histogram \a h for pixel value \a x. It takes care of handling both levels.\r
 553  */\r
 554 #define HOP(h,x,op) \\r
 555     h.coarse[x>>4] op, \\r
 556     *((HT*)h.fine + x) op\r
 557 \r
 558 #define COP(c,j,x,op) \\r
 559     h_coarse[ 16*(n*c+j) + (x>>4) ] op, \\r
 560     h_fine[ 16 * (n*(16*c+(x>>4)) + j) + (x & 0xF) ] op\r
 561 \r
 562     int cn = _dst.channels(), m = _dst.rows, r = (ksize-1)/2;\r
 563     size_t sstep = _src.step, dstep = _dst.step;\r
 564     Histogram CV_DECL_ALIGNED(16) H[4];\r
 565     HT luc[4][16];\r
 566 \r
 567     int STRIPE_SIZE = std::min( _dst.cols, 512/cn );\r
 568 \r
 569     vector<HT> _h_coarse(1 * 16 * (STRIPE_SIZE + 2*r) * cn);\r
 570     vector<HT> _h_fine(16 * 16 * (STRIPE_SIZE + 2*r) * cn);\r
 571     HT* h_coarse = &_h_coarse[0];\r
 572     HT* h_fine = &_h_fine[0];\r
 573 \r
 574     for( int x = 0; x < _dst.cols; x += STRIPE_SIZE )\r
 575     {\r
 576         int i, j, k, c, n = std::min(_dst.cols - x, STRIPE_SIZE) + r*2;\r
 577         const uchar* src = _src.data + x*cn;\r
 578         uchar* dst = _dst.data + (x - r)*cn;\r
 579 \r
 580         memset( h_coarse, 0, 16*n*cn*sizeof(h_coarse[0]) );\r
 581         memset( h_fine, 0, 16*16*n*cn*sizeof(h_fine[0]) );\r
 582 \r
 583         // First row initialization\r
 584         for( c = 0; c < cn; c++ )\r
 585         {\r
 586             for( j = 0; j < n; j++ )\r
 587                 COP( c, j, src[cn*j+c], += r+2 );\r
 588 \r
 589             for( i = 1; i < r; i++ )\r
 590             {\r
 591                 const uchar* p = src + sstep*std::min(i, m-1);\r
 592                 for ( j = 0; j < n; j++ )\r
 593                     COP( c, j, p[cn*j+c], ++ );\r
 594             }\r
 595         }\r
 596 \r
 597         for( i = 0; i < m; i++ )\r
 598         {\r
 599             const uchar* p0 = src + sstep * std::max( 0, i-r-1 );\r
 600             const uchar* p1 = src + sstep * std::min( m-1, i+r );\r
 601 \r
 602             memset( H, 0, cn*sizeof(H[0]) );\r
 603             memset( luc, 0, cn*sizeof(luc[0]) );\r
 604             for( c = 0; c < cn; c++ )\r
 605             {\r
 606                 // Update column histograms for the entire row.\r
 607                 for( j = 0; j < n; j++ )\r
 608                 {\r
 609                     COP( c, j, p0[j*cn + c], -- );\r
 610                     COP( c, j, p1[j*cn + c], ++ );\r
 611                 }\r
 612 \r
 613                 // First column initialization\r
 614                 for( j = 0; j < 2*r; ++j )\r
 615                     histogram_add( &h_coarse[16*(n*c+j)], H[c].coarse );\r
 616                 for( k = 0; k < 16; ++k )\r
 617                     histogram_muladd( 2*r+1, &h_fine[16*n*(16*c+k)], &H[c].fine[k][0] );\r
 618 \r
 619                 for( j = r; j < n-r; j++ )\r
 620                 {\r
 621                     int t = 2*r*r + 2*r, b, sum = 0;\r
 622                     HT* segment;\r
 623 \r
 624                     histogram_add( &h_coarse[16*(n*c + std::min(j+r,n-1))], H[c].coarse );\r
 625 \r
 626                     // Find median at coarse level\r
 627                     for ( k = 0; k < 16 ; ++k )\r
 628                     {\r
 629                         sum += H[c].coarse[k];\r
 630                         if ( sum > t )\r
 631                         {\r
 632                             sum -= H[c].coarse[k];\r
 633                             break;\r
 634                         }\r
 635                     }\r
 636                     assert( k < 16 );\r
 637 \r
 638                     /* Update corresponding histogram segment */\r
 639                     if ( luc[c][k] <= j-r )\r
 640                     {\r
 641                         memset( &H[c].fine[k], 0, 16 * sizeof(HT) );\r
 642                         for ( luc[c][k] = j-r; luc[c][k] < MIN(j+r+1,n); ++luc[c][k] )\r
 643                             histogram_add( &h_fine[16*(n*(16*c+k)+luc[c][k])], H[c].fine[k] );\r
 644 \r
 645                         if ( luc[c][k] < j+r+1 )\r
 646                         {\r
 647                             histogram_muladd( j+r+1 - n, &h_fine[16*(n*(16*c+k)+(n-1))], &H[c].fine[k][0] );\r
 648                             luc[c][k] = (HT)(j+r+1);\r
 649                         }\r
 650                     }\r
 651                     else\r
 652                     {\r
 653                         for ( ; luc[c][k] < j+r+1; ++luc[c][k] )\r
 654                         {\r
 655                             histogram_sub( &h_fine[16*(n*(16*c+k)+MAX(luc[c][k]-2*r-1,0))], H[c].fine[k] );\r
 656                             histogram_add( &h_fine[16*(n*(16*c+k)+MIN(luc[c][k],n-1))], H[c].fine[k] );\r
 657                         }\r
 658                     }\r
 659 \r
 660                     histogram_sub( &h_coarse[16*(n*c+MAX(j-r,0))], H[c].coarse );\r
 661 \r
 662                     /* Find median in segment */\r
 663                     segment = H[c].fine[k];\r
 664                     for ( b = 0; b < 16 ; b++ )\r
 665                     {\r
 666                         sum += segment[b];\r
 667                         if ( sum > t )\r
 668                         {\r
 669                             dst[dstep*i+cn*j+c] = (uchar)(16*k + b);\r
 670                             break;\r
 671                         }\r
 672                     }\r
 673                     assert( b < 16 );\r
 674                 }\r
 675             }\r
 676         }\r
 677     }\r
 678     #if defined(__MMX__)\r
 679         _mm_empty();\r
 680     #endif\r
 681 \r
 682 #undef HOP\r
 683 #undef COP\r
 684 }\r
 685 \r
 686 \r
 687 #if _MSC_VER >= 1200\r
 688 #pragma warning( default: 4244 )\r
 689 #endif\r
 690 \r
 691 static void\r
 692 medianBlur_8u_Om( const Mat& _src, Mat& _dst, int m )\r
 693 {\r
 694     #define N  16\r
 695     int     zone0[4][N];\r
 696     int     zone1[4][N*N];\r
 697     int     x, y;\r
 698     int     n2 = m*m/2;\r
 699     Size    size = _dst.size();\r
 700     const uchar* src = _src.data;\r
 701     uchar*  dst = _dst.data;\r
 702     int     src_step = (int)_src.step, dst_step = (int)_dst.step;\r
 703     int     cn = _src.channels();\r
 704     const uchar*  src_max = src + size.height*src_step;\r
 705 \r
 706     #define UPDATE_ACC01( pix, cn, op ) \\r
 707     {                                   \\r
 708         int p = (pix);                  \\r
 709         zone1[cn][p] op;                \\r
 710         zone0[cn][p >> 4] op;           \\r
 711     }\r
 712 \r
 713     //CV_Assert( size.height >= nx && size.width >= nx );\r
 714     for( x = 0; x < size.width; x++, src += cn, dst += cn )\r
 715     {\r
 716         uchar* dst_cur = dst;\r
 717         const uchar* src_top = src;\r
 718         const uchar* src_bottom = src;\r
 719         int k, c;\r
 720         int src_step1 = src_step, dst_step1 = dst_step;\r
 721 \r
 722         if( x % 2 != 0 )\r
 723         {\r
 724             src_bottom = src_top += src_step*(size.height-1);\r
 725             dst_cur += dst_step*(size.height-1);\r
 726             src_step1 = -src_step1;\r
 727             dst_step1 = -dst_step1;\r
 728         }\r
 729 \r
 730         // init accumulator\r
 731         memset( zone0, 0, sizeof(zone0[0])*cn );\r
 732         memset( zone1, 0, sizeof(zone1[0])*cn );\r
 733 \r
 734         for( y = 0; y <= m/2; y++ )\r
 735         {\r
 736             for( c = 0; c < cn; c++ )\r
 737             {\r
 738                 if( y > 0 )\r
 739                 {\r
 740                     for( k = 0; k < m*cn; k += cn )\r
 741                         UPDATE_ACC01( src_bottom[k+c], c, ++ );\r
 742                 }\r
 743                 else\r
 744                 {\r
 745                     for( k = 0; k < m*cn; k += cn )\r
 746                         UPDATE_ACC01( src_bottom[k+c], c, += m/2+1 );\r
 747                 }\r
 748             }\r
 749 \r
 750             if( (src_step1 > 0 && y < size.height-1) ||\r
 751                 (src_step1 < 0 && size.height-y-1 > 0) )\r
 752                 src_bottom += src_step1;\r
 753         }\r
 754 \r
 755         for( y = 0; y < size.height; y++, dst_cur += dst_step1 )\r
 756         {\r
 757             // find median\r
 758             for( c = 0; c < cn; c++ )\r
 759             {\r
 760                 int s = 0;\r
 761                 for( k = 0; ; k++ )\r
 762                 {\r
 763                     int t = s + zone0[c][k];\r
 764                     if( t > n2 ) break;\r
 765                     s = t;\r
 766                 }\r
 767 \r
 768                 for( k *= N; ;k++ )\r
 769                 {\r
 770                     s += zone1[c][k];\r
 771                     if( s > n2 ) break;\r
 772                 }\r
 773 \r
 774                 dst_cur[c] = (uchar)k;\r
 775             }\r
 776 \r
 777             if( y+1 == size.height )\r
 778                 break;\r
 779 \r
 780             if( cn == 1 )\r
 781             {\r
 782                 for( k = 0; k < m; k++ )\r
 783                 {\r
 784                     int p = src_top[k];\r
 785                     int q = src_bottom[k];\r
 786                     zone1[0][p]--;\r
 787                     zone0[0][p>>4]--;\r
 788                     zone1[0][q]++;\r
 789                     zone0[0][q>>4]++;\r
 790                 }\r
 791             }\r
 792             else if( cn == 3 )\r
 793             {\r
 794                 for( k = 0; k < m*3; k += 3 )\r
 795                 {\r
 796                     UPDATE_ACC01( src_top[k], 0, -- );\r
 797                     UPDATE_ACC01( src_top[k+1], 1, -- );\r
 798                     UPDATE_ACC01( src_top[k+2], 2, -- );\r
 799 \r
 800                     UPDATE_ACC01( src_bottom[k], 0, ++ );\r
 801                     UPDATE_ACC01( src_bottom[k+1], 1, ++ );\r
 802                     UPDATE_ACC01( src_bottom[k+2], 2, ++ );\r
 803                 }\r
 804             }\r
 805             else\r
 806             {\r
 807                 assert( cn == 4 );\r
 808                 for( k = 0; k < m*4; k += 4 )\r
 809                 {\r
 810                     UPDATE_ACC01( src_top[k], 0, -- );\r
 811                     UPDATE_ACC01( src_top[k+1], 1, -- );\r
 812                     UPDATE_ACC01( src_top[k+2], 2, -- );\r
 813                     UPDATE_ACC01( src_top[k+3], 3, -- );\r
 814 \r
 815                     UPDATE_ACC01( src_bottom[k], 0, ++ );\r
 816                     UPDATE_ACC01( src_bottom[k+1], 1, ++ );\r
 817                     UPDATE_ACC01( src_bottom[k+2], 2, ++ );\r
 818                     UPDATE_ACC01( src_bottom[k+3], 3, ++ );\r
 819                 }\r
 820             }\r
 821 \r
 822             if( (src_step1 > 0 && src_bottom + src_step1 < src_max) ||\r
 823                 (src_step1 < 0 && src_bottom + src_step1 >= src) )\r
 824                 src_bottom += src_step1;\r
 825 \r
 826             if( y >= m/2 )\r
 827                 src_top += src_step1;\r
 828         }\r
 829     }\r
 830 #undef N\r
 831 #undef UPDATE_ACC\r
 832 }\r
 833 \r
 834 \r
 835 struct MinMax8u\r
 836 {\r
 837     typedef uchar value_type;\r
 838     typedef int arg_type;\r
 839     enum { SIZE = 1 };\r
 840     arg_type load(const uchar* ptr) { return *ptr; }\r
 841     void store(uchar* ptr, arg_type val) { *ptr = (uchar)val; }\r
 842     void operator()(arg_type& a, arg_type& b) const\r
 843     {\r
 844         int t = CV_FAST_CAST_8U(a - b);\r
 845         b += t; a -= t;\r
 846     }\r
 847 };\r
 848 \r
 849 struct MinMax16u\r
 850 {\r
 851     typedef ushort value_type;\r
 852     typedef int arg_type;\r
 853     enum { SIZE = 1 };\r
 854     arg_type load(const ushort* ptr) { return *ptr; }\r
 855     void store(ushort* ptr, arg_type val) { *ptr = (ushort)val; }\r
 856     void operator()(arg_type& a, arg_type& b) const\r
 857     {\r
 858         arg_type t = a;\r
 859         a = std::min(a, b);\r
 860         b = std::max(b, t);\r
 861     }\r
 862 };\r
 863 \r
 864 struct MinMax32f\r
 865 {\r
 866     typedef float value_type;\r
 867     typedef float arg_type;\r
 868     enum { SIZE = 1 };\r
 869     arg_type load(const float* ptr) { return *ptr; }\r
 870     void store(float* ptr, arg_type val) { *ptr = val; }\r
 871     void operator()(arg_type& a, arg_type& b) const\r
 872     {\r
 873         arg_type t = a;\r
 874         a = std::min(a, b);\r
 875         b = std::max(b, t);\r
 876     }\r
 877 };\r
 878 \r
 879 #if CV_SSE2\r
 880 \r
 881 struct MinMaxVec8u\r
 882 {\r
 883     typedef uchar value_type;\r
 884     typedef __m128i arg_type;\r
 885     enum { SIZE = 16 };\r
 886     arg_type load(const uchar* ptr) { return _mm_loadu_si128((const __m128i*)ptr); }\r
 887     void store(uchar* ptr, arg_type val) { _mm_storeu_si128((__m128i*)ptr, val); }\r
 888     void operator()(arg_type& a, arg_type& b) const\r
 889     {\r
 890         arg_type t = a;\r
 891         a = _mm_min_epu8(a, b);\r
 892         b = _mm_max_epu8(b, t);\r
 893     }\r
 894 };\r
 895 \r
 896 \r
 897 struct MinMaxVec16u\r
 898 {\r
 899     typedef ushort value_type;\r
 900     typedef __m128i arg_type;\r
 901     enum { SIZE = 8 };\r
 902     arg_type load(const ushort* ptr) { return _mm_loadu_si128((const __m128i*)ptr); }\r
 903     void store(ushort* ptr, arg_type val) { _mm_storeu_si128((__m128i*)ptr, val); }\r
 904     void operator()(arg_type& a, arg_type& b) const\r
 905     {\r
 906         arg_type t = _mm_subs_epu16(a, b);\r
 907         a = _mm_subs_epu16(a, t);\r
 908         b = _mm_adds_epu16(b, t);\r
 909     }\r
 910 };\r
 911 \r
 912 \r
 913 struct MinMaxVec32f\r
 914 {\r
 915     typedef float value_type;\r
 916     typedef __m128 arg_type;\r
 917     enum { SIZE = 4 };\r
 918     arg_type load(const float* ptr) { return _mm_loadu_ps(ptr); }\r
 919     void store(float* ptr, arg_type val) { _mm_storeu_ps(ptr, val); }\r
 920     void operator()(arg_type& a, arg_type& b) const\r
 921     {\r
 922         arg_type t = a;\r
 923         a = _mm_min_ps(a, b);\r
 924         b = _mm_max_ps(b, t);\r
 925     }\r
 926 };\r
 927 \r
 928 \r
 929 #else\r
 930 \r
 931 typedef MinMax8u MinMaxVec8u;\r
 932 typedef MinMax16u MinMaxVec16u;\r
 933 typedef MinMax32f MinMaxVec32f;\r
 934 \r
 935 #endif\r
 936 \r
 937 template<class Op, class VecOp>\r
 938 static void\r
 939 medianBlur_SortNet( const Mat& _src, Mat& _dst, int m )\r
 940 {\r
 941     typedef typename Op::value_type T;\r
 942     typedef typename Op::arg_type WT;\r
 943     typedef typename VecOp::arg_type VT;\r
 944 \r
 945     const T* src = (const T*)_src.data;\r
 946     T* dst = (T*)_dst.data;\r
 947     int sstep = (int)(_src.step/sizeof(T));\r
 948     int dstep = (int)(_dst.step/sizeof(T));\r
 949     Size size = _dst.size();\r
 950     int i, j, k, cn = _src.channels();\r
 951     Op op;\r
 952     VecOp vop;\r
 953 \r
 954     if( m == 3 )\r
 955     {\r
 956         if( size.width == 1 || size.height == 1 )\r
 957         {\r
 958             int len = size.width + size.height - 1;\r
 959             int sdelta = size.height == 1 ? cn : sstep;\r
 960             int sdelta0 = size.height == 1 ? 0 : sstep - cn;\r
 961             int ddelta = size.height == 1 ? cn : dstep;\r
 962 \r
 963             for( i = 0; i < len; i++, src += sdelta0, dst += ddelta )\r
 964                 for( j = 0; j < cn; j++, src++ )\r
 965                 {\r
 966                     WT p0 = src[i > 0 ? -sdelta : 0];\r
 967                     WT p1 = src[0];\r
 968                     WT p2 = src[i < len - 1 ? sdelta : 0];\r
 969 \r
 970                     op(p0, p1); op(p1, p2); op(p0, p1);\r
 971                     dst[j] = (T)p1;\r
 972                 }\r
 973             return;\r
 974         }\r
 975         \r
 976         size.width *= cn;\r
 977         for( i = 0; i < size.height; i++, dst += dstep )\r
 978         {\r
 979             const T* row0 = src + std::max(i - 1, 0)*sstep;\r
 980             const T* row1 = src + i*sstep;\r
 981             const T* row2 = src + std::min(i + 1, size.height-1)*sstep;\r
 982             int limit = cn;\r
 983 \r
 984             for(j = 0;; )\r
 985             {\r
 986                 for( ; j < limit; j++ )\r
 987                 {\r
 988                     int j0 = j >= cn ? j - cn : j;\r
 989                     int j2 = j < size.width - cn ? j + cn : j;\r
 990                     WT p0 = row0[j0], p1 = row0[j], p2 = row0[j2];\r
 991                     WT p3 = row1[j0], p4 = row1[j], p5 = row1[j2];\r
 992                     WT p6 = row2[j0], p7 = row2[j], p8 = row2[j2];\r
 993 \r
 994                     op(p1, p2); op(p4, p5); op(p7, p8); op(p0, p1);\r
 995                     op(p3, p4); op(p6, p7); op(p1, p2); op(p4, p5);\r
 996                     op(p7, p8); op(p0, p3); op(p5, p8); op(p4, p7);\r
 997                     op(p3, p6); op(p1, p4); op(p2, p5); op(p4, p7);\r
 998                     op(p4, p2); op(p6, p4); op(p4, p2);\r
 999                     dst[j] = (T)p4;\r
1000                 }\r
1001 \r
1002                 if( limit == size.width )\r
1003                     break;\r
1004 \r
1005                 for( ; j <= size.width - VecOp::SIZE - cn; j += VecOp::SIZE )\r
1006                 {\r
1007                     VT p0 = vop.load(row0+j-cn), p1 = vop.load(row0+j), p2 = vop.load(row0+j+cn);\r
1008                     VT p3 = vop.load(row1+j-cn), p4 = vop.load(row1+j), p5 = vop.load(row1+j+cn);\r
1009                     VT p6 = vop.load(row2+j-cn), p7 = vop.load(row2+j), p8 = vop.load(row2+j+cn);\r
1010 \r
1011                     vop(p1, p2); vop(p4, p5); vop(p7, p8); vop(p0, p1);\r
1012                     vop(p3, p4); vop(p6, p7); vop(p1, p2); vop(p4, p5);\r
1013                     vop(p7, p8); vop(p0, p3); vop(p5, p8); vop(p4, p7);\r
1014                     vop(p3, p6); vop(p1, p4); vop(p2, p5); vop(p4, p7);\r
1015                     vop(p4, p2); vop(p6, p4); vop(p4, p2);\r
1016                     vop.store(dst+j, p4);\r
1017                 }\r
1018 \r
1019                 limit = size.width;\r
1020             }\r
1021         }\r
1022     }\r
1023     else if( m == 5 )\r
1024     {\r
1025         if( size.width == 1 || size.height == 1 )\r
1026         {\r
1027             int len = size.width + size.height - 1;\r
1028             int sdelta = size.height == 1 ? cn : sstep;\r
1029             int sdelta0 = size.height == 1 ? 0 : sstep - cn;\r
1030             int ddelta = size.height == 1 ? cn : dstep;\r
1031 \r
1032             for( i = 0; i < len; i++, src += sdelta0, dst += ddelta )\r
1033                 for( j = 0; j < cn; j++, src++ )\r
1034                 {\r
1035                     int i1 = i > 0 ? -sdelta : 0;\r
1036                     int i0 = i > 1 ? -sdelta*2 : i1;\r
1037                     int i3 = i < len-1 ? sdelta : 0;\r
1038                     int i4 = i < len-2 ? sdelta*2 : i3;\r
1039                     WT p0 = src[i0], p1 = src[i1], p2 = src[0], p3 = src[i3], p4 = src[i4];\r
1040 \r
1041                     op(p0, p1); op(p3, p4); op(p2, p3); op(p3, p4); op(p0, p2);\r
1042                     op(p2, p4); op(p1, p3); op(p1, p2);\r
1043                     dst[j] = (T)p2;\r
1044                 }\r
1045             return;\r
1046         }\r
1047 \r
1048         size.width *= cn;\r
1049         for( i = 0; i < size.height; i++, dst += dstep )\r
1050         {\r
1051             const T* row[5];\r
1052             row[0] = src + std::max(i - 2, 0)*sstep;\r
1053             row[1] = src + std::max(i - 1, 0)*sstep;\r
1054             row[2] = src + i*sstep;\r
1055             row[3] = src + std::min(i + 1, size.height-1)*sstep;\r
1056             row[4] = src + std::min(i + 2, size.height-1)*sstep;\r
1057             int limit = cn*2;\r
1058 \r
1059             for(j = 0;; )\r
1060             {\r
1061                 for( ; j < limit; j++ )\r
1062                 {\r
1063                     WT p[25];\r
1064                     int j1 = j >= cn ? j - cn : j;\r
1065                     int j0 = j >= cn*2 ? j - cn*2 : j1;\r
1066                     int j3 = j < size.width - cn ? j + cn : j;\r
1067                     int j4 = j < size.width - cn*2 ? j + cn*2 : j3;\r
1068                     for( k = 0; k < 5; k++ )\r
1069                     {\r
1070                         const T* rowk = row[k];\r
1071                         p[k*5] = rowk[j0]; p[k*5+1] = rowk[j1];\r
1072                         p[k*5+2] = rowk[j]; p[k*5+3] = rowk[j3];\r
1073                         p[k*5+4] = rowk[j4];\r
1074                     }\r
1075                     \r
1076                     op(p[1], p[2]); op(p[0], p[1]); op(p[1], p[2]); op(p[4], p[5]); op(p[3], p[4]);\r
1077                     op(p[4], p[5]); op(p[0], p[3]); op(p[2], p[5]); op(p[2], p[3]); op(p[1], p[4]);\r
1078                     op(p[1], p[2]); op(p[3], p[4]); op(p[7], p[8]); op(p[6], p[7]); op(p[7], p[8]);\r
1079                     op(p[10], p[11]); op(p[9], p[10]); op(p[10], p[11]); op(p[6], p[9]); op(p[8], p[11]);\r
1080                     op(p[8], p[9]); op(p[7], p[10]); op(p[7], p[8]); op(p[9], p[10]); op(p[0], p[6]);\r
1081                     op(p[4], p[10]); op(p[4], p[6]); op(p[2], p[8]); op(p[2], p[4]); op(p[6], p[8]);\r
1082                     op(p[1], p[7]); op(p[5], p[11]); op(p[5], p[7]); op(p[3], p[9]); op(p[3], p[5]);\r
1083                     op(p[7], p[9]); op(p[1], p[2]); op(p[3], p[4]); op(p[5], p[6]); op(p[7], p[8]);\r
1084                     op(p[9], p[10]); op(p[13], p[14]); op(p[12], p[13]); op(p[13], p[14]); op(p[16], p[17]);\r
1085                     op(p[15], p[16]); op(p[16], p[17]); op(p[12], p[15]); op(p[14], p[17]); op(p[14], p[15]);\r
1086                     op(p[13], p[16]); op(p[13], p[14]); op(p[15], p[16]); op(p[19], p[20]); op(p[18], p[19]);\r
1087                     op(p[19], p[20]); op(p[21], p[22]); op(p[23], p[24]); op(p[21], p[23]); op(p[22], p[24]);\r
1088                     op(p[22], p[23]); op(p[18], p[21]); op(p[20], p[23]); op(p[20], p[21]); op(p[19], p[22]);\r
1089                     op(p[22], p[24]); op(p[19], p[20]); op(p[21], p[22]); op(p[23], p[24]); op(p[12], p[18]);\r
1090                     op(p[16], p[22]); op(p[16], p[18]); op(p[14], p[20]); op(p[20], p[24]); op(p[14], p[16]);\r
1091                     op(p[18], p[20]); op(p[22], p[24]); op(p[13], p[19]); op(p[17], p[23]); op(p[17], p[19]);\r
1092                     op(p[15], p[21]); op(p[15], p[17]); op(p[19], p[21]); op(p[13], p[14]); op(p[15], p[16]);\r
1093                     op(p[17], p[18]); op(p[19], p[20]); op(p[21], p[22]); op(p[23], p[24]); op(p[0], p[12]);\r
1094                     op(p[8], p[20]); op(p[8], p[12]); op(p[4], p[16]); op(p[16], p[24]); op(p[12], p[16]);\r
1095                     op(p[2], p[14]); op(p[10], p[22]); op(p[10], p[14]); op(p[6], p[18]); op(p[6], p[10]);\r
1096                     op(p[10], p[12]); op(p[1], p[13]); op(p[9], p[21]); op(p[9], p[13]); op(p[5], p[17]);\r
1097                     op(p[13], p[17]); op(p[3], p[15]); op(p[11], p[23]); op(p[11], p[15]); op(p[7], p[19]);\r
1098                     op(p[7], p[11]); op(p[11], p[13]); op(p[11], p[12]);\r
1099                     dst[j] = (T)p[12];\r
1100                 }\r
1101 \r
1102                 if( limit == size.width )\r
1103                     break;\r
1104 \r
1105                 for( ; j <= size.width - VecOp::SIZE - cn*2; j += VecOp::SIZE )\r
1106                 {\r
1107                     VT p[25];\r
1108                     for( k = 0; k < 5; k++ )\r
1109                     {\r
1110                         const T* rowk = row[k];\r
1111                         p[k*5] = vop.load(rowk+j-cn*2); p[k*5+1] = vop.load(rowk+j-cn);\r
1112                         p[k*5+2] = vop.load(rowk+j); p[k*5+3] = vop.load(rowk+j+cn);\r
1113                         p[k*5+4] = vop.load(rowk+j+cn*2);\r
1114                     }\r
1115                     \r
1116                     vop(p[1], p[2]); vop(p[0], p[1]); vop(p[1], p[2]); vop(p[4], p[5]); vop(p[3], p[4]);\r
1117                     vop(p[4], p[5]); vop(p[0], p[3]); vop(p[2], p[5]); vop(p[2], p[3]); vop(p[1], p[4]);\r
1118                     vop(p[1], p[2]); vop(p[3], p[4]); vop(p[7], p[8]); vop(p[6], p[7]); vop(p[7], p[8]);\r
1119                     vop(p[10], p[11]); vop(p[9], p[10]); vop(p[10], p[11]); vop(p[6], p[9]); vop(p[8], p[11]);\r
1120                     vop(p[8], p[9]); vop(p[7], p[10]); vop(p[7], p[8]); vop(p[9], p[10]); vop(p[0], p[6]);\r
1121                     vop(p[4], p[10]); vop(p[4], p[6]); vop(p[2], p[8]); vop(p[2], p[4]); vop(p[6], p[8]);\r
1122                     vop(p[1], p[7]); vop(p[5], p[11]); vop(p[5], p[7]); vop(p[3], p[9]); vop(p[3], p[5]);\r
1123                     vop(p[7], p[9]); vop(p[1], p[2]); vop(p[3], p[4]); vop(p[5], p[6]); vop(p[7], p[8]);\r
1124                     vop(p[9], p[10]); vop(p[13], p[14]); vop(p[12], p[13]); vop(p[13], p[14]); vop(p[16], p[17]);\r
1125                     vop(p[15], p[16]); vop(p[16], p[17]); vop(p[12], p[15]); vop(p[14], p[17]); vop(p[14], p[15]);\r
1126                     vop(p[13], p[16]); vop(p[13], p[14]); vop(p[15], p[16]); vop(p[19], p[20]); vop(p[18], p[19]);\r
1127                     vop(p[19], p[20]); vop(p[21], p[22]); vop(p[23], p[24]); vop(p[21], p[23]); vop(p[22], p[24]);\r
1128                     vop(p[22], p[23]); vop(p[18], p[21]); vop(p[20], p[23]); vop(p[20], p[21]); vop(p[19], p[22]);\r
1129                     vop(p[22], p[24]); vop(p[19], p[20]); vop(p[21], p[22]); vop(p[23], p[24]); vop(p[12], p[18]);\r
1130                     vop(p[16], p[22]); vop(p[16], p[18]); vop(p[14], p[20]); vop(p[20], p[24]); vop(p[14], p[16]);\r
1131                     vop(p[18], p[20]); vop(p[22], p[24]); vop(p[13], p[19]); vop(p[17], p[23]); vop(p[17], p[19]);\r
1132                     vop(p[15], p[21]); vop(p[15], p[17]); vop(p[19], p[21]); vop(p[13], p[14]); vop(p[15], p[16]);\r
1133                     vop(p[17], p[18]); vop(p[19], p[20]); vop(p[21], p[22]); vop(p[23], p[24]); vop(p[0], p[12]);\r
1134                     vop(p[8], p[20]); vop(p[8], p[12]); vop(p[4], p[16]); vop(p[16], p[24]); vop(p[12], p[16]);\r
1135                     vop(p[2], p[14]); vop(p[10], p[22]); vop(p[10], p[14]); vop(p[6], p[18]); vop(p[6], p[10]);\r
1136                     vop(p[10], p[12]); vop(p[1], p[13]); vop(p[9], p[21]); vop(p[9], p[13]); vop(p[5], p[17]);\r
1137                     vop(p[13], p[17]); vop(p[3], p[15]); vop(p[11], p[23]); vop(p[11], p[15]); vop(p[7], p[19]);\r
1138                     vop(p[7], p[11]); vop(p[11], p[13]); vop(p[11], p[12]);\r
1139                     vop.store(dst+j, p[12]);\r
1140                 }\r
1141 \r
1142                 limit = size.width;\r
1143             }\r
1144         }\r
1145     }\r
1146 }\r
1147 \r
1148 \r
1149 void medianBlur( const Mat& src0, Mat& dst, int ksize )\r
1150 {\r
1151     if( ksize <= 1 )\r
1152     {\r
1153         src0.copyTo(dst);\r
1154         return;\r
1155     }\r
1156 \r
1157     CV_Assert( ksize % 2 == 1 );\r
1158     \r
1159     Size size = src0.size();\r
1160     int cn = src0.channels();\r
1161     bool useSortNet = ksize == 3 || (ksize == 5\r
1162 #if !CV_SSE2\r
1163             && src0.depth() > CV_8U\r
1164 #endif\r
1165         );\r
1166 \r
1167     dst.create( src0.size(), src0.type() );\r
1168     Mat src;\r
1169     if( useSortNet )\r
1170     {\r
1171         if( dst.data != src0.data )\r
1172             src = src0;\r
1173         else\r
1174             src0.copyTo(src);\r
1175     }\r
1176     else\r
1177         cv::copyMakeBorder( src0, src, 0, 0, ksize/2, ksize/2, BORDER_REPLICATE );\r
1178 \r
1179     if( useSortNet )\r
1180     {\r
1181         if( src.depth() == CV_8U )\r
1182             medianBlur_SortNet<MinMax8u, MinMaxVec8u>( src, dst, ksize );\r
1183         else if( src.depth() == CV_16U )\r
1184             medianBlur_SortNet<MinMax16u, MinMaxVec16u>( src, dst, ksize );\r
1185         else if( src.depth() == CV_32F )\r
1186             medianBlur_SortNet<MinMax32f, MinMaxVec32f>( src, dst, ksize );\r
1187         return;\r
1188     }\r
1189 \r
1190     CV_Assert( src.depth() == CV_8U && (cn == 1 || cn == 3 || cn == 4) );\r
1191 \r
1192     double img_size_mp = (double)(size.width*size.height)/(1 << 20);\r
1193     if( ksize <= 3 + (img_size_mp < 1 ? 12 : img_size_mp < 4 ? 6 : 2)*(MEDIAN_HAVE_SIMD ? 1 : 3))\r
1194         medianBlur_8u_Om( src, dst, ksize );\r
1195     else\r
1196         medianBlur_8u_O1( src, dst, ksize );\r
1197 }\r
1198 \r
1199 /****************************************************************************************\\r
1200                                    Bilateral Filtering\r
1201 \****************************************************************************************/\r
1202 \r
1203 static void\r
1204 bilateralFilter_8u( const Mat& src, Mat& dst, int d,\r
1205                     double sigma_color, double sigma_space,\r
1206                     int borderType )\r
1207 {\r
1208     double gauss_color_coeff = -0.5/(sigma_color*sigma_color);\r
1209     double gauss_space_coeff = -0.5/(sigma_space*sigma_space);\r
1210     int cn = src.channels();\r
1211     int i, j, k, maxk, radius;\r
1212     Size size = src.size();\r
1213 \r
1214     CV_Assert( (src.type() == CV_8UC1 || src.type() == CV_8UC3) &&\r
1215         src.type() == dst.type() && src.size() == dst.size() &&\r
1216         src.data != dst.data );\r
1217 \r
1218     if( sigma_color <= 0 )\r
1219         sigma_color = 1;\r
1220     if( sigma_space <= 0 )\r
1221         sigma_space = 1;\r
1222 \r
1223     if( d <= 0 )\r
1224         radius = cvRound(sigma_space*1.5);\r
1225     else\r
1226         radius = d/2;\r
1227     radius = MAX(radius, 1);\r
1228     d = radius*2 + 1;\r
1229 \r
1230     Mat temp;\r
1231     copyMakeBorder( src, temp, radius, radius, radius, radius, borderType );\r
1232 \r
1233     vector<float> _color_weight(cn*256);\r
1234     vector<float> _space_weight(d*d);\r
1235     vector<int> _space_ofs(d*d);\r
1236     float* color_weight = &_color_weight[0];\r
1237     float* space_weight = &_space_weight[0];\r
1238     int* space_ofs = &_space_ofs[0];\r
1239 \r
1240     // initialize color-related bilateral filter coefficients\r
1241     for( i = 0; i < 256*cn; i++ )\r
1242         color_weight[i] = (float)std::exp(i*i*gauss_color_coeff);\r
1243 \r
1244     // initialize space-related bilateral filter coefficients\r
1245     for( i = -radius, maxk = 0; i <= radius; i++ )\r
1246         for( j = -radius; j <= radius; j++ )\r
1247         {\r
1248             double r = std::sqrt((double)i*i + (double)j*j);\r
1249             if( r > radius )\r
1250                 continue;\r
1251             space_weight[maxk] = (float)std::exp(r*r*gauss_space_coeff);\r
1252             space_ofs[maxk++] = (int)(i*temp.step + j*cn);\r
1253         }\r
1254 \r
1255     for( i = 0; i < size.height; i++ )\r
1256     {\r
1257         const uchar* sptr = temp.data + (i+radius)*temp.step + radius*cn;\r
1258         uchar* dptr = dst.data + i*dst.step;\r
1259 \r
1260         if( cn == 1 )\r
1261         {\r
1262             for( j = 0; j < size.width; j++ )\r
1263             {\r
1264                 float sum = 0, wsum = 0;\r
1265                 int val0 = sptr[j];\r
1266                 for( k = 0; k < maxk; k++ )\r
1267                 {\r
1268                     int val = sptr[j + space_ofs[k]];\r
1269                     float w = space_weight[k]*color_weight[std::abs(val - val0)];\r
1270                     sum += val*w;\r
1271                     wsum += w;\r
1272                 }\r
1273                 // overflow is not possible here => there is no need to use CV_CAST_8U\r
1274                 dptr[j] = (uchar)cvRound(sum/wsum);\r
1275             }\r
1276         }\r
1277         else\r
1278         {\r
1279             assert( cn == 3 );\r
1280             for( j = 0; j < size.width*3; j += 3 )\r
1281             {\r
1282                 float sum_b = 0, sum_g = 0, sum_r = 0, wsum = 0;\r
1283                 int b0 = sptr[j], g0 = sptr[j+1], r0 = sptr[j+2];\r
1284                 for( k = 0; k < maxk; k++ )\r
1285                 {\r
1286                     const uchar* sptr_k = sptr + j + space_ofs[k];\r
1287                     int b = sptr_k[0], g = sptr_k[1], r = sptr_k[2];\r
1288                     float w = space_weight[k]*color_weight[std::abs(b - b0) +\r
1289                         std::abs(g - g0) + std::abs(r - r0)];\r
1290                     sum_b += b*w; sum_g += g*w; sum_r += r*w;\r
1291                     wsum += w;\r
1292                 }\r
1293                 wsum = 1.f/wsum;\r
1294                 b0 = cvRound(sum_b*wsum);\r
1295                 g0 = cvRound(sum_g*wsum);\r
1296                 r0 = cvRound(sum_r*wsum);\r
1297                 dptr[j] = (uchar)b0; dptr[j+1] = (uchar)g0; dptr[j+2] = (uchar)r0;\r
1298             }\r
1299         }\r
1300     }\r
1301 }\r
1302 \r
1303 \r
1304 static void\r
1305 bilateralFilter_32f( const Mat& src, Mat& dst, int d,\r
1306                      double sigma_color, double sigma_space,\r
1307                      int borderType )\r
1308 {\r
1309     double gauss_color_coeff = -0.5/(sigma_color*sigma_color);\r
1310     double gauss_space_coeff = -0.5/(sigma_space*sigma_space);\r
1311     int cn = src.channels();\r
1312     int i, j, k, maxk, radius;\r
1313     double minValSrc=-1, maxValSrc=1;\r
1314     const int kExpNumBinsPerChannel = 1 << 12;\r
1315     int kExpNumBins = 0;\r
1316     float lastExpVal = 1.f;\r
1317     float len, scale_index;\r
1318     Size size = src.size();\r
1319 \r
1320     CV_Assert( (src.type() == CV_32FC1 || src.type() == CV_32FC3) &&\r
1321         src.type() == dst.type() && src.size() == dst.size() &&\r
1322         src.data != dst.data );\r
1323 \r
1324     if( sigma_color <= 0 )\r
1325         sigma_color = 1;\r
1326     if( sigma_space <= 0 )\r
1327         sigma_space = 1;\r
1328 \r
1329     if( d <= 0 )\r
1330         radius = cvRound(sigma_space*1.5);\r
1331     else\r
1332         radius = d/2;\r
1333     radius = MAX(radius, 1);\r
1334     d = radius*2 + 1;\r
1335     // compute the min/max range for the input image (even if multichannel)\r
1336     \r
1337     minMaxLoc( src.reshape(1), &minValSrc, &maxValSrc );\r
1338     \r
1339     // temporary copy of the image with borders for easy processing\r
1340     Mat temp;\r
1341     copyMakeBorder( src, temp, radius, radius, radius, radius, borderType );\r
1342 \r
1343     // allocate lookup tables\r
1344     vector<float> _space_weight(d*d);\r
1345     vector<int> _space_ofs(d*d);\r
1346     float* space_weight = &_space_weight[0];\r
1347     int* space_ofs = &_space_ofs[0];\r
1348 \r
1349     // assign a length which is slightly more than needed\r
1350     len = (float)(maxValSrc - minValSrc) * cn;\r
1351     kExpNumBins = kExpNumBinsPerChannel * cn;\r
1352     vector<float> _expLUT(kExpNumBins+2);\r
1353     float* expLUT = &_expLUT[0];\r
1354 \r
1355     scale_index = kExpNumBins/len;\r
1356     \r
1357     // initialize the exp LUT\r
1358     for( i = 0; i < kExpNumBins+2; i++ )\r
1359     {\r
1360         if( lastExpVal > 0.f )\r
1361         {\r
1362             double val =  i / scale_index;\r
1363             expLUT[i] = (float)std::exp(val * val * gauss_color_coeff);\r
1364             lastExpVal = expLUT[i];\r
1365         }\r
1366         else\r
1367             expLUT[i] = 0.f;\r
1368     }\r
1369     \r
1370     // initialize space-related bilateral filter coefficients\r
1371     for( i = -radius, maxk = 0; i <= radius; i++ )\r
1372         for( j = -radius; j <= radius; j++ )\r
1373         {\r
1374             double r = std::sqrt((double)i*i + (double)j*j);\r
1375             if( r > radius )\r
1376                 continue;\r
1377             space_weight[maxk] = (float)std::exp(r*r*gauss_space_coeff);\r
1378             space_ofs[maxk++] = (int)(i*(temp.step/sizeof(float)) + j*cn);\r
1379         }\r
1380 \r
1381     for( i = 0; i < size.height; i++ )\r
1382     {\r
1383             const float* sptr = (const float*)(temp.data + (i+radius)*temp.step) + radius*cn;\r
1384         float* dptr = (float*)(dst.data + i*dst.step);\r
1385 \r
1386         if( cn == 1 )\r
1387         {\r
1388             for( j = 0; j < size.width; j++ )\r
1389             {\r
1390                 float sum = 0, wsum = 0;\r
1391                 float val0 = sptr[j];\r
1392                 for( k = 0; k < maxk; k++ )\r
1393                 {\r
1394                     float val = sptr[j + space_ofs[k]];\r
1395                                         float alpha = (float)(std::abs(val - val0)*scale_index);\r
1396                     int idx = cvFloor(alpha);\r
1397                     alpha -= idx;\r
1398                     float w = space_weight[k]*(expLUT[idx] + alpha*(expLUT[idx+1] - expLUT[idx]));\r
1399                         sum += val*w;\r
1400                     wsum += w;\r
1401                 }\r
1402                 dptr[j] = (float)(sum/wsum);\r
1403             }\r
1404         }\r
1405         else\r
1406         {\r
1407             assert( cn == 3 );\r
1408             for( j = 0; j < size.width*3; j += 3 )\r
1409             {\r
1410                 float sum_b = 0, sum_g = 0, sum_r = 0, wsum = 0;\r
1411                 float b0 = sptr[j], g0 = sptr[j+1], r0 = sptr[j+2];\r
1412                 for( k = 0; k < maxk; k++ )\r
1413                 {\r
1414                     const float* sptr_k = sptr + j + space_ofs[k];\r
1415                     float b = sptr_k[0], g = sptr_k[1], r = sptr_k[2];\r
1416                                         float alpha = (float)((std::abs(b - b0) +\r
1417                         std::abs(g - g0) + std::abs(r - r0))*scale_index);\r
1418                     int idx = cvFloor(alpha);\r
1419                     alpha -= idx;\r
1420                     float w = space_weight[k]*(expLUT[idx] + alpha*(expLUT[idx+1] - expLUT[idx]));\r
1421                     sum_b += b*w; sum_g += g*w; sum_r += r*w;\r
1422                     wsum += w;\r
1423                 }\r
1424                 wsum = 1.f/wsum;\r
1425                 b0 = sum_b*wsum;\r
1426                 g0 = sum_g*wsum;\r
1427                 r0 = sum_r*wsum;\r
1428                 dptr[j] = b0; dptr[j+1] = g0; dptr[j+2] = r0;\r
1429             }\r
1430         }\r
1431     }\r
1432 }\r
1433 \r
1434 \r
1435 void bilateralFilter( const Mat& src, Mat& dst, int d,\r
1436                       double sigmaColor, double sigmaSpace,\r
1437                       int borderType )\r
1438 {\r
1439     dst.create( src.size(), src.type() );\r
1440     if( src.depth() == CV_8U )\r
1441         bilateralFilter_8u( src, dst, d, sigmaColor, sigmaSpace, borderType );\r
1442     else if( src.depth() == CV_32F )\r
1443         bilateralFilter_32f( src, dst, d, sigmaColor, sigmaSpace, borderType );\r
1444     else\r
1445         CV_Error( CV_StsUnsupportedFormat,\r
1446         "Bilateral filtering is only implemented for 8u and 32f images" );\r
1447 }\r
1448 \r
1449 }\r
1450 \r
1451 //////////////////////////////////////////////////////////////////////////////////////////\r
1452 \r
1453 CV_IMPL void\r
1454 cvSmooth( const void* srcarr, void* dstarr, int smooth_type,\r
1455           int param1, int param2, double param3, double param4 )\r
1456 {\r
1457     cv::Mat src = cv::cvarrToMat(srcarr), dst0 = cv::cvarrToMat(dstarr), dst = dst0;\r
1458 \r
1459     CV_Assert( dst.size() == src.size() &&\r
1460         (smooth_type == CV_BLUR_NO_SCALE || dst.type() == src.type()) );\r
1461 \r
1462     if( param2 <= 0 )\r
1463         param2 = param1;\r
1464 \r
1465     if( smooth_type == CV_BLUR || smooth_type == CV_BLUR_NO_SCALE )\r
1466         cv::boxFilter( src, dst, dst.depth(), cv::Size(param1, param2), cv::Point(-1,-1),\r
1467             smooth_type == CV_BLUR, cv::BORDER_REPLICATE );\r
1468     else if( smooth_type == CV_GAUSSIAN )\r
1469         cv::GaussianBlur( src, dst, cv::Size(param1, param2), param3, param4, cv::BORDER_REPLICATE );\r
1470     else if( smooth_type == CV_MEDIAN )\r
1471         cv::medianBlur( src, dst, param1 );\r
1472     else\r
1473         cv::bilateralFilter( src, dst, param1, param3, param4, cv::BORDER_REPLICATE );\r
1474 \r
1475     if( dst.data != dst0.data )\r
1476         CV_Error( CV_StsUnmatchedFormats, "The destination image does not have the proper type" );\r
1477 }\r
1478 \r
1479 /* End of file. */\r