vcs.maemo.org Git - opencv/blob - src/cv/cvoptflowgf.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 "_cv.h"
  44
  45 //
  46 // 2D dense optical flow algorithm from the following paper:
  47 // Gunnar Farneback. "Two-Frame Motion Estimation Based on Polynomial Expansion".
  48 // Proceedings of the 13th Scandinavian Conference on Image Analysis, Gothenburg, Sweden
  49 //
  50
  51 namespace cv
  52 {
  53
  54 static void
  55 FarnebackPolyExp( const Mat& src, Mat& dst, int n, double sigma )
  56 {
  57     int k, x, y;
  58
  59     assert( src.type() == CV_32FC1 );
  60     int width = src.cols;
  61     int height = src.rows;
  62     AutoBuffer<float> kbuf(n*6 + 3), _row((width + n*2)*3);
  63     float* g = kbuf + n;
  64     float* xg = g + n*2 + 1;
  65     float* xxg = xg + n*2 + 1;
  66     float *row = (float*)_row + n*3;
  67
  68     if( sigma < FLT_EPSILON )
  69         sigma = n*0.3;
  70
  71     double s = 0.;
  72     for( x = -n; x <= n; x++ )
  73     {
  74         g[x] = (float)std::exp(-x*x/(2*sigma*sigma));
  75         s += g[x];
  76     }
  77
  78     s = 1./s;
  79     for( x = -n; x <= n; x++ )
  80     {
  81         g[x] = (float)(g[x]*s);
  82         xg[x] = (float)(x*g[x]);
  83         xxg[x] = (float)(x*x*g[x]);
  84     }
  85
  86     Mat_<double> G = Mat_<double>::zeros(6, 6);
  87
  88     for( y = -n; y <= n; y++ )
  89         for( x = -n; x <= n; x++ )
  90         {
  91             G(0,0) += g[y]*g[x];
  92             G(1,1) += g[y]*g[x]*x*x;
  93             G(3,3) += g[y]*g[x]*x*x*x*x;
  94             G(5,5) += g[y]*g[x]*x*x*y*y;
  95         }
  96
  97     //G[0][0] = 1.;
  98     G(2,2) = G(0,3) = G(0,4) = G(3,0) = G(4,0) = G(1,1);
  99     G(4,4) = G(3,3);
 100     G(3,4) = G(4,3) = G(5,5);
 101
 102     // invG:
 103     // [ x        e  e    ]
 104     // [    y             ]
 105     // [       y          ]
 106     // [ e        z       ]
 107     // [ e           z    ]
 108     // [                u ]
 109     Mat_<double> invG = G.inv(DECOMP_CHOLESKY);
 110     double ig11 = invG(1,1), ig03 = invG(0,3), ig33 = invG(3,3), ig55 = invG(5,5);
 111
 112     dst.create( height, width, CV_32FC(5));
 113
 114     for( y = 0; y < height; y++ )
 115     {
 116         float g0 = g[0], g1, g2;
 117         float *srow0 = (float*)(src.data + src.step*y), *srow1 = 0;
 118         float *drow = (float*)(dst.data + dst.step*y);
 119
 120         // vertical part of convolution
 121         for( x = 0; x < width; x++ )
 122         {
 123             row[x*3] = srow0[x]*g0;
 124             row[x*3+1] = row[x*3+2] = 0.f;
 125         }
 126
 127         for( k = 1; k <= n; k++ )
 128         {
 129             g0 = g[k]; g1 = xg[k]; g2 = xxg[k];
 130             srow0 = (float*)(src.data + src.step*std::max(y-k,0));
 131             srow1 = (float*)(src.data + src.step*std::min(y+k,height-1));
 132
 133             for( x = 0; x < width; x++ )
 134             {
 135                 float p = srow0[x] + srow1[x];
 136                 float t0 = row[x*3] + g0*p;
 137                 float t1 = row[x*3+1] + g1*(srow1[x] - srow0[x]);
 138                 float t2 = row[x*3+2] + g2*p;
 139
 140                 row[x*3] = t0;
 141                 row[x*3+1] = t1;
 142                 row[x*3+2] = t2;
 143             }
 144         }
 145
 146         // horizontal part of convolution
 147         for( x = 0; x < n*3; x++ )
 148         {
 149             row[-1-x] = row[2-x];
 150             row[width*3+x] = row[width*3+x-3];
 151         }
 152
 153         for( x = 0; x < width; x++ )
 154         {
 155             g0 = g[0];
 156             // r1 ~ 1, r2 ~ x, r3 ~ y, r4 ~ x^2, r5 ~ y^2, r6 ~ xy
 157             double b1 = row[x*3]*g0, b2 = 0, b3 = row[x*3+1]*g0,
 158                 b4 = 0, b5 = row[x*3+2]*g0, b6 = 0;
 159
 160             for( k = 1; k <= n; k++ )
 161             {
 162                 double tg = row[(x+k)*3] + row[(x-k)*3];
 163                 g0 = g[k];
 164                 b1 += tg*g0;
 165                 b4 += tg*xxg[k];
 166                 b2 += (row[(x+k)*3] - row[(x-k)*3])*xg[k];
 167                 b3 += (row[(x+k)*3+1] + row[(x-k)*3+1])*g0;
 168                 b6 += (row[(x+k)*3+1] - row[(x-k)*3+1])*xg[k];
 169                 b5 += (row[(x+k)*3+2] + row[(x-k)*3+2])*g0;
 170             }
 171
 172             // do not store r1
 173             drow[x*5+1] = (float)(b2*ig11);
 174             drow[x*5] = (float)(b3*ig11);
 175             drow[x*5+3] = (float)(b1*ig03 + b4*ig33);
 176             drow[x*5+2] = (float)(b1*ig03 + b5*ig33);
 177             drow[x*5+4] = (float)(b6*ig55);
 178         }
 179     }
 180
 181     row -= n*3;
 182 }
 183
 184
 185 /*static void
 186 FarnebackPolyExpPyr( const Mat& src0, Vector<Mat>& pyr, int maxlevel, int n, double sigma )
 187 {
 188     Vector<Mat> imgpyr;
 189     buildPyramid( src0, imgpyr, maxlevel );
 190
 191     for( int i = 0; i <= maxlevel; i++ )
 192         FarnebackPolyExp( imgpyr[i], pyr[i], n, sigma );
 193 }*/
 194
 195
 196 static void
 197 FarnebackUpdateMatrices( const Mat& _R0, const Mat& _R1, const Mat& _flow, Mat& _M, int _y0, int _y1 )
 198 {
 199     const int BORDER = 5;
 200     static const float border[BORDER] = {0.14f, 0.14f, 0.4472f, 0.4472f, 0.4472f};
 201
 202     int x, y, width = _flow.cols, height = _flow.rows;
 203     const float* R1 = (float*)_R1.data;
 204     size_t step1 = _R1.step/sizeof(R1[0]);
 205
 206     _M.create(height, width, CV_32FC(5));
 207
 208     for( y = _y0; y < _y1; y++ )
 209     {
 210         const float* flow = (float*)(_flow.data + y*_flow.step);
 211         const float* R0 = (float*)(_R0.data + y*_R0.step);
 212         float* M = (float*)(_M.data + y*_M.step);
 213
 214         for( x = 0; x < width; x++ )
 215         {
 216             float dx = flow[x*2], dy = flow[x*2+1];
 217             float fx = x + dx, fy = y + dy;
 218
 219 #if 1
 220             int x1 = cvFloor(fx), y1 = cvFloor(fy);
 221             const float* ptr = R1 + y1*step1 + x1*5;
 222             float r2, r3, r4, r5, r6;
 223
 224             fx -= x1; fy -= y1;
 225
 226             if( (unsigned)x1 < (unsigned)(width-1) &&
 227                 (unsigned)y1 < (unsigned)(height-1) )
 228             {
 229                 float a00 = (1.f-fx)*(1.f-fy), a01 = fx*(1.f-fy),
 230                       a10 = (1.f-fx)*fy, a11 = fx*fy;
 231
 232                 r2 = a00*ptr[0] + a01*ptr[5] + a10*ptr[step1] + a11*ptr[step1+5];
 233                 r3 = a00*ptr[1] + a01*ptr[6] + a10*ptr[step1+1] + a11*ptr[step1+6];
 234                 r4 = a00*ptr[2] + a01*ptr[7] + a10*ptr[step1+2] + a11*ptr[step1+7];
 235                 r5 = a00*ptr[3] + a01*ptr[8] + a10*ptr[step1+3] + a11*ptr[step1+8];
 236                 r6 = a00*ptr[4] + a01*ptr[9] + a10*ptr[step1+4] + a11*ptr[step1+9];
 237
 238                 r4 = (R0[x*5+2] + r4)*0.5f;
 239                 r5 = (R0[x*5+3] + r5)*0.5f;
 240                 r6 = (R0[x*5+4] + r6)*0.25f;
 241             }
 242 #else
 243             int x1 = cvRound(fx), y1 = cvRound(fy);
 244             const float* ptr = R1 + y1*step1 + x1*5;
 245             float r2, r3, r4, r5, r6;
 246
 247             if( (unsigned)x1 < (unsigned)width &&
 248                 (unsigned)y1 < (unsigned)height )
 249             {
 250                 r2 = ptr[0];
 251                 r3 = ptr[1];
 252                 r4 = (R0[x*5+2] + ptr[2])*0.5f;
 253                 r5 = (R0[x*5+3] + ptr[3])*0.5f;
 254                 r6 = (R0[x*5+4] + ptr[4])*0.25f;
 255             }
 256 #endif
 257             else
 258             {
 259                 r2 = r3 = 0.f;
 260                 r4 = R0[x*5+2];
 261                 r5 = R0[x*5+3];
 262                 r6 = R0[x*5+4]*0.5f;
 263             }
 264
 265             r2 = (R0[x*5] - r2)*0.5f;
 266             r3 = (R0[x*5+1] - r3)*0.5f;
 267
 268             r2 += r4*dy + r6*dx;
 269             r3 += r6*dy + r5*dx;
 270
 271             if( (unsigned)(x - BORDER) >= (unsigned)(width - BORDER*2) ||
 272                 (unsigned)(y - BORDER) >= (unsigned)(height - BORDER*2))
 273             {
 274                 float scale = (x < BORDER ? border[x] : 1.f)*
 275                     (x >= width - BORDER ? border[width - x - 1] : 1.f)*
 276                     (y < BORDER ? border[y] : 1.f)*
 277                     (y >= height - BORDER ? border[height - y - 1] : 1.f);
 278
 279                 r2 *= scale; r3 *= scale; r4 *= scale;
 280                 r5 *= scale; r6 *= scale;
 281             }
 282
 283             M[x*5]   = r4*r4 + r6*r6; // G(1,1)
 284             M[x*5+1] = (r4 + r5)*r6;  // G(1,2)=G(2,1)
 285             M[x*5+2] = r5*r5 + r6*r6; // G(2,2)
 286             M[x*5+3] = r4*r2 + r6*r3; // h(1)
 287             M[x*5+4] = r6*r2 + r5*r3; // h(2)
 288         }
 289     }
 290 }
 291
 292
 293 static void
 294 FarnebackUpdateFlow_Blur( const Mat& _R0, const Mat& _R1,
 295                           Mat& _flow, Mat& _M, int block_size,
 296                           bool update_matrices )
 297 {
 298     int x, y, width = _flow.cols, height = _flow.rows;
 299     int m = block_size/2;
 300     int y0 = 0, y1;
 301     int min_update_stripe = std::max((1 << 10)/width, block_size);
 302     double scale = 1./(block_size*block_size);
 303
 304     AutoBuffer<double> _vsum((width+m*2+2)*5);
 305     double* vsum = _vsum + (m+1)*5;
 306
 307     // init vsum
 308     const float* srow0 = (const float*)_M.data;
 309     for( x = 0; x < width*5; x++ )
 310         vsum[x] = srow0[x]*(m+2);
 311
 312     for( y = 1; y < m; y++ )
 313     {
 314         srow0 = (float*)(_M.data + _M.step*std::min(y,height-1));
 315         for( x = 0; x < width*5; x++ )
 316             vsum[x] += srow0[x];
 317     }
 318
 319     // compute blur(G)*flow=blur(h)
 320     for( y = 0; y < height; y++ )
 321     {
 322         double g11, g12, g22, h1, h2;
 323         float* flow = (float*)(_flow.data + _flow.step*y);
 324
 325         srow0 = (const float*)(_M.data + _M.step*std::max(y-m-1,0));
 326         const float* srow1 = (const float*)(_M.data + _M.step*std::min(y+m,height-1));
 327
 328         // vertical blur
 329         for( x = 0; x < width*5; x++ )
 330             vsum[x] += srow1[x] - srow0[x];
 331
 332         // update borders
 333         for( x = 0; x < (m+1)*5; x++ )
 334         {
 335             vsum[-1-x] = vsum[4-x];
 336             vsum[width*5+x] = vsum[width*5+x-5];
 337         }
 338
 339         // init g** and h*
 340         g11 = vsum[0]*(m+2);
 341         g12 = vsum[1]*(m+2);
 342         g22 = vsum[2]*(m+2);
 343         h1 = vsum[3]*(m+2);
 344         h2 = vsum[4]*(m+2);
 345
 346         for( x = 1; x < m; x++ )
 347         {
 348             g11 += vsum[x*5];
 349             g12 += vsum[x*5+1];
 350             g22 += vsum[x*5+2];
 351             h1 += vsum[x*5+3];
 352             h2 += vsum[x*5+4];
 353         }
 354
 355         // horizontal blur
 356         for( x = 0; x < width; x++ )
 357         {
 358             g11 += vsum[(x+m)*5] - vsum[(x-m)*5 - 5];
 359             g12 += vsum[(x+m)*5 + 1] - vsum[(x-m)*5 - 4];
 360             g22 += vsum[(x+m)*5 + 2] - vsum[(x-m)*5 - 3];
 361             h1 += vsum[(x+m)*5 + 3] - vsum[(x-m)*5 - 2];
 362             h2 += vsum[(x+m)*5 + 4] - vsum[(x-m)*5 - 1];
 363
 364             double g11_ = g11*scale;
 365             double g12_ = g12*scale;
 366             double g22_ = g22*scale;
 367             double h1_ = h1*scale;
 368             double h2_ = h2*scale;
 369
 370             double idet = 1./(g11_*g22_ - g12_*g12_+1e-3);
 371
 372             flow[x*2] = (float)((g11_*h2_-g12_*h1_)*idet);
 373             flow[x*2+1] = (float)((g22_*h1_-g12_*h2_)*idet);
 374         }
 375
 376         y1 = y == height - 1 ? height : y - block_size;
 377         if( update_matrices && (y1 == height || y1 >= y0 + min_update_stripe) )
 378         {
 379             FarnebackUpdateMatrices( _R0, _R1, _flow, _M, y0, y1 );
 380             y0 = y1;
 381         }
 382     }
 383 }
 384
 385
 386 static void
 387 FarnebackUpdateFlow_GaussianBlur( const Mat& _R0, const Mat& _R1,
 388                                   Mat& _flow, Mat& _M, int block_size,
 389                                   bool update_matrices )
 390 {
 391     int x, y, i, width = _flow.cols, height = _flow.rows;
 392     int m = block_size/2;
 393     int y0 = 0, y1;
 394     int min_update_stripe = std::max((1 << 10)/width, block_size);
 395     double sigma = m*0.3, s = 1;
 396
 397     AutoBuffer<float> _vsum((width+m*2+2)*5 + 16), _hsum(width*5 + 16);
 398     AutoBuffer<float, 4096> _kernel((m+1)*5 + 16);
 399     AutoBuffer<float*, 1024> _srow(m*2+1);
 400     float *vsum = alignPtr((float*)_vsum + (m+1)*5, 16), *hsum = alignPtr((float*)_hsum, 16);
 401     float* kernel = (float*)_kernel;
 402     const float** srow = (const float**)&_srow[0];
 403     kernel[0] = (float)s;
 404
 405     for( i = 1; i <= m; i++ )
 406     {
 407         float t = (float)std::exp(-i*i/(2*sigma*sigma) );
 408         kernel[i] = t;
 409         s += t*2;
 410     }
 411
 412     s = 1./s;
 413     for( i = 0; i <= m; i++ )
 414         kernel[i] = (float)(kernel[i]*s);
 415
 416 #if CV_SSE2
 417     float* simd_kernel = alignPtr(kernel + m+1, 16);
 418     for( i = 0; i <= m; i++ )
 419         _mm_store_ps(simd_kernel + i*4, _mm_set1_ps(kernel[i]));
 420 #endif
 421
 422     // compute blur(G)*flow=blur(h)
 423     for( y = 0; y < height; y++ )
 424     {
 425         double g11, g12, g22, h1, h2;
 426         float* flow = (float*)(_flow.data + _flow.step*y);
 427
 428         // vertical blur
 429         for( i = 0; i <= m; i++ )
 430         {
 431             srow[m-i] = (const float*)(_M.data + _M.step*std::max(y-i,0));
 432             srow[m+i] = (const float*)(_M.data + _M.step*std::min(y+i,height-1));
 433         }
 434
 435         x = 0;
 436 #if CV_SSE2
 437         for( ; x <= width*5 - 16; x += 16 )
 438         {
 439             const float *sptr0 = srow[m], *sptr1;
 440             __m128 g4 = _mm_load_ps(simd_kernel);
 441             __m128 s0, s1, s2, s3;
 442             s0 = _mm_mul_ps(_mm_loadu_ps(sptr0 + x), g4);
 443             s1 = _mm_mul_ps(_mm_loadu_ps(sptr0 + x + 4), g4);
 444             s2 = _mm_mul_ps(_mm_loadu_ps(sptr0 + x + 8), g4);
 445             s3 = _mm_mul_ps(_mm_loadu_ps(sptr0 + x + 12), g4);
 446
 447             for( i = 1; i <= m; i++ )
 448             {
 449                 __m128 x0, x1;
 450                 sptr0 = srow[m+i], sptr1 = srow[m-i];
 451                 g4 = _mm_load_ps(simd_kernel + i*4);
 452                 x0 = _mm_add_ps(_mm_loadu_ps(sptr0 + x), _mm_loadu_ps(sptr1 + x));
 453                 x1 = _mm_add_ps(_mm_loadu_ps(sptr0 + x + 4), _mm_loadu_ps(sptr1 + x + 4));
 454                 s0 = _mm_add_ps(s0, _mm_mul_ps(x0, g4));
 455                 s1 = _mm_add_ps(s1, _mm_mul_ps(x1, g4));
 456                 x0 = _mm_add_ps(_mm_loadu_ps(sptr0 + x + 8), _mm_loadu_ps(sptr1 + x + 8));
 457                 x1 = _mm_add_ps(_mm_loadu_ps(sptr0 + x + 12), _mm_loadu_ps(sptr1 + x + 12));
 458                 s2 = _mm_add_ps(s2, _mm_mul_ps(x0, g4));
 459                 s3 = _mm_add_ps(s3, _mm_mul_ps(x1, g4));
 460             }
 461
 462             _mm_store_ps(vsum + x, s0);
 463             _mm_store_ps(vsum + x + 4, s1);
 464             _mm_store_ps(vsum + x + 8, s2);
 465             _mm_store_ps(vsum + x + 12, s3);
 466         }
 467
 468         for( ; x <= width*5 - 4; x += 4 )
 469         {
 470             const float *sptr0 = srow[m], *sptr1;
 471             __m128 g4 = _mm_load_ps(simd_kernel);
 472             __m128 s0 = _mm_mul_ps(_mm_loadu_ps(sptr0 + x), g4);
 473
 474             for( i = 1; i <= m; i++ )
 475             {
 476                 sptr0 = srow[m+i], sptr1 = srow[m-i];
 477                 g4 = _mm_load_ps(simd_kernel + i*4);
 478                 __m128 x0 = _mm_add_ps(_mm_loadu_ps(sptr0 + x), _mm_loadu_ps(sptr1 + x));
 479                 s0 = _mm_add_ps(s0, _mm_mul_ps(x0, g4));
 480             }
 481             _mm_store_ps(vsum + x, s0);
 482         }
 483 #endif
 484         for( ; x < width*5; x++ )
 485         {
 486             float s0 = srow[m][x]*kernel[0];
 487             for( i = 1; i <= m; i++ )
 488                 s0 += (srow[m+i][x] + srow[m-i][x])*kernel[i];
 489             vsum[x] = s0;
 490         }
 491
 492         // update borders
 493         for( x = 0; x < m*5; x++ )
 494         {
 495             vsum[-1-x] = vsum[4-x];
 496             vsum[width*5+x] = vsum[width*5+x-5];
 497         }
 498
 499         // horizontal blur
 500         x = 0;
 501 #if CV_SSE2
 502         for( ; x <= width*5 - 8; x += 8 )
 503         {
 504             __m128 g4 = _mm_load_ps(simd_kernel);
 505             __m128 s0 = _mm_mul_ps(_mm_loadu_ps(vsum + x), g4);
 506             __m128 s1 = _mm_mul_ps(_mm_loadu_ps(vsum + x + 4), g4);
 507
 508             for( i = 1; i <= m; i++ )
 509             {
 510                 g4 = _mm_load_ps(simd_kernel + i*4);
 511                 __m128 x0 = _mm_add_ps(_mm_loadu_ps(vsum + x - i*5),
 512                                        _mm_loadu_ps(vsum + x + i*5));
 513                 __m128 x1 = _mm_add_ps(_mm_loadu_ps(vsum + x - i*5 + 4),
 514                                        _mm_loadu_ps(vsum + x + i*5 + 4));
 515                 s0 = _mm_add_ps(s0, _mm_mul_ps(x0, g4));
 516                 s1 = _mm_add_ps(s1, _mm_mul_ps(x1, g4));
 517             }
 518
 519             _mm_store_ps(hsum + x, s0);
 520             _mm_store_ps(hsum + x + 4, s1);
 521         }
 522 #endif
 523         for( ; x < width*5; x++ )
 524         {
 525             float s = vsum[x]*kernel[0];
 526             for( i = 1; i <= m; i++ )
 527                 s += kernel[i]*(vsum[x - i*5] + vsum[x + i*5]);
 528             hsum[x] = s;
 529         }
 530
 531         for( x = 0; x < width; x++ )
 532         {
 533             g11 = hsum[x*5];
 534             g12 = hsum[x*5+1];
 535             g22 = hsum[x*5+2];
 536             h1 = hsum[x*5+3];
 537             h2 = hsum[x*5+4];
 538
 539             double idet = 1./(g11*g22 - g12*g12 + 1e-3);
 540
 541             flow[x*2] = (float)((g11*h2-g12*h1)*idet);
 542             flow[x*2+1] = (float)((g22*h1-g12*h2)*idet);
 543         }
 544
 545         y1 = y == height - 1 ? height : y - block_size;
 546         if( update_matrices && (y1 == height || y1 >= y0 + min_update_stripe) )
 547         {
 548             FarnebackUpdateMatrices( _R0, _R1, _flow, _M, y0, y1 );
 549             y0 = y1;
 550         }
 551     }
 552 }
 553
 554
 555 void calcOpticalFlowFarneback( const Mat& prev0, const Mat& next0,
 556                                Mat& flow0, double pyr_scale, int levels, int winsize,
 557                                int iterations, int poly_n, double poly_sigma, int flags )
 558 {
 559     const int min_size = 32;
 560     const Mat* img[2] = { &prev0, &next0 };
 561     Mat fimg;
 562
 563     int i, k;
 564     double scale;
 565     Mat prevFlow, flow;
 566
 567     CV_Assert( prev0.size() == next0.size() && prev0.channels() == next0.channels() &&
 568         prev0.channels() == 1 );
 569     flow0.create( prev0.size(), CV_32FC2 );
 570
 571     for( k = 0, scale = 1; k < levels; k++ )
 572     {
 573         scale *= pyr_scale;
 574         if( prev0.cols*scale < min_size || prev0.rows*scale < min_size )
 575             break;
 576     }
 577
 578     levels = k;
 579
 580     for( k = levels; k >= 0; k-- )
 581     {
 582         for( i = 0, scale = 1; i < k; i++ )
 583             scale *= pyr_scale;
 584
 585         double sigma = (1./scale-1)*0.5;
 586         int smooth_sz = cvRound(sigma*5)|1;
 587         smooth_sz = std::max(smooth_sz, 3);
 588
 589         int width = cvRound(prev0.cols*scale);
 590         int height = cvRound(prev0.rows*scale);
 591
 592         if( k > 0 )
 593             flow.create( height, width, CV_32FC2 );
 594         else
 595             flow = flow0;
 596
 597         if( !prevFlow.data )
 598         {
 599             if( flags & OPTFLOW_USE_INITIAL_FLOW )
 600             {
 601                 resize( flow0, flow, Size(width, height), 0, 0, INTER_AREA );
 602                 flow *= scale;
 603             }
 604             else
 605                 flow = Mat::zeros( height, width, CV_32FC2 );
 606         }
 607         else
 608         {
 609             resize( prevFlow, flow, Size(width, height), 0, 0, INTER_LINEAR );
 610             flow *= 1./pyr_scale;
 611         }
 612
 613         Mat R[2], I, M;
 614         for( i = 0; i < 2; i++ )
 615         {
 616             img[i]->convertTo(fimg, CV_32F);
 617             GaussianBlur(fimg, fimg, Size(smooth_sz, smooth_sz), sigma, sigma);
 618             resize( fimg, I, Size(width, height), CV_INTER_LINEAR );
 619             FarnebackPolyExp( I, R[i], poly_n, poly_sigma );
 620         }
 621
 622         FarnebackUpdateMatrices( R[0], R[1], flow, M, 0, flow.rows );
 623
 624         for( i = 0; i < iterations; i++ )
 625         {
 626             if( flags & OPTFLOW_FARNEBACK_GAUSSIAN )
 627                 FarnebackUpdateFlow_GaussianBlur( R[0], R[1], flow, M, winsize, i < iterations - 1 );
 628             else
 629                 FarnebackUpdateFlow_Blur( R[0], R[1], flow, M, winsize, i < iterations - 1 );
 630         }
 631
 632         prevFlow = flow;
 633     }
 634 }
 635
 636 }