modules/imgproc/src/smooth.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 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44
  45 /*
  46  * This file includes the code, contributed by Simon Perreault
  47  * (the function icvMedianBlur_8u_O1)
  48  *
  49  * Constant-time median filtering -- http://nomis80.org/ctmf.html
  50  * Copyright (C) 2006 Simon Perreault
  51  *
  52  * Contact:
  53  *  Laboratoire de vision et systemes numeriques
  54  *  Pavillon Adrien-Pouliot
  55  *  Universite Laval
  56  *  Sainte-Foy, Quebec, Canada
  57  *  G1K 7P4
  58  *
  59  *  perreaul@gel.ulaval.ca
  60  */
  61
  62 namespace cv
  63 {
  64
  65 /****************************************************************************************\
  66                                          Box Filter
  67 \****************************************************************************************/
  68
  69 template<typename T, typename ST> struct RowSum : public BaseRowFilter
  70 {
  71     RowSum( int _ksize, int _anchor )
  72     {
  73         ksize = _ksize;
  74         anchor = _anchor;
  75     }
  76
  77     void operator()(const uchar* src, uchar* dst, int width, int cn)
  78     {
  79         const T* S = (const T*)src;
  80         ST* D = (ST*)dst;
  81         int i = 0, k, ksz_cn = ksize*cn;
  82
  83         width = (width - 1)*cn;
  84         for( k = 0; k < cn; k++, S++, D++ )
  85         {
  86             ST s = 0;
  87             for( i = 0; i < ksz_cn; i += cn )
  88                 s += S[i];
  89             D[0] = s;
  90             for( i = 0; i < width; i += cn )
  91             {
  92                 s += S[i + ksz_cn] - S[i];
  93                 D[i+cn] = s;
  94             }
  95         }
  96     }
  97 };
  98
  99
 100 template<typename ST, typename T> struct ColumnSum : public BaseColumnFilter
 101 {
 102     ColumnSum( int _ksize, int _anchor, double _scale )
 103     {
 104         ksize = _ksize;
 105         anchor = _anchor;
 106         scale = _scale;
 107         sumCount = 0;
 108     }
 109
 110     void reset() { sumCount = 0; }
 111
 112     void operator()(const uchar** src, uchar* dst, int dststep, int count, int width)
 113     {
 114         int i;
 115         ST* SUM;
 116         bool haveScale = scale != 1;
 117         double _scale = scale;
 118
 119         if( width != (int)sum.size() )
 120         {
 121             sum.resize(width);
 122             sumCount = 0;
 123         }
 124
 125         SUM = &sum[0];
 126         if( sumCount == 0 )
 127         {
 128             for( i = 0; i < width; i++ )
 129                 SUM[i] = 0;
 130             for( ; sumCount < ksize - 1; sumCount++, src++ )
 131             {
 132                 const ST* Sp = (const ST*)src[0];
 133                 for( i = 0; i <= width - 2; i += 2 )
 134                 {
 135                     ST s0 = SUM[i] + Sp[i], s1 = SUM[i+1] + Sp[i+1];
 136                     SUM[i] = s0; SUM[i+1] = s1;
 137                 }
 138
 139                 for( ; i < width; i++ )
 140                     SUM[i] += Sp[i];
 141             }
 142         }
 143         else
 144         {
 145             CV_Assert( sumCount == ksize-1 );
 146             src += ksize-1;
 147         }
 148
 149         for( ; count--; src++ )
 150         {
 151             const ST* Sp = (const ST*)src[0];
 152             const ST* Sm = (const ST*)src[1-ksize];
 153             T* D = (T*)dst;
 154             if( haveScale )
 155             {
 156                 for( i = 0; i <= width - 2; i += 2 )
 157                 {
 158                     ST s0 = SUM[i] + Sp[i], s1 = SUM[i+1] + Sp[i+1];
 159                     D[i] = saturate_cast<T>(s0*_scale);
 160                     D[i+1] = saturate_cast<T>(s1*_scale);
 161                     s0 -= Sm[i]; s1 -= Sm[i+1];
 162                     SUM[i] = s0; SUM[i+1] = s1;
 163                 }
 164
 165                 for( ; i < width; i++ )
 166                 {
 167                     ST s0 = SUM[i] + Sp[i];
 168                     D[i] = saturate_cast<T>(s0*_scale);
 169                     SUM[i] = s0 - Sm[i];
 170                 }
 171             }
 172             else
 173             {
 174                 for( i = 0; i <= width - 2; i += 2 )
 175                 {
 176                     ST s0 = SUM[i] + Sp[i], s1 = SUM[i+1] + Sp[i+1];
 177                     D[i] = saturate_cast<T>(s0);
 178                     D[i+1] = saturate_cast<T>(s1);
 179                     s0 -= Sm[i]; s1 -= Sm[i+1];
 180                     SUM[i] = s0; SUM[i+1] = s1;
 181                 }
 182
 183                 for( ; i < width; i++ )
 184                 {
 185                     ST s0 = SUM[i] + Sp[i];
 186                     D[i] = saturate_cast<T>(s0);
 187                     SUM[i] = s0 - Sm[i];
 188                 }
 189             }
 190             dst += dststep;
 191         }
 192     }
 193
 194     double scale;
 195     int sumCount;
 196     vector<ST> sum;
 197 };
 198
 199
 200 template<> struct ColumnSum<int, uchar> : public BaseColumnFilter
 201 {
 202     ColumnSum( int _ksize, int _anchor, double _scale )
 203     {
 204         ksize = _ksize;
 205         anchor = _anchor;
 206         scale = _scale;
 207         sumCount = 0;
 208     }
 209
 210     void reset() { sumCount = 0; }
 211
 212     void operator()(const uchar** src, uchar* dst, int dststep, int count, int width)
 213     {
 214         int i;
 215         int* SUM;
 216         bool haveScale = scale != 1;
 217         double _scale = scale;
 218
 219         #if CV_SSE2
 220             bool haveSSE2 = checkHardwareSupport(CV_CPU_SSE2);
 221         #endif
 222
 223         if( width != (int)sum.size() )
 224         {
 225             sum.resize(width);
 226             sumCount = 0;
 227         }
 228
 229         SUM = &sum[0];
 230         if( sumCount == 0 )
 231         {
 232             memset((void*)SUM, 0, width*sizeof(int));
 233             for( ; sumCount < ksize - 1; sumCount++, src++ )
 234             {
 235                 const int* Sp = (const int*)src[0];
 236                 i = 0;
 237                 #if CV_SSE2
 238                 if(haveSSE2)
 239                 {
 240                     for( ; i < width-4; i+=4 )
 241                     {
 242                         __m128i _sum = _mm_loadu_si128((const __m128i*)(SUM+i));
 243                         __m128i _sp = _mm_loadu_si128((const __m128i*)(Sp+i));
 244                         _mm_storeu_si128((__m128i*)(SUM+i),_mm_add_epi32(_sum, _sp));
 245                     }
 246                 }
 247                 #endif
 248                 for( ; i < width; i++ )
 249                     SUM[i] += Sp[i];
 250             }
 251         }
 252         else
 253         {
 254             CV_Assert( sumCount == ksize-1 );
 255             src += ksize-1;
 256         }
 257
 258         for( ; count--; src++ )
 259         {
 260             const int* Sp = (const int*)src[0];
 261             const int* Sm = (const int*)src[1-ksize];
 262             uchar* D = (uchar*)dst;
 263             if( haveScale )
 264             {
 265                 i = 0;
 266                 #if CV_SSE2
 267                 if(haveSSE2)
 268                 {
 269                     const __m128 scale4 = _mm_set1_ps((float)_scale);
 270                     for( ; i < width-8; i+=8 )
 271                     {
 272                         __m128i _sm  = _mm_loadu_si128((const __m128i*)(Sm+i));
 273                         __m128i _sm1  = _mm_loadu_si128((const __m128i*)(Sm+i+4));
 274
 275                         __m128i _s0  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i)),
 276                                                      _mm_loadu_si128((const __m128i*)(Sp+i)));
 277                         __m128i _s01  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i+4)),
 278                                                       _mm_loadu_si128((const __m128i*)(Sp+i+4)));
 279
 280                         __m128i _s0T = _mm_cvtps_epi32(_mm_mul_ps(scale4, _mm_cvtepi32_ps(_s0)));
 281                         __m128i _s0T1 = _mm_cvtps_epi32(_mm_mul_ps(scale4, _mm_cvtepi32_ps(_s01)));
 282
 283                         _s0T = _mm_packs_epi32(_s0T, _s0T1);
 284
 285                         _mm_storel_epi64((__m128i*)(D+i), _mm_packus_epi16(_s0T, _s0T));
 286
 287                         _mm_storeu_si128((__m128i*)(SUM+i), _mm_sub_epi32(_s0,_sm));
 288                         _mm_storeu_si128((__m128i*)(SUM+i+4),_mm_sub_epi32(_s01,_sm1));
 289                     }
 290                 }
 291                 #endif
 292                 for( ; i < width; i++ )
 293                 {
 294                     int s0 = SUM[i] + Sp[i];
 295                     D[i] = saturate_cast<uchar>(s0*_scale);
 296                     SUM[i] = s0 - Sm[i];
 297                 }
 298             }
 299             else
 300             {
 301                 i = 0;
 302                 #if CV_SSE2
 303                 if(haveSSE2)
 304                 {
 305                     for( ; i < width-8; i+=8 )
 306                     {
 307                         __m128i _sm  = _mm_loadu_si128((const __m128i*)(Sm+i));
 308                         __m128i _sm1  = _mm_loadu_si128((const __m128i*)(Sm+i+4));
 309
 310                         __m128i _s0  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i)),
 311                                                      _mm_loadu_si128((const __m128i*)(Sp+i)));
 312                         __m128i _s01  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i+4)),
 313                                                       _mm_loadu_si128((const __m128i*)(Sp+i+4)));
 314
 315                         __m128i _s0T = _mm_packs_epi32(_s0, _s01);
 316
 317                         _mm_storel_epi64((__m128i*)(D+i), _mm_packus_epi16(_s0T, _s0T));
 318
 319                         _mm_storeu_si128((__m128i*)(SUM+i), _mm_sub_epi32(_s0,_sm));
 320                         _mm_storeu_si128((__m128i*)(SUM+i+4),_mm_sub_epi32(_s01,_sm1));
 321                     }
 322                 }
 323                 #endif
 324
 325                 for( ; i < width; i++ )
 326                 {
 327                     int s0 = SUM[i] + Sp[i];
 328                     D[i] = saturate_cast<uchar>(s0);
 329                     SUM[i] = s0 - Sm[i];
 330                 }
 331             }
 332             dst += dststep;
 333         }
 334     }
 335
 336     double scale;
 337     int sumCount;
 338     vector<int> sum;
 339 };
 340
 341 template<> struct ColumnSum<int, short> : public BaseColumnFilter
 342 {
 343     ColumnSum( int _ksize, int _anchor, double _scale )
 344     {
 345         ksize = _ksize;
 346         anchor = _anchor;
 347         scale = _scale;
 348         sumCount = 0;
 349     }
 350
 351     void reset() { sumCount = 0; }
 352
 353     void operator()(const uchar** src, uchar* dst, int dststep, int count, int width)
 354     {
 355         int i;
 356         int* SUM;
 357         bool haveScale = scale != 1;
 358         double _scale = scale;
 359
 360         #if CV_SSE2
 361             bool haveSSE2 = checkHardwareSupport(CV_CPU_SSE2);
 362         #endif
 363
 364         if( width != (int)sum.size() )
 365         {
 366             sum.resize(width);
 367             sumCount = 0;
 368         }
 369         SUM = &sum[0];
 370         if( sumCount == 0 )
 371         {
 372             memset((void*)SUM, 0, width*sizeof(int));
 373             for( ; sumCount < ksize - 1; sumCount++, src++ )
 374             {
 375                 const int* Sp = (const int*)src[0];
 376                 i = 0;
 377                 #if CV_SSE2
 378                 if(haveSSE2)
 379                 {
 380                     for( ; i < width-4; i+=4 )
 381                     {
 382                         __m128i _sum = _mm_loadu_si128((const __m128i*)(SUM+i));
 383                         __m128i _sp = _mm_loadu_si128((const __m128i*)(Sp+i));
 384                         _mm_storeu_si128((__m128i*)(SUM+i),_mm_add_epi32(_sum, _sp));
 385                     }
 386                 }
 387                 #endif
 388                 for( ; i < width; i++ )
 389                     SUM[i] += Sp[i];
 390             }
 391         }
 392         else
 393         {
 394             CV_Assert( sumCount == ksize-1 );
 395             src += ksize-1;
 396         }
 397
 398         for( ; count--; src++ )
 399         {
 400             const int* Sp = (const int*)src[0];
 401             const int* Sm = (const int*)src[1-ksize];
 402             short* D = (short*)dst;
 403             if( haveScale )
 404             {
 405                 i = 0;
 406                 #if CV_SSE2
 407                 if(haveSSE2)
 408                 {
 409                     const __m128 scale4 = _mm_set1_ps((float)_scale);
 410                     for( ; i < width-8; i+=8 )
 411                     {
 412                         __m128i _sm   = _mm_loadu_si128((const __m128i*)(Sm+i));
 413                         __m128i _sm1  = _mm_loadu_si128((const __m128i*)(Sm+i+4));
 414
 415                         __m128i _s0  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i)),
 416                                                      _mm_loadu_si128((const __m128i*)(Sp+i)));
 417                         __m128i _s01  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i+4)),
 418                                                       _mm_loadu_si128((const __m128i*)(Sp+i+4)));
 419
 420                         __m128i _s0T  = _mm_cvtps_epi32(_mm_mul_ps(scale4, _mm_cvtepi32_ps(_s0)));
 421                         __m128i _s0T1 = _mm_cvtps_epi32(_mm_mul_ps(scale4, _mm_cvtepi32_ps(_s01)));
 422
 423                         _mm_storeu_si128((__m128i*)(D+i), _mm_packs_epi32(_s0T, _s0T1));
 424
 425                         _mm_storeu_si128((__m128i*)(SUM+i),_mm_sub_epi32(_s0,_sm));
 426                         _mm_storeu_si128((__m128i*)(SUM+i+4), _mm_sub_epi32(_s01,_sm1));
 427                     }
 428                 }
 429                 #endif
 430                 for( ; i < width; i++ )
 431                 {
 432                     int s0 = SUM[i] + Sp[i];
 433                     D[i] = saturate_cast<short>(s0*_scale);
 434                     SUM[i] = s0 - Sm[i];
 435                 }
 436             }
 437             else
 438             {
 439                 i = 0;
 440                 #if CV_SSE2
 441                 if(haveSSE2)
 442                 {
 443                     for( ; i < width-8; i+=8 )
 444                     {
 445
 446                         __m128i _sm  = _mm_loadu_si128((const __m128i*)(Sm+i));
 447                         __m128i _sm1  = _mm_loadu_si128((const __m128i*)(Sm+i+4));
 448
 449                         __m128i _s0  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i)),
 450                                                      _mm_loadu_si128((const __m128i*)(Sp+i)));
 451                         __m128i _s01  = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i+4)),
 452                                                       _mm_loadu_si128((const __m128i*)(Sp+i+4)));
 453
 454                         _mm_storeu_si128((__m128i*)(D+i), _mm_packs_epi32(_s0, _s01));
 455
 456                         _mm_storeu_si128((__m128i*)(SUM+i), _mm_sub_epi32(_s0,_sm));
 457                         _mm_storeu_si128((__m128i*)(SUM+i+4),_mm_sub_epi32(_s01,_sm1));
 458                     }
 459                 }
 460                 #endif
 461
 462                 for( ; i < width; i++ )
 463                 {
 464                     int s0 = SUM[i] + Sp[i];
 465                     D[i] = saturate_cast<short>(s0);
 466                     SUM[i] = s0 - Sm[i];
 467                 }
 468             }
 469             dst += dststep;
 470         }
 471     }
 472
 473     double scale;
 474     int sumCount;
 475     vector<int> sum;
 476 };
 477
 478
 479 template<> struct ColumnSum<int, ushort> : public BaseColumnFilter
 480 {
 481     ColumnSum( int _ksize, int _anchor, double _scale )
 482     {
 483         ksize = _ksize;
 484         anchor = _anchor;
 485         scale = _scale;
 486         sumCount = 0;
 487     }
 488
 489     void reset() { sumCount = 0; }
 490
 491     void operator()(const uchar** src, uchar* dst, int dststep, int count, int width)
 492     {
 493         int i;
 494         int* SUM;
 495         bool haveScale = scale != 1;
 496         double _scale = scale;
 497         #if CV_SSE2
 498                 bool haveSSE2 =  checkHardwareSupport(CV_CPU_SSE2);
 499         #endif
 500
 501         if( width != (int)sum.size() )
 502         {
 503             sum.resize(width);
 504             sumCount = 0;
 505         }
 506         SUM = &sum[0];
 507         if( sumCount == 0 )
 508         {
 509             memset((void*)SUM, 0, width*sizeof(int));
 510             for( ; sumCount < ksize - 1; sumCount++, src++ )
 511             {
 512                 const int* Sp = (const int*)src[0];
 513                 i = 0;
 514                 #if CV_SSE2
 515                 if(haveSSE2)
 516                 {
 517                     for( ; i < width-4; i+=4 )
 518                     {
 519                         __m128i _sum = _mm_loadu_si128((const __m128i*)(SUM+i));
 520                         __m128i _sp = _mm_loadu_si128((const __m128i*)(Sp+i));
 521                         _mm_storeu_si128((__m128i*)(SUM+i), _mm_add_epi32(_sum, _sp));
 522                     }
 523                 }
 524                 #endif
 525                 for( ; i < width; i++ )
 526                     SUM[i] += Sp[i];
 527             }
 528         }
 529         else
 530         {
 531             CV_Assert( sumCount == ksize-1 );
 532             src += ksize-1;
 533         }
 534
 535         for( ; count--; src++ )
 536         {
 537             const int* Sp = (const int*)src[0];
 538             const int* Sm = (const int*)src[1-ksize];
 539             ushort* D = (ushort*)dst;
 540             if( haveScale )
 541             {
 542                 i = 0;
 543                 #if CV_SSE2
 544                 if(haveSSE2)
 545                 {
 546                     const __m128 scale4 = _mm_set1_ps((float)_scale);
 547                     const __m128i delta0 = _mm_set1_epi32(0x8000);
 548                     const __m128i delta1 = _mm_set1_epi32(0x80008000);
 549
 550                     for( ; i < width-4; i+=4)
 551                     {
 552                         __m128i _sm   = _mm_loadu_si128((const __m128i*)(Sm+i));
 553                         __m128i _s0   = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i)),
 554                                                       _mm_loadu_si128((const __m128i*)(Sp+i)));
 555
 556                         __m128i _res = _mm_cvtps_epi32(_mm_mul_ps(scale4, _mm_cvtepi32_ps(_s0)));
 557
 558                         _res = _mm_sub_epi32(_res, delta0);
 559                         _res = _mm_add_epi16(_mm_packs_epi32(_res, _res), delta1);
 560
 561                         _mm_storel_epi64((__m128i*)(D+i), _res);
 562                         _mm_storeu_si128((__m128i*)(SUM+i), _mm_sub_epi32(_s0,_sm));
 563                     }
 564                 }
 565                 #endif
 566                 for( ; i < width; i++ )
 567                 {
 568                     int s0 = SUM[i] + Sp[i];
 569                     D[i] = saturate_cast<ushort>(s0*_scale);
 570                     SUM[i] = s0 - Sm[i];
 571                 }
 572             }
 573             else
 574             {
 575                 i = 0;
 576                 #if  CV_SSE2
 577                 if(haveSSE2)
 578                 {
 579                     const __m128i delta0 = _mm_set1_epi32(0x8000);
 580                     const __m128i delta1 = _mm_set1_epi32(0x80008000);
 581
 582                     for( ; i < width-4; i+=4 )
 583                     {
 584                         __m128i _sm   = _mm_loadu_si128((const __m128i*)(Sm+i));
 585                         __m128i _s0   = _mm_add_epi32(_mm_loadu_si128((const __m128i*)(SUM+i)),
 586                                                       _mm_loadu_si128((const __m128i*)(Sp+i)));
 587
 588                         __m128i _res = _mm_sub_epi32(_s0, delta0);
 589                         _res = _mm_add_epi16(_mm_packs_epi32(_res, _res), delta1);
 590
 591                         _mm_storel_epi64((__m128i*)(D+i), _res);
 592                         _mm_storeu_si128((__m128i*)(SUM+i), _mm_sub_epi32(_s0,_sm));
 593                     }
 594                 }
 595                 #endif
 596
 597                 for( ; i < width; i++ )
 598                 {
 599                     int s0 = SUM[i] + Sp[i];
 600                     D[i] = saturate_cast<ushort>(s0);
 601                     SUM[i] = s0 - Sm[i];
 602                 }
 603             }
 604             dst += dststep;
 605         }
 606     }
 607
 608     double scale;
 609     int sumCount;
 610     vector<int> sum;
 611 };
 612
 613
 614 }
 615
 616 cv::Ptr<cv::BaseRowFilter> cv::getRowSumFilter(int srcType, int sumType, int ksize, int anchor)
 617 {
 618     int sdepth = CV_MAT_DEPTH(srcType), ddepth = CV_MAT_DEPTH(sumType);
 619     CV_Assert( CV_MAT_CN(sumType) == CV_MAT_CN(srcType) );
 620
 621     if( anchor < 0 )
 622         anchor = ksize/2;
 623
 624     if( sdepth == CV_8U && ddepth == CV_32S )
 625         return Ptr<BaseRowFilter>(new RowSum<uchar, int>(ksize, anchor));
 626     if( sdepth == CV_8U && ddepth == CV_64F )
 627         return Ptr<BaseRowFilter>(new RowSum<uchar, double>(ksize, anchor));
 628     if( sdepth == CV_16U && ddepth == CV_32S )
 629         return Ptr<BaseRowFilter>(new RowSum<ushort, int>(ksize, anchor));
 630     if( sdepth == CV_16U && ddepth == CV_64F )
 631         return Ptr<BaseRowFilter>(new RowSum<ushort, double>(ksize, anchor));
 632     if( sdepth == CV_16S && ddepth == CV_32S )
 633         return Ptr<BaseRowFilter>(new RowSum<short, int>(ksize, anchor));
 634     if( sdepth == CV_32S && ddepth == CV_32S )
 635         return Ptr<BaseRowFilter>(new RowSum<int, int>(ksize, anchor));
 636     if( sdepth == CV_16S && ddepth == CV_64F )
 637         return Ptr<BaseRowFilter>(new RowSum<short, double>(ksize, anchor));
 638     if( sdepth == CV_32F && ddepth == CV_64F )
 639         return Ptr<BaseRowFilter>(new RowSum<float, double>(ksize, anchor));
 640     if( sdepth == CV_64F && ddepth == CV_64F )
 641         return Ptr<BaseRowFilter>(new RowSum<double, double>(ksize, anchor));
 642
 643     CV_Error_( CV_StsNotImplemented,
 644         ("Unsupported combination of source format (=%d), and buffer format (=%d)",
 645         srcType, sumType));
 646
 647     return Ptr<BaseRowFilter>(0);
 648 }
 649
 650
 651 cv::Ptr<cv::BaseColumnFilter> cv::getColumnSumFilter(int sumType, int dstType, int ksize,
 652                                                      int anchor, double scale)
 653 {
 654     int sdepth = CV_MAT_DEPTH(sumType), ddepth = CV_MAT_DEPTH(dstType);
 655     CV_Assert( CV_MAT_CN(sumType) == CV_MAT_CN(dstType) );
 656
 657     if( anchor < 0 )
 658         anchor = ksize/2;
 659
 660     if( ddepth == CV_8U && sdepth == CV_32S )
 661         return Ptr<BaseColumnFilter>(new ColumnSum<int, uchar>(ksize, anchor, scale));
 662     if( ddepth == CV_8U && sdepth == CV_64F )
 663         return Ptr<BaseColumnFilter>(new ColumnSum<double, uchar>(ksize, anchor, scale));
 664     if( ddepth == CV_16U && sdepth == CV_32S )
 665         return Ptr<BaseColumnFilter>(new ColumnSum<int, ushort>(ksize, anchor, scale));
 666     if( ddepth == CV_16U && sdepth == CV_64F )
 667         return Ptr<BaseColumnFilter>(new ColumnSum<double, ushort>(ksize, anchor, scale));
 668     if( ddepth == CV_16S && sdepth == CV_32S )
 669         return Ptr<BaseColumnFilter>(new ColumnSum<int, short>(ksize, anchor, scale));
 670     if( ddepth == CV_16S && sdepth == CV_64F )
 671         return Ptr<BaseColumnFilter>(new ColumnSum<double, short>(ksize, anchor, scale));
 672     if( ddepth == CV_32S && sdepth == CV_32S )
 673         return Ptr<BaseColumnFilter>(new ColumnSum<int, int>(ksize, anchor, scale));
 674     if( ddepth == CV_32F && sdepth == CV_32S )
 675         return Ptr<BaseColumnFilter>(new ColumnSum<int, float>(ksize, anchor, scale));
 676     if( ddepth == CV_32F && sdepth == CV_64F )
 677         return Ptr<BaseColumnFilter>(new ColumnSum<double, float>(ksize, anchor, scale));
 678     if( ddepth == CV_64F && sdepth == CV_32S )
 679         return Ptr<BaseColumnFilter>(new ColumnSum<int, double>(ksize, anchor, scale));
 680     if( ddepth == CV_64F && sdepth == CV_64F )
 681         return Ptr<BaseColumnFilter>(new ColumnSum<double, double>(ksize, anchor, scale));
 682
 683     CV_Error_( CV_StsNotImplemented,
 684         ("Unsupported combination of sum format (=%d), and destination format (=%d)",
 685         sumType, dstType));
 686
 687     return Ptr<BaseColumnFilter>(0);
 688 }
 689
 690
 691 cv::Ptr<cv::FilterEngine> cv::createBoxFilter( int srcType, int dstType, Size ksize,
 692                     Point anchor, bool normalize, int borderType )
 693 {
 694     int sdepth = CV_MAT_DEPTH(srcType);
 695     int cn = CV_MAT_CN(srcType), sumType = CV_64F;
 696     if( sdepth <= CV_32S && (!normalize ||
 697         ksize.width*ksize.height <= (sdepth == CV_8U ? (1<<23) :
 698             sdepth == CV_16U ? (1 << 15) : (1 << 16))) )
 699         sumType = CV_32S;
 700     sumType = CV_MAKETYPE( sumType, cn );
 701
 702     Ptr<BaseRowFilter> rowFilter = getRowSumFilter(srcType, sumType, ksize.width, anchor.x );
 703     Ptr<BaseColumnFilter> columnFilter = getColumnSumFilter(sumType,
 704         dstType, ksize.height, anchor.y, normalize ? 1./(ksize.width*ksize.height) : 1);
 705
 706     return Ptr<FilterEngine>(new FilterEngine(Ptr<BaseFilter>(0), rowFilter, columnFilter,
 707            srcType, dstType, sumType, borderType ));
 708 }
 709
 710
 711 void cv::boxFilter( InputArray _src, OutputArray _dst, int ddepth,
 712                 Size ksize, Point anchor,
 713                 bool normalize, int borderType )
 714 {
 715     Mat src = _src.getMat();
 716     int sdepth = src.depth(), cn = src.channels();
 717     if( ddepth < 0 )
 718         ddepth = sdepth;
 719     _dst.create( src.size(), CV_MAKETYPE(ddepth, cn) );
 720     Mat dst = _dst.getMat();
 721     if( borderType != BORDER_CONSTANT && normalize && (borderType & BORDER_ISOLATED) != 0 )
 722     {
 723         if( src.rows == 1 )
 724             ksize.height = 1;
 725         if( src.cols == 1 )
 726             ksize.width = 1;
 727     }
 728 #ifdef HAVE_TEGRA_OPTIMIZATION
 729     if ( tegra::box(src, dst, ksize, anchor, normalize, borderType) )
 730         return;
 731 #endif
 732
 733     Ptr<FilterEngine> f = createBoxFilter( src.type(), dst.type(),
 734                         ksize, anchor, normalize, borderType );
 735     f->apply( src, dst );
 736 }
 737
 738 void cv::blur( InputArray src, OutputArray dst,
 739            Size ksize, Point anchor, int borderType )
 740 {
 741     boxFilter( src, dst, -1, ksize, anchor, true, borderType );
 742 }
 743
 744 /****************************************************************************************\
 745                                      Gaussian Blur
 746 \****************************************************************************************/
 747
 748 cv::Mat cv::getGaussianKernel( int n, double sigma, int ktype )
 749 {
 750     const int SMALL_GAUSSIAN_SIZE = 7;
 751     static const float small_gaussian_tab[][SMALL_GAUSSIAN_SIZE] =
 752     {
 753         {1.f},
 754         {0.25f, 0.5f, 0.25f},
 755         {0.0625f, 0.25f, 0.375f, 0.25f, 0.0625f},
 756         {0.03125f, 0.109375f, 0.21875f, 0.28125f, 0.21875f, 0.109375f, 0.03125f}
 757     };
 758
 759     const float* fixed_kernel = n % 2 == 1 && n <= SMALL_GAUSSIAN_SIZE && sigma <= 0 ?
 760         small_gaussian_tab[n>>1] : 0;
 761
 762     CV_Assert( ktype == CV_32F || ktype == CV_64F );
 763     Mat kernel(n, 1, ktype);
 764     float* cf = (float*)kernel.data;
 765     double* cd = (double*)kernel.data;
 766
 767     double sigmaX = sigma > 0 ? sigma : ((n-1)*0.5 - 1)*0.3 + 0.8;
 768     double scale2X = -0.5/(sigmaX*sigmaX);
 769     double sum = 0;
 770
 771     int i;
 772     for( i = 0; i < n; i++ )
 773     {
 774         double x = i - (n-1)*0.5;
 775         double t = fixed_kernel ? (double)fixed_kernel[i] : std::exp(scale2X*x*x);
 776         if( ktype == CV_32F )
 777         {
 778             cf[i] = (float)t;
 779             sum += cf[i];
 780         }
 781         else
 782         {
 783             cd[i] = t;
 784             sum += cd[i];
 785         }
 786     }
 787
 788     sum = 1./sum;
 789     for( i = 0; i < n; i++ )
 790     {
 791         if( ktype == CV_32F )
 792             cf[i] = (float)(cf[i]*sum);
 793         else
 794             cd[i] *= sum;
 795     }
 796
 797     return kernel;
 798 }
 799
 800
 801 cv::Ptr<cv::FilterEngine> cv::createGaussianFilter( int type, Size ksize,
 802                                         double sigma1, double sigma2,
 803                                         int borderType )
 804 {
 805     int depth = CV_MAT_DEPTH(type);
 806     if( sigma2 <= 0 )
 807         sigma2 = sigma1;
 808
 809     // automatic detection of kernel size from sigma
 810     if( ksize.width <= 0 && sigma1 > 0 )
 811         ksize.width = cvRound(sigma1*(depth == CV_8U ? 3 : 4)*2 + 1)|1;
 812     if( ksize.height <= 0 && sigma2 > 0 )
 813         ksize.height = cvRound(sigma2*(depth == CV_8U ? 3 : 4)*2 + 1)|1;
 814
 815     CV_Assert( ksize.width > 0 && ksize.width % 2 == 1 &&
 816         ksize.height > 0 && ksize.height % 2 == 1 );
 817
 818     sigma1 = std::max( sigma1, 0. );
 819     sigma2 = std::max( sigma2, 0. );
 820
 821     Mat kx = getGaussianKernel( ksize.width, sigma1, std::max(depth, CV_32F) );
 822     Mat ky;
 823     if( ksize.height == ksize.width && std::abs(sigma1 - sigma2) < DBL_EPSILON )
 824         ky = kx;
 825     else
 826         ky = getGaussianKernel( ksize.height, sigma2, std::max(depth, CV_32F) );
 827
 828     return createSeparableLinearFilter( type, type, kx, ky, Point(-1,-1), 0, borderType );
 829 }
 830
 831
 832 void cv::GaussianBlur( InputArray _src, OutputArray _dst, Size ksize,
 833                    double sigma1, double sigma2,
 834                    int borderType )
 835 {
 836     Mat src = _src.getMat();
 837     _dst.create( src.size(), src.type() );
 838     Mat dst = _dst.getMat();
 839
 840     if( borderType != BORDER_CONSTANT )
 841     {
 842         if( src.rows == 1 )
 843             ksize.height = 1;
 844         if( src.cols == 1 )
 845             ksize.width = 1;
 846     }
 847
 848     if( ksize.width == 1 && ksize.height == 1 )
 849     {
 850         src.copyTo(dst);
 851         return;
 852     }
 853
 854 #ifdef HAVE_TEGRA_OPTIMIZATION
 855     if(sigma1 == 0 && sigma2 == 0 && tegra::gaussian(src, dst, ksize, borderType))
 856         return;
 857 #endif
 858
 859 #if defined HAVE_IPP && (IPP_VERSION_MAJOR >= 7)
 860     if(src.type() == CV_32FC1 && sigma1 == sigma2 && ksize.width == ksize.height && sigma1 != 0.0 )
 861     {
 862         IppiSize roi = {src.cols, src.rows};
 863         int bufSize = 0;
 864         ippiFilterGaussGetBufferSize_32f_C1R(roi, ksize.width, &bufSize);
 865         AutoBuffer<uchar> buf(bufSize+128);
 866         if( ippiFilterGaussBorder_32f_C1R((const Ipp32f *)src.data, (int)src.step,
 867                                           (Ipp32f *)dst.data, (int)dst.step,
 868                                           roi, ksize.width, (Ipp32f)sigma1,
 869                                           (IppiBorderType)borderType, 0.0,
 870                                           alignPtr(&buf[0],32)) >= 0 )
 871             return;
 872     }
 873 #endif
 874
 875     Ptr<FilterEngine> f = createGaussianFilter( src.type(), ksize, sigma1, sigma2, borderType );
 876     f->apply( src, dst );
 877 }
 878
 879
 880 /****************************************************************************************\
 881                                       Median Filter
 882 \****************************************************************************************/
 883
 884 namespace cv
 885 {
 886 typedef ushort HT;
 887
 888 /**
 889  * This structure represents a two-tier histogram. The first tier (known as the
 890  * "coarse" level) is 4 bit wide and the second tier (known as the "fine" level)
 891  * is 8 bit wide. Pixels inserted in the fine level also get inserted into the
 892  * coarse bucket designated by the 4 MSBs of the fine bucket value.
 893  *
 894  * The structure is aligned on 16 bits, which is a prerequisite for SIMD
 895  * instructions. Each bucket is 16 bit wide, which means that extra care must be
 896  * taken to prevent overflow.
 897  */
 898 typedef struct
 899 {
 900     HT coarse[16];
 901     HT fine[16][16];
 902 } Histogram;
 903
 904
 905 #if CV_SSE2
 906 #define MEDIAN_HAVE_SIMD 1
 907
 908 static inline void histogram_add_simd( const HT x[16], HT y[16] )
 909 {
 910     const __m128i* rx = (const __m128i*)x;
 911     __m128i* ry = (__m128i*)y;
 912     __m128i r0 = _mm_add_epi16(_mm_load_si128(ry+0),_mm_load_si128(rx+0));
 913     __m128i r1 = _mm_add_epi16(_mm_load_si128(ry+1),_mm_load_si128(rx+1));
 914     _mm_store_si128(ry+0, r0);
 915     _mm_store_si128(ry+1, r1);
 916 }
 917
 918 static inline void histogram_sub_simd( const HT x[16], HT y[16] )
 919 {
 920     const __m128i* rx = (const __m128i*)x;
 921     __m128i* ry = (__m128i*)y;
 922     __m128i r0 = _mm_sub_epi16(_mm_load_si128(ry+0),_mm_load_si128(rx+0));
 923     __m128i r1 = _mm_sub_epi16(_mm_load_si128(ry+1),_mm_load_si128(rx+1));
 924     _mm_store_si128(ry+0, r0);
 925     _mm_store_si128(ry+1, r1);
 926 }
 927
 928 #else
 929 #define MEDIAN_HAVE_SIMD 0
 930 #endif
 931
 932
 933 static inline void histogram_add( const HT x[16], HT y[16] )
 934 {
 935     int i;
 936     for( i = 0; i < 16; ++i )
 937         y[i] = (HT)(y[i] + x[i]);
 938 }
 939
 940 static inline void histogram_sub( const HT x[16], HT y[16] )
 941 {
 942     int i;
 943     for( i = 0; i < 16; ++i )
 944         y[i] = (HT)(y[i] - x[i]);
 945 }
 946
 947 static inline void histogram_muladd( int a, const HT x[16],
 948         HT y[16] )
 949 {
 950     for( int i = 0; i < 16; ++i )
 951         y[i] = (HT)(y[i] + a * x[i]);
 952 }
 953
 954 static void
 955 medianBlur_8u_O1( const Mat& _src, Mat& _dst, int ksize )
 956 {
 957 /**
 958  * HOP is short for Histogram OPeration. This macro makes an operation \a op on
 959  * histogram \a h for pixel value \a x. It takes care of handling both levels.
 960  */
 961 #define HOP(h,x,op) \
 962     h.coarse[x>>4] op, \
 963     *((HT*)h.fine + x) op
 964
 965 #define COP(c,j,x,op) \
 966     h_coarse[ 16*(n*c+j) + (x>>4) ] op, \
 967     h_fine[ 16 * (n*(16*c+(x>>4)) + j) + (x & 0xF) ] op
 968
 969     int cn = _dst.channels(), m = _dst.rows, r = (ksize-1)/2;
 970     size_t sstep = _src.step, dstep = _dst.step;
 971     Histogram CV_DECL_ALIGNED(16) H[4];
 972     HT CV_DECL_ALIGNED(16) luc[4][16];
 973
 974     int STRIPE_SIZE = std::min( _dst.cols, 512/cn );
 975
 976     vector<HT> _h_coarse(1 * 16 * (STRIPE_SIZE + 2*r) * cn + 16);
 977     vector<HT> _h_fine(16 * 16 * (STRIPE_SIZE + 2*r) * cn + 16);
 978     HT* h_coarse = alignPtr(&_h_coarse[0], 16);
 979     HT* h_fine = alignPtr(&_h_fine[0], 16);
 980 #if MEDIAN_HAVE_SIMD
 981     volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
 982 #endif
 983
 984     for( int x = 0; x < _dst.cols; x += STRIPE_SIZE )
 985     {
 986         int i, j, k, c, n = std::min(_dst.cols - x, STRIPE_SIZE) + r*2;
 987         const uchar* src = _src.data + x*cn;
 988         uchar* dst = _dst.data + (x - r)*cn;
 989
 990         memset( h_coarse, 0, 16*n*cn*sizeof(h_coarse[0]) );
 991         memset( h_fine, 0, 16*16*n*cn*sizeof(h_fine[0]) );
 992
 993         // First row initialization
 994         for( c = 0; c < cn; c++ )
 995         {
 996             for( j = 0; j < n; j++ )
 997                 COP( c, j, src[cn*j+c], += (cv::HT)(r+2) );
 998
 999             for( i = 1; i < r; i++ )
1000             {
1001                 const uchar* p = src + sstep*std::min(i, m-1);
1002                 for ( j = 0; j < n; j++ )
1003                     COP( c, j, p[cn*j+c], ++ );
1004             }
1005         }
1006
1007         for( i = 0; i < m; i++ )
1008         {
1009             const uchar* p0 = src + sstep * std::max( 0, i-r-1 );
1010             const uchar* p1 = src + sstep * std::min( m-1, i+r );
1011
1012             memset( H, 0, cn*sizeof(H[0]) );
1013             memset( luc, 0, cn*sizeof(luc[0]) );
1014             for( c = 0; c < cn; c++ )
1015             {
1016                 // Update column histograms for the entire row.
1017                 for( j = 0; j < n; j++ )
1018                 {
1019                     COP( c, j, p0[j*cn + c], -- );
1020                     COP( c, j, p1[j*cn + c], ++ );
1021                 }
1022
1023                 // First column initialization
1024                 for( k = 0; k < 16; ++k )
1025                     histogram_muladd( 2*r+1, &h_fine[16*n*(16*c+k)], &H[c].fine[k][0] );
1026
1027             #if MEDIAN_HAVE_SIMD
1028                 if( useSIMD )
1029                 {
1030                     for( j = 0; j < 2*r; ++j )
1031                         histogram_add_simd( &h_coarse[16*(n*c+j)], H[c].coarse );
1032
1033                     for( j = r; j < n-r; j++ )
1034                     {
1035                         int t = 2*r*r + 2*r, b, sum = 0;
1036                         HT* segment;
1037
1038                         histogram_add_simd( &h_coarse[16*(n*c + std::min(j+r,n-1))], H[c].coarse );
1039
1040                         // Find median at coarse level
1041                         for ( k = 0; k < 16 ; ++k )
1042                         {
1043                             sum += H[c].coarse[k];
1044                             if ( sum > t )
1045                             {
1046                                 sum -= H[c].coarse[k];
1047                                 break;
1048                             }
1049                         }
1050                         assert( k < 16 );
1051
1052                         /* Update corresponding histogram segment */
1053                         if ( luc[c][k] <= j-r )
1054                         {
1055                             memset( &H[c].fine[k], 0, 16 * sizeof(HT) );
1056                             for ( luc[c][k] = cv::HT(j-r); luc[c][k] < MIN(j+r+1,n); ++luc[c][k] )
1057                                 histogram_add_simd( &h_fine[16*(n*(16*c+k)+luc[c][k])], H[c].fine[k] );
1058
1059                             if ( luc[c][k] < j+r+1 )
1060                             {
1061                                 histogram_muladd( j+r+1 - n, &h_fine[16*(n*(16*c+k)+(n-1))], &H[c].fine[k][0] );
1062                                 luc[c][k] = (HT)(j+r+1);
1063                             }
1064                         }
1065                         else
1066                         {
1067                             for ( ; luc[c][k] < j+r+1; ++luc[c][k] )
1068                             {
1069                                 histogram_sub_simd( &h_fine[16*(n*(16*c+k)+MAX(luc[c][k]-2*r-1,0))], H[c].fine[k] );
1070                                 histogram_add_simd( &h_fine[16*(n*(16*c+k)+MIN(luc[c][k],n-1))], H[c].fine[k] );
1071                             }
1072                         }
1073
1074                         histogram_sub_simd( &h_coarse[16*(n*c+MAX(j-r,0))], H[c].coarse );
1075
1076                         /* Find median in segment */
1077                         segment = H[c].fine[k];
1078                         for ( b = 0; b < 16 ; b++ )
1079                         {
1080                             sum += segment[b];
1081                             if ( sum > t )
1082                             {
1083                                 dst[dstep*i+cn*j+c] = (uchar)(16*k + b);
1084                                 break;
1085                             }
1086                         }
1087                         assert( b < 16 );
1088                     }
1089                 }
1090                 else
1091             #endif
1092                 {
1093                     for( j = 0; j < 2*r; ++j )
1094                         histogram_add( &h_coarse[16*(n*c+j)], H[c].coarse );
1095
1096                     for( j = r; j < n-r; j++ )
1097                     {
1098                         int t = 2*r*r + 2*r, b, sum = 0;
1099                         HT* segment;
1100
1101                         histogram_add( &h_coarse[16*(n*c + std::min(j+r,n-1))], H[c].coarse );
1102
1103                         // Find median at coarse level
1104                         for ( k = 0; k < 16 ; ++k )
1105                         {
1106                             sum += H[c].coarse[k];
1107                             if ( sum > t )
1108                             {
1109                                 sum -= H[c].coarse[k];
1110                                 break;
1111                             }
1112                         }
1113                         assert( k < 16 );
1114
1115                         /* Update corresponding histogram segment */
1116                         if ( luc[c][k] <= j-r )
1117                         {
1118                             memset( &H[c].fine[k], 0, 16 * sizeof(HT) );
1119                             for ( luc[c][k] = cv::HT(j-r); luc[c][k] < MIN(j+r+1,n); ++luc[c][k] )
1120                                 histogram_add( &h_fine[16*(n*(16*c+k)+luc[c][k])], H[c].fine[k] );
1121
1122                             if ( luc[c][k] < j+r+1 )
1123                             {
1124                                 histogram_muladd( j+r+1 - n, &h_fine[16*(n*(16*c+k)+(n-1))], &H[c].fine[k][0] );
1125                                 luc[c][k] = (HT)(j+r+1);
1126                             }
1127                         }
1128                         else
1129                         {
1130                             for ( ; luc[c][k] < j+r+1; ++luc[c][k] )
1131                             {
1132                                 histogram_sub( &h_fine[16*(n*(16*c+k)+MAX(luc[c][k]-2*r-1,0))], H[c].fine[k] );
1133                                 histogram_add( &h_fine[16*(n*(16*c+k)+MIN(luc[c][k],n-1))], H[c].fine[k] );
1134                             }
1135                         }
1136
1137                         histogram_sub( &h_coarse[16*(n*c+MAX(j-r,0))], H[c].coarse );
1138
1139                         /* Find median in segment */
1140                         segment = H[c].fine[k];
1141                         for ( b = 0; b < 16 ; b++ )
1142                         {
1143                             sum += segment[b];
1144                             if ( sum > t )
1145                             {
1146                                 dst[dstep*i+cn*j+c] = (uchar)(16*k + b);
1147                                 break;
1148                             }
1149                         }
1150                         assert( b < 16 );
1151                     }
1152                 }
1153             }
1154         }
1155     }
1156
1157 #undef HOP
1158 #undef COP
1159 }
1160
1161 static void
1162 medianBlur_8u_Om( const Mat& _src, Mat& _dst, int m )
1163 {
1164     #define N  16
1165     int     zone0[4][N];
1166     int     zone1[4][N*N];
1167     int     x, y;
1168     int     n2 = m*m/2;
1169     Size    size = _dst.size();
1170     const uchar* src = _src.data;
1171     uchar*  dst = _dst.data;
1172     int     src_step = (int)_src.step, dst_step = (int)_dst.step;
1173     int     cn = _src.channels();
1174     const uchar*  src_max = src + size.height*src_step;
1175
1176     #define UPDATE_ACC01( pix, cn, op ) \
1177     {                                   \
1178         int p = (pix);                  \
1179         zone1[cn][p] op;                \
1180         zone0[cn][p >> 4] op;           \
1181     }
1182
1183     //CV_Assert( size.height >= nx && size.width >= nx );
1184     for( x = 0; x < size.width; x++, src += cn, dst += cn )
1185     {
1186         uchar* dst_cur = dst;
1187         const uchar* src_top = src;
1188         const uchar* src_bottom = src;
1189         int k, c;
1190         int src_step1 = src_step, dst_step1 = dst_step;
1191
1192         if( x % 2 != 0 )
1193         {
1194             src_bottom = src_top += src_step*(size.height-1);
1195             dst_cur += dst_step*(size.height-1);
1196             src_step1 = -src_step1;
1197             dst_step1 = -dst_step1;
1198         }
1199
1200         // init accumulator
1201         memset( zone0, 0, sizeof(zone0[0])*cn );
1202         memset( zone1, 0, sizeof(zone1[0])*cn );
1203
1204         for( y = 0; y <= m/2; y++ )
1205         {
1206             for( c = 0; c < cn; c++ )
1207             {
1208                 if( y > 0 )
1209                 {
1210                     for( k = 0; k < m*cn; k += cn )
1211                         UPDATE_ACC01( src_bottom[k+c], c, ++ );
1212                 }
1213                 else
1214                 {
1215                     for( k = 0; k < m*cn; k += cn )
1216                         UPDATE_ACC01( src_bottom[k+c], c, += m/2+1 );
1217                 }
1218             }
1219
1220             if( (src_step1 > 0 && y < size.height-1) ||
1221                 (src_step1 < 0 && size.height-y-1 > 0) )
1222                 src_bottom += src_step1;
1223         }
1224
1225         for( y = 0; y < size.height; y++, dst_cur += dst_step1 )
1226         {
1227             // find median
1228             for( c = 0; c < cn; c++ )
1229             {
1230                 int s = 0;
1231                 for( k = 0; ; k++ )
1232                 {
1233                     int t = s + zone0[c][k];
1234                     if( t > n2 ) break;
1235                     s = t;
1236                 }
1237
1238                 for( k *= N; ;k++ )
1239                 {
1240                     s += zone1[c][k];
1241                     if( s > n2 ) break;
1242                 }
1243
1244                 dst_cur[c] = (uchar)k;
1245             }
1246
1247             if( y+1 == size.height )
1248                 break;
1249
1250             if( cn == 1 )
1251             {
1252                 for( k = 0; k < m; k++ )
1253                 {
1254                     int p = src_top[k];
1255                     int q = src_bottom[k];
1256                     zone1[0][p]--;
1257                     zone0[0][p>>4]--;
1258                     zone1[0][q]++;
1259                     zone0[0][q>>4]++;
1260                 }
1261             }
1262             else if( cn == 3 )
1263             {
1264                 for( k = 0; k < m*3; k += 3 )
1265                 {
1266                     UPDATE_ACC01( src_top[k], 0, -- );
1267                     UPDATE_ACC01( src_top[k+1], 1, -- );
1268                     UPDATE_ACC01( src_top[k+2], 2, -- );
1269
1270                     UPDATE_ACC01( src_bottom[k], 0, ++ );
1271                     UPDATE_ACC01( src_bottom[k+1], 1, ++ );
1272                     UPDATE_ACC01( src_bottom[k+2], 2, ++ );
1273                 }
1274             }
1275             else
1276             {
1277                 assert( cn == 4 );
1278                 for( k = 0; k < m*4; k += 4 )
1279                 {
1280                     UPDATE_ACC01( src_top[k], 0, -- );
1281                     UPDATE_ACC01( src_top[k+1], 1, -- );
1282                     UPDATE_ACC01( src_top[k+2], 2, -- );
1283                     UPDATE_ACC01( src_top[k+3], 3, -- );
1284
1285                     UPDATE_ACC01( src_bottom[k], 0, ++ );
1286                     UPDATE_ACC01( src_bottom[k+1], 1, ++ );
1287                     UPDATE_ACC01( src_bottom[k+2], 2, ++ );
1288                     UPDATE_ACC01( src_bottom[k+3], 3, ++ );
1289                 }
1290             }
1291
1292             if( (src_step1 > 0 && src_bottom + src_step1 < src_max) ||
1293                 (src_step1 < 0 && src_bottom + src_step1 >= src) )
1294                 src_bottom += src_step1;
1295
1296             if( y >= m/2 )
1297                 src_top += src_step1;
1298         }
1299     }
1300 #undef N
1301 #undef UPDATE_ACC
1302 }
1303
1304
1305 struct MinMax8u
1306 {
1307     typedef uchar value_type;
1308     typedef int arg_type;
1309     enum { SIZE = 1 };
1310     arg_type load(const uchar* ptr) { return *ptr; }
1311     void store(uchar* ptr, arg_type val) { *ptr = (uchar)val; }
1312     void operator()(arg_type& a, arg_type& b) const
1313     {
1314         int t = CV_FAST_CAST_8U(a - b);
1315         b += t; a -= t;
1316     }
1317 };
1318
1319 struct MinMax16u
1320 {
1321     typedef ushort value_type;
1322     typedef int arg_type;
1323     enum { SIZE = 1 };
1324     arg_type load(const ushort* ptr) { return *ptr; }
1325     void store(ushort* ptr, arg_type val) { *ptr = (ushort)val; }
1326     void operator()(arg_type& a, arg_type& b) const
1327     {
1328         arg_type t = a;
1329         a = std::min(a, b);
1330         b = std::max(b, t);
1331     }
1332 };
1333
1334 struct MinMax16s
1335 {
1336     typedef short value_type;
1337     typedef int arg_type;
1338     enum { SIZE = 1 };
1339     arg_type load(const short* ptr) { return *ptr; }
1340     void store(short* ptr, arg_type val) { *ptr = (short)val; }
1341     void operator()(arg_type& a, arg_type& b) const
1342     {
1343         arg_type t = a;
1344         a = std::min(a, b);
1345         b = std::max(b, t);
1346     }
1347 };
1348
1349 struct MinMax32f
1350 {
1351     typedef float value_type;
1352     typedef float arg_type;
1353     enum { SIZE = 1 };
1354     arg_type load(const float* ptr) { return *ptr; }
1355     void store(float* ptr, arg_type val) { *ptr = val; }
1356     void operator()(arg_type& a, arg_type& b) const
1357     {
1358         arg_type t = a;
1359         a = std::min(a, b);
1360         b = std::max(b, t);
1361     }
1362 };
1363
1364 #if CV_SSE2
1365
1366 struct MinMaxVec8u
1367 {
1368     typedef uchar value_type;
1369     typedef __m128i arg_type;
1370     enum { SIZE = 16 };
1371     arg_type load(const uchar* ptr) { return _mm_loadu_si128((const __m128i*)ptr); }
1372     void store(uchar* ptr, arg_type val) { _mm_storeu_si128((__m128i*)ptr, val); }
1373     void operator()(arg_type& a, arg_type& b) const
1374     {
1375         arg_type t = a;
1376         a = _mm_min_epu8(a, b);
1377         b = _mm_max_epu8(b, t);
1378     }
1379 };
1380
1381
1382 struct MinMaxVec16u
1383 {
1384     typedef ushort value_type;
1385     typedef __m128i arg_type;
1386     enum { SIZE = 8 };
1387     arg_type load(const ushort* ptr) { return _mm_loadu_si128((const __m128i*)ptr); }
1388     void store(ushort* ptr, arg_type val) { _mm_storeu_si128((__m128i*)ptr, val); }
1389     void operator()(arg_type& a, arg_type& b) const
1390     {
1391         arg_type t = _mm_subs_epu16(a, b);
1392         a = _mm_subs_epu16(a, t);
1393         b = _mm_adds_epu16(b, t);
1394     }
1395 };
1396
1397
1398 struct MinMaxVec16s
1399 {
1400     typedef short value_type;
1401     typedef __m128i arg_type;
1402     enum { SIZE = 8 };
1403     arg_type load(const short* ptr) { return _mm_loadu_si128((const __m128i*)ptr); }
1404     void store(short* ptr, arg_type val) { _mm_storeu_si128((__m128i*)ptr, val); }
1405     void operator()(arg_type& a, arg_type& b) const
1406     {
1407         arg_type t = a;
1408         a = _mm_min_epi16(a, b);
1409         b = _mm_max_epi16(b, t);
1410     }
1411 };
1412
1413
1414 struct MinMaxVec32f
1415 {
1416     typedef float value_type;
1417     typedef __m128 arg_type;
1418     enum { SIZE = 4 };
1419     arg_type load(const float* ptr) { return _mm_loadu_ps(ptr); }
1420     void store(float* ptr, arg_type val) { _mm_storeu_ps(ptr, val); }
1421     void operator()(arg_type& a, arg_type& b) const
1422     {
1423         arg_type t = a;
1424         a = _mm_min_ps(a, b);
1425         b = _mm_max_ps(b, t);
1426     }
1427 };
1428
1429
1430 #else
1431
1432 typedef MinMax8u MinMaxVec8u;
1433 typedef MinMax16u MinMaxVec16u;
1434 typedef MinMax16s MinMaxVec16s;
1435 typedef MinMax32f MinMaxVec32f;
1436
1437 #endif
1438
1439 template<class Op, class VecOp>
1440 static void
1441 medianBlur_SortNet( const Mat& _src, Mat& _dst, int m )
1442 {
1443     typedef typename Op::value_type T;
1444     typedef typename Op::arg_type WT;
1445     typedef typename VecOp::arg_type VT;
1446
1447     const T* src = (const T*)_src.data;
1448     T* dst = (T*)_dst.data;
1449     int sstep = (int)(_src.step/sizeof(T));
1450     int dstep = (int)(_dst.step/sizeof(T));
1451     Size size = _dst.size();
1452     int i, j, k, cn = _src.channels();
1453     Op op;
1454     VecOp vop;
1455     volatile bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);
1456
1457     if( m == 3 )
1458     {
1459         if( size.width == 1 || size.height == 1 )
1460         {
1461             int len = size.width + size.height - 1;
1462             int sdelta = size.height == 1 ? cn : sstep;
1463             int sdelta0 = size.height == 1 ? 0 : sstep - cn;
1464             int ddelta = size.height == 1 ? cn : dstep;
1465
1466             for( i = 0; i < len; i++, src += sdelta0, dst += ddelta )
1467                 for( j = 0; j < cn; j++, src++ )
1468                 {
1469                     WT p0 = src[i > 0 ? -sdelta : 0];
1470                     WT p1 = src[0];
1471                     WT p2 = src[i < len - 1 ? sdelta : 0];
1472
1473                     op(p0, p1); op(p1, p2); op(p0, p1);
1474                     dst[j] = (T)p1;
1475                 }
1476             return;
1477         }
1478
1479         size.width *= cn;
1480         for( i = 0; i < size.height; i++, dst += dstep )
1481         {
1482             const T* row0 = src + std::max(i - 1, 0)*sstep;
1483             const T* row1 = src + i*sstep;
1484             const T* row2 = src + std::min(i + 1, size.height-1)*sstep;
1485             int limit = useSIMD ? cn : size.width;
1486
1487             for(j = 0;; )
1488             {
1489                 for( ; j < limit; j++ )
1490                 {
1491                     int j0 = j >= cn ? j - cn : j;
1492                     int j2 = j < size.width - cn ? j + cn : j;
1493                     WT p0 = row0[j0], p1 = row0[j], p2 = row0[j2];
1494                     WT p3 = row1[j0], p4 = row1[j], p5 = row1[j2];
1495                     WT p6 = row2[j0], p7 = row2[j], p8 = row2[j2];
1496
1497                     op(p1, p2); op(p4, p5); op(p7, p8); op(p0, p1);
1498                     op(p3, p4); op(p6, p7); op(p1, p2); op(p4, p5);
1499                     op(p7, p8); op(p0, p3); op(p5, p8); op(p4, p7);
1500                     op(p3, p6); op(p1, p4); op(p2, p5); op(p4, p7);
1501                     op(p4, p2); op(p6, p4); op(p4, p2);
1502                     dst[j] = (T)p4;
1503                 }
1504
1505                 if( limit == size.width )
1506                     break;
1507
1508                 for( ; j <= size.width - VecOp::SIZE - cn; j += VecOp::SIZE )
1509                 {
1510                     VT p0 = vop.load(row0+j-cn), p1 = vop.load(row0+j), p2 = vop.load(row0+j+cn);
1511                     VT p3 = vop.load(row1+j-cn), p4 = vop.load(row1+j), p5 = vop.load(row1+j+cn);
1512                     VT p6 = vop.load(row2+j-cn), p7 = vop.load(row2+j), p8 = vop.load(row2+j+cn);
1513
1514                     vop(p1, p2); vop(p4, p5); vop(p7, p8); vop(p0, p1);
1515                     vop(p3, p4); vop(p6, p7); vop(p1, p2); vop(p4, p5);
1516                     vop(p7, p8); vop(p0, p3); vop(p5, p8); vop(p4, p7);
1517                     vop(p3, p6); vop(p1, p4); vop(p2, p5); vop(p4, p7);
1518                     vop(p4, p2); vop(p6, p4); vop(p4, p2);
1519                     vop.store(dst+j, p4);
1520                 }
1521
1522                 limit = size.width;
1523             }
1524         }
1525     }
1526     else if( m == 5 )
1527     {
1528         if( size.width == 1 || size.height == 1 )
1529         {
1530             int len = size.width + size.height - 1;
1531             int sdelta = size.height == 1 ? cn : sstep;
1532             int sdelta0 = size.height == 1 ? 0 : sstep - cn;
1533             int ddelta = size.height == 1 ? cn : dstep;
1534
1535             for( i = 0; i < len; i++, src += sdelta0, dst += ddelta )
1536                 for( j = 0; j < cn; j++, src++ )
1537                 {
1538                     int i1 = i > 0 ? -sdelta : 0;
1539                     int i0 = i > 1 ? -sdelta*2 : i1;
1540                     int i3 = i < len-1 ? sdelta : 0;
1541                     int i4 = i < len-2 ? sdelta*2 : i3;
1542                     WT p0 = src[i0], p1 = src[i1], p2 = src[0], p3 = src[i3], p4 = src[i4];
1543
1544                     op(p0, p1); op(p3, p4); op(p2, p3); op(p3, p4); op(p0, p2);
1545                     op(p2, p4); op(p1, p3); op(p1, p2);
1546                     dst[j] = (T)p2;
1547                 }
1548             return;
1549         }
1550
1551         size.width *= cn;
1552         for( i = 0; i < size.height; i++, dst += dstep )
1553         {
1554             const T* row[5];
1555             row[0] = src + std::max(i - 2, 0)*sstep;
1556             row[1] = src + std::max(i - 1, 0)*sstep;
1557             row[2] = src + i*sstep;
1558             row[3] = src + std::min(i + 1, size.height-1)*sstep;
1559             row[4] = src + std::min(i + 2, size.height-1)*sstep;
1560             int limit = useSIMD ? cn*2 : size.width;
1561
1562             for(j = 0;; )
1563             {
1564                 for( ; j < limit; j++ )
1565                 {
1566                     WT p[25];
1567                     int j1 = j >= cn ? j - cn : j;
1568                     int j0 = j >= cn*2 ? j - cn*2 : j1;
1569                     int j3 = j < size.width - cn ? j + cn : j;
1570                     int j4 = j < size.width - cn*2 ? j + cn*2 : j3;
1571                     for( k = 0; k < 5; k++ )
1572                     {
1573                         const T* rowk = row[k];
1574                         p[k*5] = rowk[j0]; p[k*5+1] = rowk[j1];
1575                         p[k*5+2] = rowk[j]; p[k*5+3] = rowk[j3];
1576                         p[k*5+4] = rowk[j4];
1577                     }
1578
1579                     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]);
1580                     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]);
1581                     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]);
1582                     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]);
1583                     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]);
1584                     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]);
1585                     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]);
1586                     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]);
1587                     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]);
1588                     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]);
1589                     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]);
1590                     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]);
1591                     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]);
1592                     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]);
1593                     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]);
1594                     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]);
1595                     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]);
1596                     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]);
1597                     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]);
1598                     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]);
1599                     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]);
1600                     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]);
1601                     op(p[7], p[11]); op(p[11], p[13]); op(p[11], p[12]);
1602                     dst[j] = (T)p[12];
1603                 }
1604
1605                 if( limit == size.width )
1606                     break;
1607
1608                 for( ; j <= size.width - VecOp::SIZE - cn*2; j += VecOp::SIZE )
1609                 {
1610                     VT p[25];
1611                     for( k = 0; k < 5; k++ )
1612                     {
1613                         const T* rowk = row[k];
1614                         p[k*5] = vop.load(rowk+j-cn*2); p[k*5+1] = vop.load(rowk+j-cn);
1615                         p[k*5+2] = vop.load(rowk+j); p[k*5+3] = vop.load(rowk+j+cn);
1616                         p[k*5+4] = vop.load(rowk+j+cn*2);
1617                     }
1618
1619                     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]);
1620                     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]);
1621                     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]);
1622                     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]);
1623                     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]);
1624                     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]);
1625                     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]);
1626                     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]);
1627                     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]);
1628                     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]);
1629                     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]);
1630                     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]);
1631                     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]);
1632                     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]);
1633                     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]);
1634                     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]);
1635                     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]);
1636                     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]);
1637                     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]);
1638                     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]);
1639                     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]);
1640                     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]);
1641                     vop(p[7], p[11]); vop(p[11], p[13]); vop(p[11], p[12]);
1642                     vop.store(dst+j, p[12]);
1643                 }
1644
1645                 limit = size.width;
1646             }
1647         }
1648     }
1649 }
1650
1651 }
1652
1653 void cv::medianBlur( InputArray _src0, OutputArray _dst, int ksize )
1654 {
1655     Mat src0 = _src0.getMat();
1656     _dst.create( src0.size(), src0.type() );
1657     Mat dst = _dst.getMat();
1658
1659     if( ksize <= 1 )
1660     {
1661         src0.copyTo(dst);
1662         return;
1663     }
1664
1665     CV_Assert( ksize % 2 == 1 );
1666
1667 #ifdef HAVE_TEGRA_OPTIMIZATION
1668     if (tegra::medianBlur(src0, dst, ksize))
1669         return;
1670 #endif
1671
1672     bool useSortNet = ksize == 3 || (ksize == 5
1673 #if !CV_SSE2
1674             && src0.depth() > CV_8U
1675 #endif
1676         );
1677
1678     Mat src;
1679     if( useSortNet )
1680     {
1681         if( dst.data != src0.data )
1682             src = src0;
1683         else
1684             src0.copyTo(src);
1685
1686         if( src.depth() == CV_8U )
1687             medianBlur_SortNet<MinMax8u, MinMaxVec8u>( src, dst, ksize );
1688         else if( src.depth() == CV_16U )
1689             medianBlur_SortNet<MinMax16u, MinMaxVec16u>( src, dst, ksize );
1690         else if( src.depth() == CV_16S )
1691             medianBlur_SortNet<MinMax16s, MinMaxVec16s>( src, dst, ksize );
1692         else if( src.depth() == CV_32F )
1693             medianBlur_SortNet<MinMax32f, MinMaxVec32f>( src, dst, ksize );
1694         else
1695             CV_Error(CV_StsUnsupportedFormat, "");
1696
1697         return;
1698     }
1699     else
1700     {
1701         cv::copyMakeBorder( src0, src, 0, 0, ksize/2, ksize/2, BORDER_REPLICATE );
1702
1703         int cn = src0.channels();
1704         CV_Assert( src.depth() == CV_8U && (cn == 1 || cn == 3 || cn == 4) );
1705
1706         double img_size_mp = (double)(src0.total())/(1 << 20);
1707         if( ksize <= 3 + (img_size_mp < 1 ? 12 : img_size_mp < 4 ? 6 : 2)*(MEDIAN_HAVE_SIMD && checkHardwareSupport(CV_CPU_SSE2) ? 1 : 3))
1708             medianBlur_8u_Om( src, dst, ksize );
1709         else
1710             medianBlur_8u_O1( src, dst, ksize );
1711     }
1712 }
1713
1714 /****************************************************************************************\
1715                                    Bilateral Filtering
1716 \****************************************************************************************/
1717
1718 namespace cv
1719 {
1720
1721 class BilateralFilter_8u_Invoker :
1722     public ParallelLoopBody
1723 {
1724 public:
1725     BilateralFilter_8u_Invoker(Mat& _dest, const Mat& _temp, int _radius, int _maxk,
1726         int* _space_ofs, float *_space_weight, float *_color_weight) :
1727         temp(&_temp), dest(&_dest), radius(_radius),
1728         maxk(_maxk), space_ofs(_space_ofs), space_weight(_space_weight), color_weight(_color_weight)
1729     {
1730     }
1731
1732     virtual void operator() (const Range& range) const
1733     {
1734         int i, j, cn = dest->channels(), k;
1735         Size size = dest->size();
1736         #if CV_SSE3
1737         int CV_DECL_ALIGNED(16) buf[4];
1738         float CV_DECL_ALIGNED(16) bufSum[4];
1739         static const int CV_DECL_ALIGNED(16) bufSignMask[] = { 0x80000000, 0x80000000, 0x80000000, 0x80000000 };
1740         bool haveSSE3 = checkHardwareSupport(CV_CPU_SSE3);
1741         #endif
1742
1743         for( i = range.start; i < range.end; i++ )
1744         {
1745             const uchar* sptr = temp->ptr(i+radius) + radius*cn;
1746             uchar* dptr = dest->ptr(i);
1747
1748             if( cn == 1 )
1749             {
1750                 for( j = 0; j < size.width; j++ )
1751                 {
1752                     float sum = 0, wsum = 0;
1753                     int val0 = sptr[j];
1754                     k = 0;
1755                     #if CV_SSE3
1756                     if( haveSSE3 )
1757                     {
1758                         __m128 _val0 = _mm_set1_ps(static_cast<float>(val0));
1759                         const __m128 _signMask = _mm_load_ps((const float*)bufSignMask);
1760
1761                         for( ; k <= maxk - 4; k += 4 )
1762                         {
1763                             __m128 _valF = _mm_set_ps(sptr[j + space_ofs[k+3]], sptr[j + space_ofs[k+2]],
1764                                                       sptr[j + space_ofs[k+1]], sptr[j + space_ofs[k]]);
1765
1766                             __m128 _val = _mm_andnot_ps(_signMask, _mm_sub_ps(_valF, _val0));
1767                             _mm_store_si128((__m128i*)buf, _mm_cvtps_epi32(_val));
1768
1769                             __m128 _cw = _mm_set_ps(color_weight[buf[3]],color_weight[buf[2]],
1770                                                     color_weight[buf[1]],color_weight[buf[0]]);
1771                             __m128 _sw = _mm_loadu_ps(space_weight+k);
1772                             __m128 _w = _mm_mul_ps(_cw, _sw);
1773                              _cw = _mm_mul_ps(_w, _valF);
1774
1775                              _sw = _mm_hadd_ps(_w, _cw);
1776                              _sw = _mm_hadd_ps(_sw, _sw);
1777                              _mm_storel_pi((__m64*)bufSum, _sw);
1778
1779                              sum += bufSum[1];
1780                              wsum += bufSum[0];
1781                         }
1782                     }
1783                     #endif
1784                     for( ; k < maxk; k++ )
1785                     {
1786                         int val = sptr[j + space_ofs[k]];
1787                         float w = space_weight[k]*color_weight[std::abs(val - val0)];
1788                         sum += val*w;
1789                         wsum += w;
1790                     }
1791                     // overflow is not possible here => there is no need to use CV_CAST_8U
1792                     dptr[j] = (uchar)cvRound(sum/wsum);
1793                 }
1794             }
1795             else
1796             {
1797                 assert( cn == 3 );
1798                 for( j = 0; j < size.width*3; j += 3 )
1799                 {
1800                     float sum_b = 0, sum_g = 0, sum_r = 0, wsum = 0;
1801                     int b0 = sptr[j], g0 = sptr[j+1], r0 = sptr[j+2];
1802                     k = 0;
1803                     #if CV_SSE3
1804                     if( haveSSE3 )
1805                     {
1806                         const __m128 _b0 = _mm_set1_ps(static_cast<float>(b0));
1807                         const __m128 _g0 = _mm_set1_ps(static_cast<float>(g0));
1808                         const __m128 _r0 = _mm_set1_ps(static_cast<float>(r0));
1809                         const __m128 _signMask = _mm_load_ps((const float*)bufSignMask);
1810
1811                         for( ; k <= maxk - 4; k += 4 )
1812                         {
1813                             const uchar* sptr_k  = sptr + j + space_ofs[k];
1814                             const uchar* sptr_k1 = sptr + j + space_ofs[k+1];
1815                             const uchar* sptr_k2 = sptr + j + space_ofs[k+2];
1816                             const uchar* sptr_k3 = sptr + j + space_ofs[k+3];
1817
1818                             __m128 _b = _mm_set_ps(sptr_k3[0],sptr_k2[0],sptr_k1[0],sptr_k[0]);
1819                             __m128 _g = _mm_set_ps(sptr_k3[1],sptr_k2[1],sptr_k1[1],sptr_k[1]);
1820                             __m128 _r = _mm_set_ps(sptr_k3[2],sptr_k2[2],sptr_k1[2],sptr_k[2]);
1821
1822                             __m128 bt = _mm_andnot_ps(_signMask, _mm_sub_ps(_b,_b0));
1823                             __m128 gt = _mm_andnot_ps(_signMask, _mm_sub_ps(_g,_g0));
1824                             __m128 rt = _mm_andnot_ps(_signMask, _mm_sub_ps(_r,_r0));
1825
1826                             bt =_mm_add_ps(rt, _mm_add_ps(bt, gt));
1827                             _mm_store_si128((__m128i*)buf, _mm_cvtps_epi32(bt));
1828
1829                             __m128 _w  = _mm_set_ps(color_weight[buf[3]],color_weight[buf[2]],
1830                                                     color_weight[buf[1]],color_weight[buf[0]]);
1831                             __m128 _sw = _mm_loadu_ps(space_weight+k);
1832
1833                             _w = _mm_mul_ps(_w,_sw);
1834                             _b = _mm_mul_ps(_b, _w);
1835                             _g = _mm_mul_ps(_g, _w);
1836                             _r = _mm_mul_ps(_r, _w);
1837
1838                              _w = _mm_hadd_ps(_w, _b);
1839                              _g = _mm_hadd_ps(_g, _r);
1840
1841                              _w = _mm_hadd_ps(_w, _g);
1842                              _mm_store_ps(bufSum, _w);
1843
1844                              wsum  += bufSum[0];
1845                              sum_b += bufSum[1];
1846                              sum_g += bufSum[2];
1847                              sum_r += bufSum[3];
1848                          }
1849                     }
1850                     #endif
1851
1852                     for( ; k < maxk; k++ )
1853                     {
1854                         const uchar* sptr_k = sptr + j + space_ofs[k];
1855                         int b = sptr_k[0], g = sptr_k[1], r = sptr_k[2];
1856                         float w = space_weight[k]*color_weight[std::abs(b - b0) +
1857                                                                std::abs(g - g0) + std::abs(r - r0)];
1858                         sum_b += b*w; sum_g += g*w; sum_r += r*w;
1859                         wsum += w;
1860                     }
1861                     wsum = 1.f/wsum;
1862                     b0 = cvRound(sum_b*wsum);
1863                     g0 = cvRound(sum_g*wsum);
1864                     r0 = cvRound(sum_r*wsum);
1865                     dptr[j] = (uchar)b0; dptr[j+1] = (uchar)g0; dptr[j+2] = (uchar)r0;
1866                 }
1867             }
1868         }
1869     }
1870
1871 private:
1872     const Mat *temp;
1873     Mat *dest;
1874     int radius, maxk, *space_ofs;
1875     float *space_weight, *color_weight;
1876 };
1877
1878 #if defined (HAVE_IPP) && (IPP_VERSION_MAJOR >= 7)
1879 class IPPBilateralFilter_8u_Invoker :
1880     public ParallelLoopBody
1881 {
1882 public:
1883     IPPBilateralFilter_8u_Invoker(Mat &_src, Mat &_dst, double _sigma_color, double _sigma_space, int _radius, bool *_ok) :
1884       ParallelLoopBody(), src(_src), dst(_dst), sigma_color(_sigma_color), sigma_space(_sigma_space), radius(_radius), ok(_ok)
1885       {
1886           *ok = true;
1887       }
1888
1889       virtual void operator() (const Range& range) const
1890       {
1891           int d = radius * 2 + 1;
1892           IppiSize kernel = {d, d};
1893           IppiSize roi={dst.cols, range.end - range.start};
1894           int bufsize=0;
1895           ippiFilterBilateralGetBufSize_8u_C1R( ippiFilterBilateralGauss, roi, kernel, &bufsize);
1896           AutoBuffer<uchar> buf(bufsize);
1897           IppiFilterBilateralSpec *pSpec = (IppiFilterBilateralSpec *)alignPtr(&buf[0], 32);
1898           ippiFilterBilateralInit_8u_C1R( ippiFilterBilateralGauss, kernel, (Ipp32f)sigma_color, (Ipp32f)sigma_space, 1, pSpec );
1899           if( ippiFilterBilateral_8u_C1R( src.ptr<uchar>(range.start) + radius * ((int)src.step[0] + 1), (int)src.step[0], dst.ptr<uchar>(range.start), (int)dst.step[0], roi, kernel, pSpec ) < 0)
1900               *ok = false;
1901       }
1902 private:
1903     Mat &src;
1904     Mat &dst;
1905     double sigma_color;
1906     double sigma_space;
1907     int radius;
1908     bool *ok;
1909     const IPPBilateralFilter_8u_Invoker& operator= (const IPPBilateralFilter_8u_Invoker&);
1910 };
1911 #endif
1912
1913 static void
1914 bilateralFilter_8u( const Mat& src, Mat& dst, int d,
1915     double sigma_color, double sigma_space,
1916     int borderType )
1917 {
1918
1919     int cn = src.channels();
1920     int i, j, maxk, radius;
1921     Size size = src.size();
1922
1923     CV_Assert( (src.type() == CV_8UC1 || src.type() == CV_8UC3) &&
1924               src.type() == dst.type() && src.size() == dst.size() &&
1925               src.data != dst.data );
1926
1927     if( sigma_color <= 0 )
1928         sigma_color = 1;
1929     if( sigma_space <= 0 )
1930         sigma_space = 1;
1931
1932     double gauss_color_coeff = -0.5/(sigma_color*sigma_color);
1933     double gauss_space_coeff = -0.5/(sigma_space*sigma_space);
1934
1935     if( d <= 0 )
1936         radius = cvRound(sigma_space*1.5);
1937     else
1938         radius = d/2;
1939     radius = MAX(radius, 1);
1940     d = radius*2 + 1;
1941
1942     Mat temp;
1943     copyMakeBorder( src, temp, radius, radius, radius, radius, borderType );
1944
1945 #if defined HAVE_IPP && (IPP_VERSION_MAJOR >= 7)
1946     if( cn == 1 )
1947     {
1948         bool ok;
1949         IPPBilateralFilter_8u_Invoker body(temp, dst, sigma_color * sigma_color, sigma_space * sigma_space, radius, &ok );
1950         parallel_for_(Range(0, dst.rows), body, dst.total()/(double)(1<<16));
1951         if( ok ) return;
1952     }
1953 #endif
1954
1955     vector<float> _color_weight(cn*256);
1956     vector<float> _space_weight(d*d);
1957     vector<int> _space_ofs(d*d);
1958     float* color_weight = &_color_weight[0];
1959     float* space_weight = &_space_weight[0];
1960     int* space_ofs = &_space_ofs[0];
1961
1962     // initialize color-related bilateral filter coefficients
1963
1964     for( i = 0; i < 256*cn; i++ )
1965         color_weight[i] = (float)std::exp(i*i*gauss_color_coeff);
1966
1967     // initialize space-related bilateral filter coefficients
1968     for( i = -radius, maxk = 0; i <= radius; i++ )
1969     {
1970         j = -radius;
1971
1972         for( ;j <= radius; j++ )
1973         {
1974             double r = std::sqrt((double)i*i + (double)j*j);
1975             if( r > radius )
1976                 continue;
1977             space_weight[maxk] = (float)std::exp(r*r*gauss_space_coeff);
1978             space_ofs[maxk++] = (int)(i*temp.step + j*cn);
1979         }
1980     }
1981
1982     BilateralFilter_8u_Invoker body(dst, temp, radius, maxk, space_ofs, space_weight, color_weight);
1983     parallel_for_(Range(0, size.height), body, dst.total()/(double)(1<<16));
1984 }
1985
1986
1987 class BilateralFilter_32f_Invoker :
1988     public ParallelLoopBody
1989 {
1990 public:
1991
1992     BilateralFilter_32f_Invoker(int _cn, int _radius, int _maxk, int *_space_ofs,
1993         const Mat& _temp, Mat& _dest, float _scale_index, float *_space_weight, float *_expLUT) :
1994         cn(_cn), radius(_radius), maxk(_maxk), space_ofs(_space_ofs),
1995         temp(&_temp), dest(&_dest), scale_index(_scale_index), space_weight(_space_weight), expLUT(_expLUT)
1996     {
1997     }
1998
1999     virtual void operator() (const Range& range) const
2000     {
2001         int i, j, k;
2002         Size size = dest->size();
2003         #if CV_SSE3
2004         int CV_DECL_ALIGNED(16) idxBuf[4];
2005         float CV_DECL_ALIGNED(16) bufSum32[4];
2006         static const int CV_DECL_ALIGNED(16) bufSignMask[] = { 0x80000000, 0x80000000, 0x80000000, 0x80000000 };
2007         bool haveSSE3 = checkHardwareSupport(CV_CPU_SSE3);
2008         #endif
2009
2010         for( i = range.start; i < range.end; i++ )
2011         {
2012             const float* sptr = temp->ptr<float>(i+radius) + radius*cn;
2013             float* dptr = dest->ptr<float>(i);
2014
2015             if( cn == 1 )
2016             {
2017                 for( j = 0; j < size.width; j++ )
2018                 {
2019                     float sum = 0, wsum = 0;
2020                     float val0 = sptr[j];
2021                     k = 0;
2022                     #if CV_SSE3
2023                     if( haveSSE3 )
2024                     {
2025                         const __m128 _val0 = _mm_set1_ps(sptr[j]);
2026                         const __m128 _scale_index = _mm_set1_ps(scale_index);
2027                         const __m128 _signMask = _mm_load_ps((const float*)bufSignMask);
2028
2029                         for( ; k <= maxk - 4 ; k += 4 )
2030                         {
2031                             __m128 _sw    = _mm_loadu_ps(space_weight + k);
2032                             __m128 _val   = _mm_set_ps(sptr[j + space_ofs[k+3]], sptr[j + space_ofs[k+2]],
2033                                                        sptr[j + space_ofs[k+1]], sptr[j + space_ofs[k]]);
2034                             __m128 _alpha = _mm_mul_ps(_mm_andnot_ps( _signMask, _mm_sub_ps(_val,_val0)), _scale_index);
2035
2036                             __m128i _idx = _mm_cvtps_epi32(_alpha);
2037                             _mm_store_si128((__m128i*)idxBuf, _idx);
2038                             _alpha = _mm_sub_ps(_alpha, _mm_cvtepi32_ps(_idx));
2039
2040                             __m128 _explut  = _mm_set_ps(expLUT[idxBuf[3]], expLUT[idxBuf[2]],
2041                                                          expLUT[idxBuf[1]], expLUT[idxBuf[0]]);
2042                             __m128 _explut1 = _mm_set_ps(expLUT[idxBuf[3]+1], expLUT[idxBuf[2]+1],
2043                                                          expLUT[idxBuf[1]+1], expLUT[idxBuf[0]+1]);
2044
2045                             __m128 _w = _mm_mul_ps(_sw, _mm_add_ps(_explut, _mm_mul_ps(_alpha, _mm_sub_ps(_explut1, _explut))));
2046                             _val = _mm_mul_ps(_w, _val);
2047
2048                              _sw = _mm_hadd_ps(_w, _val);
2049                              _sw = _mm_hadd_ps(_sw, _sw);
2050                              _mm_storel_pi((__m64*)bufSum32, _sw);
2051
2052                              sum += bufSum32[1];
2053                              wsum += bufSum32[0];
2054                         }
2055                     }
2056                     #endif
2057
2058                     for( ; k < maxk; k++ )
2059                     {
2060                         float val = sptr[j + space_ofs[k]];
2061                         float alpha = (float)(std::abs(val - val0)*scale_index);
2062                         int idx = cvFloor(alpha);
2063                         alpha -= idx;
2064                         float w = space_weight[k]*(expLUT[idx] + alpha*(expLUT[idx+1] - expLUT[idx]));
2065                         sum += val*w;
2066                         wsum += w;
2067                     }
2068                     dptr[j] = (float)(sum/wsum);
2069                 }
2070             }
2071             else
2072             {
2073                 assert( cn == 3 );
2074                 for( j = 0; j < size.width*3; j += 3 )
2075                 {
2076                     float sum_b = 0, sum_g = 0, sum_r = 0, wsum = 0;
2077                     float b0 = sptr[j], g0 = sptr[j+1], r0 = sptr[j+2];
2078                     k = 0;
2079                     #if  CV_SSE3
2080                     if( haveSSE3 )
2081                     {
2082                         const __m128 _b0 = _mm_set1_ps(b0);
2083                         const __m128 _g0 = _mm_set1_ps(g0);
2084                         const __m128 _r0 = _mm_set1_ps(r0);
2085                         const __m128 _scale_index = _mm_set1_ps(scale_index);
2086                         const __m128 _signMask = _mm_load_ps((const float*)bufSignMask);
2087
2088                         for( ; k <= maxk-4; k += 4 )
2089                         {
2090                             __m128 _sw = _mm_loadu_ps(space_weight + k);
2091
2092                             const float* sptr_k  = sptr + j + space_ofs[k];
2093                             const float* sptr_k1 = sptr + j + space_ofs[k+1];
2094                             const float* sptr_k2 = sptr + j + space_ofs[k+2];
2095                             const float* sptr_k3 = sptr + j + space_ofs[k+3];
2096
2097                             __m128 _b = _mm_set_ps(sptr_k3[0], sptr_k2[0], sptr_k1[0], sptr_k[0]);
2098                             __m128 _g = _mm_set_ps(sptr_k3[1], sptr_k2[1], sptr_k1[1], sptr_k[1]);
2099                             __m128 _r = _mm_set_ps(sptr_k3[2], sptr_k2[2], sptr_k1[2], sptr_k[2]);
2100
2101                             __m128 _bt = _mm_andnot_ps(_signMask,_mm_sub_ps(_b,_b0));
2102                             __m128 _gt = _mm_andnot_ps(_signMask,_mm_sub_ps(_g,_g0));
2103                             __m128 _rt = _mm_andnot_ps(_signMask,_mm_sub_ps(_r,_r0));
2104
2105                             __m128 _alpha = _mm_mul_ps(_scale_index, _mm_add_ps(_rt,_mm_add_ps(_bt, _gt)));
2106
2107                             __m128i _idx  = _mm_cvtps_epi32(_alpha);
2108                             _mm_store_si128((__m128i*)idxBuf, _idx);
2109                             _alpha = _mm_sub_ps(_alpha, _mm_cvtepi32_ps(_idx));
2110
2111                             __m128 _explut  = _mm_set_ps(expLUT[idxBuf[3]], expLUT[idxBuf[2]], expLUT[idxBuf[1]], expLUT[idxBuf[0]]);
2112                             __m128 _explut1 = _mm_set_ps(expLUT[idxBuf[3]+1], expLUT[idxBuf[2]+1], expLUT[idxBuf[1]+1], expLUT[idxBuf[0]+1]);
2113
2114                             __m128 _w = _mm_mul_ps(_sw, _mm_add_ps(_explut, _mm_mul_ps(_alpha, _mm_sub_ps(_explut1, _explut))));
2115
2116                             _b = _mm_mul_ps(_b, _w);
2117                             _g = _mm_mul_ps(_g, _w);
2118                             _r = _mm_mul_ps(_r, _w);
2119
2120                              _w = _mm_hadd_ps(_w, _b);
2121                              _g = _mm_hadd_ps(_g, _r);
2122
2123                              _w = _mm_hadd_ps(_w, _g);
2124                              _mm_store_ps(bufSum32, _w);
2125
2126                              wsum  += bufSum32[0];
2127                              sum_b += bufSum32[1];
2128                              sum_g += bufSum32[2];
2129                              sum_r += bufSum32[3];
2130                         }
2131
2132                     }
2133                     #endif
2134
2135                     for(; k < maxk; k++ )
2136                     {
2137                         const float* sptr_k = sptr + j + space_ofs[k];
2138                         float b = sptr_k[0], g = sptr_k[1], r = sptr_k[2];
2139                         float alpha = (float)((std::abs(b - b0) +
2140                             std::abs(g - g0) + std::abs(r - r0))*scale_index);
2141                         int idx = cvFloor(alpha);
2142                         alpha -= idx;
2143                         float w = space_weight[k]*(expLUT[idx] + alpha*(expLUT[idx+1] - expLUT[idx]));
2144                         sum_b += b*w; sum_g += g*w; sum_r += r*w;
2145                         wsum += w;
2146                     }
2147                     wsum = 1.f/wsum;
2148                     b0 = sum_b*wsum;
2149                     g0 = sum_g*wsum;
2150                     r0 = sum_r*wsum;
2151                     dptr[j] = b0; dptr[j+1] = g0; dptr[j+2] = r0;
2152                 }
2153             }
2154         }
2155     }
2156
2157 private:
2158     int cn, radius, maxk, *space_ofs;
2159     const Mat* temp;
2160     Mat *dest;
2161     float scale_index, *space_weight, *expLUT;
2162 };
2163
2164
2165 static void
2166 bilateralFilter_32f( const Mat& src, Mat& dst, int d,
2167                      double sigma_color, double sigma_space,
2168                      int borderType )
2169 {
2170     int cn = src.channels();
2171     int i, j, maxk, radius;
2172     double minValSrc=-1, maxValSrc=1;
2173     const int kExpNumBinsPerChannel = 1 << 12;
2174     int kExpNumBins = 0;
2175     float lastExpVal = 1.f;
2176     float len, scale_index;
2177     Size size = src.size();
2178
2179     CV_Assert( (src.type() == CV_32FC1 || src.type() == CV_32FC3) &&
2180         src.type() == dst.type() && src.size() == dst.size() &&
2181         src.data != dst.data );
2182
2183     if( sigma_color <= 0 )
2184         sigma_color = 1;
2185     if( sigma_space <= 0 )
2186         sigma_space = 1;
2187
2188     double gauss_color_coeff = -0.5/(sigma_color*sigma_color);
2189     double gauss_space_coeff = -0.5/(sigma_space*sigma_space);
2190
2191     if( d <= 0 )
2192         radius = cvRound(sigma_space*1.5);
2193     else
2194         radius = d/2;
2195     radius = MAX(radius, 1);
2196     d = radius*2 + 1;
2197     // compute the min/max range for the input image (even if multichannel)
2198
2199     minMaxLoc( src.reshape(1), &minValSrc, &maxValSrc );
2200     if(std::abs(minValSrc - maxValSrc) < FLT_EPSILON)
2201     {
2202         src.copyTo(dst);
2203         return;
2204     }
2205
2206     // temporary copy of the image with borders for easy processing
2207     Mat temp;
2208     copyMakeBorder( src, temp, radius, radius, radius, radius, borderType );
2209     const double insteadNaNValue = -5. * sigma_color;
2210     patchNaNs( temp, insteadNaNValue ); // this replacement of NaNs makes the assumption that depth values are nonnegative
2211                                         // TODO: make insteadNaNValue avalible in the outside function interface to control the cases breaking the assumption
2212     // allocate lookup tables
2213     vector<float> _space_weight(d*d);
2214     vector<int> _space_ofs(d*d);
2215     float* space_weight = &_space_weight[0];
2216     int* space_ofs = &_space_ofs[0];
2217
2218     // assign a length which is slightly more than needed
2219     len = (float)(maxValSrc - minValSrc) * cn;
2220     kExpNumBins = kExpNumBinsPerChannel * cn;
2221     vector<float> _expLUT(kExpNumBins+2);
2222     float* expLUT = &_expLUT[0];
2223
2224     scale_index = kExpNumBins/len;
2225
2226     // initialize the exp LUT
2227     for( i = 0; i < kExpNumBins+2; i++ )
2228     {
2229         if( lastExpVal > 0.f )
2230         {
2231             double val =  i / scale_index;
2232             expLUT[i] = (float)std::exp(val * val * gauss_color_coeff);
2233             lastExpVal = expLUT[i];
2234         }
2235         else
2236             expLUT[i] = 0.f;
2237     }
2238
2239     // initialize space-related bilateral filter coefficients
2240     for( i = -radius, maxk = 0; i <= radius; i++ )
2241         for( j = -radius; j <= radius; j++ )
2242         {
2243             double r = std::sqrt((double)i*i + (double)j*j);
2244             if( r > radius )
2245                 continue;
2246             space_weight[maxk] = (float)std::exp(r*r*gauss_space_coeff);
2247             space_ofs[maxk++] = (int)(i*(temp.step/sizeof(float)) + j*cn);
2248         }
2249
2250     // parallel_for usage
2251
2252     BilateralFilter_32f_Invoker body(cn, radius, maxk, space_ofs, temp, dst, scale_index, space_weight, expLUT);
2253     parallel_for_(Range(0, size.height), body, dst.total()/(double)(1<<16));
2254 }
2255
2256 }
2257
2258 void cv::bilateralFilter( InputArray _src, OutputArray _dst, int d,
2259                       double sigmaColor, double sigmaSpace,
2260                       int borderType )
2261 {
2262     Mat src = _src.getMat();
2263     _dst.create( src.size(), src.type() );
2264     Mat dst = _dst.getMat();
2265
2266     if( src.depth() == CV_8U )
2267         bilateralFilter_8u( src, dst, d, sigmaColor, sigmaSpace, borderType );
2268     else if( src.depth() == CV_32F )
2269         bilateralFilter_32f( src, dst, d, sigmaColor, sigmaSpace, borderType );
2270     else
2271         CV_Error( CV_StsUnsupportedFormat,
2272         "Bilateral filtering is only implemented for 8u and 32f images" );
2273 }
2274
2275
2276 /****************************************************************************************\
2277                                   Adaptive Bilateral Filtering
2278 \****************************************************************************************/
2279
2280 namespace cv
2281 {
2282 #ifndef ABF_CALCVAR
2283 #define ABF_CALCVAR 1
2284 #endif
2285
2286 #ifndef ABF_FIXED_WEIGHT
2287 #define ABF_FIXED_WEIGHT 0
2288 #endif
2289
2290 #ifndef ABF_GAUSSIAN
2291 #define ABF_GAUSSIAN 1
2292 #endif
2293
2294 class adaptiveBilateralFilter_8u_Invoker :
2295     public ParallelLoopBody
2296 {
2297 public:
2298     adaptiveBilateralFilter_8u_Invoker(Mat& _dest, const Mat& _temp, Size _ksize, double _sigma_space, double _maxSigmaColor, Point _anchor) :
2299         temp(&_temp), dest(&_dest), ksize(_ksize), sigma_space(_sigma_space), maxSigma_Color(_maxSigmaColor), anchor(_anchor)
2300     {
2301         if( sigma_space <= 0 )
2302             sigma_space = 1;
2303         CV_Assert((ksize.width & 1) && (ksize.height & 1));
2304         space_weight.resize(ksize.width * ksize.height);
2305         double sigma2 = sigma_space * sigma_space;
2306         int idx = 0;
2307         int w = ksize.width / 2;
2308         int h = ksize.height / 2;
2309         for(int y=-h; y<=h; y++)
2310             for(int x=-w; x<=w; x++)
2311         {
2312 #if ABF_GAUSSIAN
2313             space_weight[idx++] = (float)exp ( -0.5*(x * x + y * y)/sigma2);
2314 #else
2315             space_weight[idx++] = (float)(sigma2 / (sigma2 + x * x + y * y));
2316 #endif
2317         }
2318     }
2319     virtual void operator()(const Range& range) const
2320     {
2321         int cn = dest->channels();
2322         int anX = anchor.x;
2323
2324         const uchar *tptr;
2325
2326         for(int i = range.start;i < range.end; i++)
2327         {
2328             int startY = i;
2329             if(cn == 1)
2330             {
2331                 float var;
2332                 int currVal;
2333                 int sumVal = 0;
2334                 int sumValSqr = 0;
2335                 int currValCenter;
2336                 int currWRTCenter;
2337                 float weight;
2338                 float totalWeight = 0.;
2339                 float tmpSum = 0.;
2340
2341                 for(int j = 0;j < dest->cols *cn; j+=cn)
2342                 {
2343                     sumVal = 0;
2344                     sumValSqr= 0;
2345                     totalWeight = 0.;
2346                     tmpSum = 0.;
2347
2348                     // Top row: don't sum the very last element
2349                     int startLMJ = 0;
2350                     int endLMJ  = ksize.width  - 1;
2351                     int howManyAll = (anX *2 +1)*(ksize.width );
2352 #if ABF_CALCVAR
2353                     for(int x = startLMJ; x< endLMJ; x++)
2354                     {
2355                         tptr = temp->ptr(startY + x) +j;
2356                         for(int y=-anX; y<=anX; y++)
2357                         {
2358                             currVal = tptr[cn*(y+anX)];
2359                             sumVal += currVal;
2360                             sumValSqr += (currVal *currVal);
2361                         }
2362                     }
2363                     var = ( (sumValSqr * howManyAll)- sumVal * sumVal )  /  ( (float)(howManyAll*howManyAll));
2364
2365                     if(var < 0.01)
2366                         var = 0.01f;
2367                     else if(var > (float)(maxSigma_Color*maxSigma_Color) )
2368                         var =  (float)(maxSigma_Color*maxSigma_Color) ;
2369
2370 #else
2371                     var = maxSigmaColor*maxSigmaColor;
2372 #endif
2373                     startLMJ = 0;
2374                     endLMJ = ksize.width;
2375                     tptr = temp->ptr(startY + (startLMJ+ endLMJ)/2);
2376                     currValCenter =tptr[j+cn*anX];
2377                     for(int x = startLMJ; x< endLMJ; x++)
2378                     {
2379                         tptr = temp->ptr(startY + x) +j;
2380                         for(int y=-anX; y<=anX; y++)
2381                         {
2382 #if ABF_FIXED_WEIGHT
2383                             weight = 1.0;
2384 #else
2385                             currVal = tptr[cn*(y+anX)];
2386                             currWRTCenter = currVal - currValCenter;
2387
2388 #if ABF_GAUSSIAN
2389                             weight = exp ( -0.5f * currWRTCenter * currWRTCenter/var ) * space_weight[x*ksize.width+y+anX];
2390 #else
2391                             weight = var / ( var + (currWRTCenter * currWRTCenter) ) * space_weight[x*ksize.width+y+anX];
2392 #endif
2393
2394 #endif
2395                             tmpSum += ((float)tptr[cn*(y+anX)] * weight);
2396                             totalWeight += weight;
2397                         }
2398                     }
2399                     tmpSum /= totalWeight;
2400
2401                    dest->at<uchar>(startY ,j)= static_cast<uchar>(tmpSum);
2402                 }
2403             }
2404             else
2405             {
2406                 assert(cn == 3);
2407                 float var_b, var_g, var_r;
2408                 int currVal_b, currVal_g, currVal_r;
2409                 int sumVal_b= 0, sumVal_g= 0, sumVal_r= 0;
2410                 int sumValSqr_b= 0, sumValSqr_g= 0, sumValSqr_r= 0;
2411                 int currValCenter_b= 0, currValCenter_g= 0, currValCenter_r= 0;
2412                 int currWRTCenter_b, currWRTCenter_g, currWRTCenter_r;
2413                 float weight_b, weight_g, weight_r;
2414                 float totalWeight_b= 0., totalWeight_g= 0., totalWeight_r= 0.;
2415                 float tmpSum_b = 0., tmpSum_g= 0., tmpSum_r = 0.;
2416
2417                 for(int j = 0;j < dest->cols *cn; j+=cn)
2418                 {
2419                     sumVal_b= 0, sumVal_g= 0, sumVal_r= 0;
2420                     sumValSqr_b= 0, sumValSqr_g= 0, sumValSqr_r= 0;
2421                     totalWeight_b= 0., totalWeight_g= 0., totalWeight_r= 0.;
2422                     tmpSum_b = 0., tmpSum_g= 0., tmpSum_r = 0.;
2423
2424                     // Top row: don't sum the very last element
2425                     int startLMJ = 0;
2426                     int endLMJ  = ksize.width - 1;
2427                     int howManyAll = (anX *2 +1)*(ksize.width);
2428 #if ABF_CALCVAR
2429                     float max_var = (float)( maxSigma_Color*maxSigma_Color);
2430                     for(int x = startLMJ; x< endLMJ; x++)
2431                     {
2432                         tptr = temp->ptr(startY + x) +j;
2433                         for(int y=-anX; y<=anX; y++)
2434                         {
2435                             currVal_b = tptr[cn*(y+anX)], currVal_g = tptr[cn*(y+anX)+1], currVal_r =tptr[cn*(y+anX)+2];
2436                             sumVal_b += currVal_b;
2437                             sumVal_g += currVal_g;
2438                             sumVal_r += currVal_r;
2439                             sumValSqr_b += (currVal_b *currVal_b);
2440                             sumValSqr_g += (currVal_g *currVal_g);
2441                             sumValSqr_r += (currVal_r *currVal_r);
2442                         }
2443                     }
2444                     var_b =  ( (sumValSqr_b * howManyAll)- sumVal_b * sumVal_b )  /  ( (float)(howManyAll*howManyAll));
2445                     var_g =  ( (sumValSqr_g * howManyAll)- sumVal_g * sumVal_g )  /  ( (float)(howManyAll*howManyAll));
2446                     var_r =  ( (sumValSqr_r * howManyAll)- sumVal_r * sumVal_r )  /  ( (float)(howManyAll*howManyAll));
2447
2448                     if(var_b < 0.01)
2449                         var_b = 0.01f;
2450                     else if(var_b > max_var )
2451                         var_b =  (float)(max_var) ;
2452
2453                     if(var_g < 0.01)
2454                         var_g = 0.01f;
2455                     else if(var_g > max_var )
2456                         var_g =  (float)(max_var) ;
2457
2458                     if(var_r < 0.01)
2459                         var_r = 0.01f;
2460                     else if(var_r > max_var )
2461                         var_r =  (float)(max_var) ;
2462
2463 #else
2464                     var_b = maxSigma_Color*maxSigma_Color; var_g = maxSigma_Color*maxSigma_Color; var_r = maxSigma_Color*maxSigma_Color;
2465 #endif
2466                     startLMJ = 0;
2467                     endLMJ = ksize.width;
2468                     tptr = temp->ptr(startY + (startLMJ+ endLMJ)/2) + j;
2469                     currValCenter_b =tptr[cn*anX], currValCenter_g =tptr[cn*anX+1], currValCenter_r =tptr[cn*anX+2];
2470                     for(int x = startLMJ; x< endLMJ; x++)
2471                     {
2472                         tptr = temp->ptr(startY + x) +j;
2473                         for(int y=-anX; y<=anX; y++)
2474                         {
2475 #if ABF_FIXED_WEIGHT
2476                             weight_b = 1.0;
2477                             weight_g = 1.0;
2478                             weight_r = 1.0;
2479 #else
2480                             currVal_b = tptr[cn*(y+anX)];currVal_g=tptr[cn*(y+anX)+1];currVal_r=tptr[cn*(y+anX)+2];
2481                             currWRTCenter_b = currVal_b - currValCenter_b;
2482                             currWRTCenter_g = currVal_g - currValCenter_g;
2483                             currWRTCenter_r = currVal_r - currValCenter_r;
2484
2485                             float cur_spw = space_weight[x*ksize.width+y+anX];
2486
2487 #if ABF_GAUSSIAN
2488                             weight_b = exp( -0.5f * currWRTCenter_b * currWRTCenter_b/ var_b ) * cur_spw;
2489                             weight_g = exp( -0.5f * currWRTCenter_g * currWRTCenter_g/ var_g ) * cur_spw;
2490                             weight_r = exp( -0.5f * currWRTCenter_r * currWRTCenter_r/ var_r ) * cur_spw;
2491 #else
2492                             weight_b = var_b / ( var_b + (currWRTCenter_b * currWRTCenter_b) ) * cur_spw;
2493                             weight_g = var_g / ( var_g + (currWRTCenter_g * currWRTCenter_g) ) * cur_spw;
2494                             weight_r = var_r / ( var_r + (currWRTCenter_r * currWRTCenter_r) ) * cur_spw;
2495 #endif
2496 #endif
2497                             tmpSum_b += ((float)tptr[cn*(y+anX)]   * weight_b);
2498                             tmpSum_g += ((float)tptr[cn*(y+anX)+1] * weight_g);
2499                             tmpSum_r += ((float)tptr[cn*(y+anX)+2] * weight_r);
2500                             totalWeight_b += weight_b, totalWeight_g += weight_g, totalWeight_r += weight_r;
2501                         }
2502                     }
2503                     tmpSum_b /= totalWeight_b;
2504                     tmpSum_g /= totalWeight_g;
2505                     tmpSum_r /= totalWeight_r;
2506
2507                     dest->at<uchar>(startY,j  )= static_cast<uchar>(tmpSum_b);
2508                     dest->at<uchar>(startY,j+1)= static_cast<uchar>(tmpSum_g);
2509                     dest->at<uchar>(startY,j+2)= static_cast<uchar>(tmpSum_r);
2510                 }
2511             }
2512         }
2513     }
2514 private:
2515     const Mat *temp;
2516     Mat *dest;
2517     Size ksize;
2518     double sigma_space;
2519     double maxSigma_Color;
2520     Point anchor;
2521     vector<float> space_weight;
2522 };
2523 static void adaptiveBilateralFilter_8u( const Mat& src, Mat& dst, Size ksize, double sigmaSpace, double maxSigmaColor, Point anchor, int borderType )
2524 {
2525     Size size = src.size();
2526
2527     CV_Assert( (src.type() == CV_8UC1 || src.type() == CV_8UC3) &&
2528               src.type() == dst.type() && src.size() == dst.size() &&
2529               src.data != dst.data );
2530     Mat temp;
2531     copyMakeBorder(src, temp, anchor.x, anchor.y, anchor.x, anchor.y, borderType);
2532
2533     adaptiveBilateralFilter_8u_Invoker body(dst, temp, ksize, sigmaSpace, maxSigmaColor, anchor);
2534     parallel_for_(Range(0, size.height), body, dst.total()/(double)(1<<16));
2535 }
2536 }
2537 void cv::adaptiveBilateralFilter( InputArray _src, OutputArray _dst, Size ksize,
2538                                   double sigmaSpace, double maxSigmaColor, Point anchor, int borderType )
2539 {
2540     Mat src = _src.getMat();
2541     _dst.create(src.size(), src.type());
2542     Mat dst = _dst.getMat();
2543
2544     CV_Assert(src.type() == CV_8UC1 || src.type() == CV_8UC3);
2545
2546     anchor = normalizeAnchor(anchor,ksize);
2547     if( src.depth() == CV_8U )
2548         adaptiveBilateralFilter_8u( src, dst, ksize, sigmaSpace, maxSigmaColor, anchor, borderType );
2549     else
2550         CV_Error( CV_StsUnsupportedFormat,
2551         "Adaptive Bilateral filtering is only implemented for 8u images" );
2552 }
2553
2554 //////////////////////////////////////////////////////////////////////////////////////////
2555
2556 CV_IMPL void
2557 cvSmooth( const void* srcarr, void* dstarr, int smooth_type,
2558           int param1, int param2, double param3, double param4 )
2559 {
2560     cv::Mat src = cv::cvarrToMat(srcarr), dst0 = cv::cvarrToMat(dstarr), dst = dst0;
2561
2562     CV_Assert( dst.size() == src.size() &&
2563         (smooth_type == CV_BLUR_NO_SCALE || dst.type() == src.type()) );
2564
2565     if( param2 <= 0 )
2566         param2 = param1;
2567
2568     if( smooth_type == CV_BLUR || smooth_type == CV_BLUR_NO_SCALE )
2569         cv::boxFilter( src, dst, dst.depth(), cv::Size(param1, param2), cv::Point(-1,-1),
2570             smooth_type == CV_BLUR, cv::BORDER_REPLICATE );
2571     else if( smooth_type == CV_GAUSSIAN )
2572         cv::GaussianBlur( src, dst, cv::Size(param1, param2), param3, param4, cv::BORDER_REPLICATE );
2573     else if( smooth_type == CV_MEDIAN )
2574         cv::medianBlur( src, dst, param1 );
2575     else
2576         cv::bilateralFilter( src, dst, param1, param3, param4, cv::BORDER_REPLICATE );
2577
2578     if( dst.data != dst0.data )
2579         CV_Error( CV_StsUnmatchedFormats, "The destination image does not have the proper type" );
2580 }
2581
2582 /* End of file. */