modules/ocl/src/optical_flow_farneback.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) 2010-2012, Multicoreware, Inc., all rights reserved.
  14 // Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // @Authors
  18 //      Sen Liu, swjtuls1987@126.com
  19 //
  20 // Redistribution and use in source and binary forms, with or without modification,
  21 // are permitted provided that the following conditions are met:
  22 //
  23 //   * Redistribution's of source code must retain the above copyright notice,
  24 //     this list of conditions and the following disclaimer.
  25 //
  26 //   * Redistribution's in binary form must reproduce the above copyright notice,
  27 //     this list of conditions and the following disclaimer in the documentation
  28 //     and/or other oclMaterials provided with the distribution.
  29 //
  30 //   * The name of the copyright holders may not be used to endorse or promote products
  31 //     derived from this software without specific prior written permission.
  32 //
  33 // This software is provided by the copyright holders and contributors "as is" and
  34 // any express or implied warranties, including, but not limited to, the implied
  35 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  36 // In no event shall the Intel Corporation or contributors be liable for any direct,
  37 // indirect, incidental, special, exemplary, or consequential damages
  38 // (including, but not limited to, procurement of substitute goods or services;
  39 // loss of use, data, or profits; or business interruption) however caused
  40 // and on any theory of liability, whether in contract, strict liability,
  41 // or tort (including negligence or otherwise) arising in any way out of
  42 // the use of this software, even if advised of the possibility of such damage.
  43 //
  44 //M*/
  45
  46
  47 #include "precomp.hpp"
  48 #include "opencv2/video/tracking.hpp"
  49
  50 using namespace std;
  51 using namespace cv;
  52 using namespace cv::ocl;
  53
  54 #define MIN_SIZE 32
  55
  56 namespace cv
  57 {
  58 namespace ocl
  59 {
  60 ///////////////////////////OpenCL kernel strings///////////////////////////
  61 extern const char *optical_flow_farneback;
  62 }
  63 }
  64
  65 namespace cv {
  66 namespace ocl {
  67 namespace optflow_farneback
  68 {
  69 oclMat g;
  70 oclMat xg;
  71 oclMat xxg;
  72 oclMat gKer;
  73
  74 float ig[4];
  75
  76 inline int divUp(int total, int grain)
  77 {
  78     return (total + grain - 1) / grain;
  79 }
  80
  81 inline void setGaussianBlurKernel(const float *c_gKer, int ksizeHalf)
  82 {
  83     cv::Mat t_gKer(1, ksizeHalf + 1, CV_32FC1, const_cast<float *>(c_gKer));
  84     gKer.upload(t_gKer);
  85 }
  86
  87 static void gaussianBlurOcl(const oclMat &src, int ksizeHalf, oclMat &dst)
  88 {
  89     string kernelName("gaussianBlur");
  90     size_t localThreads[3] = { 256, 1, 1 };
  91     size_t globalThreads[3] = { divUp(src.cols, localThreads[0]) * localThreads[0], src.rows, 1 };
  92     int smem_size = (localThreads[0] + 2*ksizeHalf) * sizeof(float);
  93
  94     CV_Assert(dst.size() == src.size());
  95     std::vector< std::pair<size_t, const void *> > args;
  96     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&dst.data));
  97     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&src.data));
  98     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&gKer.data));
  99     args.push_back(std::make_pair(smem_size, (void *)NULL));
 100     args.push_back(std::make_pair(sizeof(cl_int), (void *)&dst.rows));
 101     args.push_back(std::make_pair(sizeof(cl_int), (void *)&dst.cols));
 102     args.push_back(std::make_pair(sizeof(cl_int), (void *)&dst.step));
 103     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.step));
 104     args.push_back(std::make_pair(sizeof(cl_int), (void *)&ksizeHalf));
 105
 106     openCLExecuteKernel(Context::getContext(), &optical_flow_farneback, kernelName,
 107                         globalThreads, localThreads, args, -1, -1);
 108 }
 109
 110 static void polynomialExpansionOcl(const oclMat &src, int polyN, oclMat &dst)
 111 {
 112     string kernelName("polynomialExpansion");
 113     size_t localThreads[3] = { 256, 1, 1 };
 114     size_t globalThreads[3] = { divUp(src.cols, localThreads[0] - 2*polyN) * localThreads[0], src.rows, 1 };
 115     int smem_size = 3 * localThreads[0] * sizeof(float);
 116
 117     std::vector< std::pair<size_t, const void *> > args;
 118     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&dst.data));
 119     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&src.data));
 120     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&g.data));
 121     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&xg.data));
 122     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&xxg.data));
 123     args.push_back(std::make_pair(smem_size, (void *)NULL));
 124     args.push_back(std::make_pair(sizeof(cl_float4), (void *)&ig));
 125     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.rows));
 126     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.cols));
 127     args.push_back(std::make_pair(sizeof(cl_int), (void *)&dst.step));
 128     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.step));
 129
 130     char opt [128];
 131     sprintf(opt, "-D polyN=%d", polyN);
 132
 133     openCLExecuteKernel(Context::getContext(), &optical_flow_farneback, kernelName,
 134                         globalThreads, localThreads, args, -1, -1, opt);
 135 }
 136
 137 static void updateMatricesOcl(const oclMat &flowx, const oclMat &flowy, const oclMat &R0, const oclMat &R1, oclMat &M)
 138 {
 139     string kernelName("updateMatrices");
 140     size_t localThreads[3] = { 32, 8, 1 };
 141     size_t globalThreads[3] = { divUp(flowx.cols, localThreads[0]) * localThreads[0],
 142                                 divUp(flowx.rows, localThreads[1]) * localThreads[1],
 143                                 1
 144                               };
 145
 146     std::vector< std::pair<size_t, const void *> > args;
 147     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&M.data));
 148     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&flowx.data));
 149     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&flowy.data));
 150     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&R0.data));
 151     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&R1.data));
 152     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowx.rows));
 153     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowx.cols));
 154     args.push_back(std::make_pair(sizeof(cl_int), (void *)&M.step));
 155     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowx.step));
 156     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowy.step));
 157     args.push_back(std::make_pair(sizeof(cl_int), (void *)&R0.step));
 158     args.push_back(std::make_pair(sizeof(cl_int), (void *)&R1.step));
 159
 160     openCLExecuteKernel(Context::getContext(), &optical_flow_farneback, kernelName,
 161                         globalThreads, localThreads, args, -1, -1);
 162 }
 163
 164 static void boxFilter5Ocl(const oclMat &src, int ksizeHalf, oclMat &dst)
 165 {
 166     string kernelName("boxFilter5");
 167     int height = src.rows / 5;
 168     size_t localThreads[3] = { 256, 1, 1 };
 169     size_t globalThreads[3] = { divUp(src.cols, localThreads[0]) * localThreads[0], height, 1 };
 170     int smem_size = (localThreads[0] + 2*ksizeHalf) * 5 * sizeof(float);
 171
 172     std::vector< std::pair<size_t, const void *> > args;
 173     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&dst.data));
 174     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&src.data));
 175     args.push_back(std::make_pair(smem_size, (void *)NULL));
 176     args.push_back(std::make_pair(sizeof(cl_int), (void *)&height));
 177     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.cols));
 178     args.push_back(std::make_pair(sizeof(cl_int), (void *)&dst.step));
 179     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.step));
 180     args.push_back(std::make_pair(sizeof(cl_int), (void *)&ksizeHalf));
 181
 182     openCLExecuteKernel(Context::getContext(), &optical_flow_farneback, kernelName,
 183                         globalThreads, localThreads, args, -1, -1);
 184 }
 185
 186 static void updateFlowOcl(const oclMat &M, oclMat &flowx, oclMat &flowy)
 187 {
 188     string kernelName("updateFlow");
 189     int cols = divUp(flowx.cols, 4);
 190     size_t localThreads[3] = { 32, 8, 1 };
 191     size_t globalThreads[3] = { divUp(cols, localThreads[0]) * localThreads[0],
 192                                 divUp(flowx.rows, localThreads[1]) * localThreads[0],
 193                                 1
 194                               };
 195
 196     std::vector< std::pair<size_t, const void *> > args;
 197     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&flowx.data));
 198     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&flowy.data));
 199     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&M.data));
 200     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowx.rows));
 201     args.push_back(std::make_pair(sizeof(cl_int), (void *)&cols));
 202     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowx.step));
 203     args.push_back(std::make_pair(sizeof(cl_int), (void *)&flowy.step));
 204     args.push_back(std::make_pair(sizeof(cl_int), (void *)&M.step));
 205
 206     openCLExecuteKernel(Context::getContext(), &optical_flow_farneback, kernelName,
 207                         globalThreads, localThreads, args, -1, -1);
 208 }
 209
 210 static void gaussianBlur5Ocl(const oclMat &src, int ksizeHalf, oclMat &dst)
 211 {
 212     string kernelName("gaussianBlur5");
 213     int height = src.rows / 5;
 214     int width = src.cols;
 215     size_t localThreads[3] = { 256, 1, 1 };
 216     size_t globalThreads[3] = { divUp(width, localThreads[0]) * localThreads[0], height, 1 };
 217     int smem_size = (localThreads[0] + 2*ksizeHalf) * 5 * sizeof(float);
 218
 219     std::vector< std::pair<size_t, const void *> > args;
 220     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&dst.data));
 221     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&src.data));
 222     args.push_back(std::make_pair(sizeof(cl_mem), (void *)&gKer.data));
 223     args.push_back(std::make_pair(smem_size, (void *)NULL));
 224     args.push_back(std::make_pair(sizeof(cl_int), (void *)&height));
 225     args.push_back(std::make_pair(sizeof(cl_int), (void *)&width));
 226     args.push_back(std::make_pair(sizeof(cl_int), (void *)&dst.step));
 227     args.push_back(std::make_pair(sizeof(cl_int), (void *)&src.step));
 228     args.push_back(std::make_pair(sizeof(cl_int), (void *)&ksizeHalf));
 229
 230     openCLExecuteKernel(Context::getContext(), &optical_flow_farneback, kernelName,
 231                         globalThreads, localThreads, args, -1, -1);
 232 }
 233 }
 234 }
 235 } // namespace cv { namespace ocl { namespace optflow_farneback
 236
 237 static oclMat allocMatFromBuf(int rows, int cols, int type, oclMat &mat)
 238 {
 239     if (!mat.empty() && mat.type() == type && mat.rows >= rows && mat.cols >= cols)
 240         return mat(Rect(0, 0, cols, rows));
 241     return mat = oclMat(rows, cols, type);
 242 }
 243
 244 void cv::ocl::FarnebackOpticalFlow::prepareGaussian(
 245     int n, double sigma, float *g, float *xg, float *xxg,
 246     double &ig11, double &ig03, double &ig33, double &ig55)
 247 {
 248     double s = 0.;
 249     for (int x = -n; x <= n; x++)
 250     {
 251         g[x] = (float)std::exp(-x*x/(2*sigma*sigma));
 252         s += g[x];
 253     }
 254
 255     s = 1./s;
 256     for (int x = -n; x <= n; x++)
 257     {
 258         g[x] = (float)(g[x]*s);
 259         xg[x] = (float)(x*g[x]);
 260         xxg[x] = (float)(x*x*g[x]);
 261     }
 262
 263     Mat_<double> G(6, 6);
 264     G.setTo(0);
 265
 266     for (int y = -n; y <= n; y++)
 267     {
 268         for (int x = -n; x <= n; x++)
 269         {
 270             G(0,0) += g[y]*g[x];
 271             G(1,1) += g[y]*g[x]*x*x;
 272             G(3,3) += g[y]*g[x]*x*x*x*x;
 273             G(5,5) += g[y]*g[x]*x*x*y*y;
 274         }
 275     }
 276
 277     //G[0][0] = 1.;
 278     G(2,2) = G(0,3) = G(0,4) = G(3,0) = G(4,0) = G(1,1);
 279     G(4,4) = G(3,3);
 280     G(3,4) = G(4,3) = G(5,5);
 281
 282     // invG:
 283     // [ x        e  e    ]
 284     // [    y             ]
 285     // [       y          ]
 286     // [ e        z       ]
 287     // [ e           z    ]
 288     // [                u ]
 289     Mat_<double> invG = G.inv(DECOMP_CHOLESKY);
 290
 291     ig11 = invG(1,1);
 292     ig03 = invG(0,3);
 293     ig33 = invG(3,3);
 294     ig55 = invG(5,5);
 295 }
 296
 297 void cv::ocl::FarnebackOpticalFlow::setPolynomialExpansionConsts(int n, double sigma)
 298 {
 299     vector<float> buf(n*6 + 3);
 300     float* g = &buf[0] + n;
 301     float* xg = g + n*2 + 1;
 302     float* xxg = xg + n*2 + 1;
 303
 304     if (sigma < FLT_EPSILON)
 305         sigma = n*0.3;
 306
 307     double ig11, ig03, ig33, ig55;
 308     prepareGaussian(n, sigma, g, xg, xxg, ig11, ig03, ig33, ig55);
 309
 310     cv::Mat t_g(1, n + 1, CV_32FC1, g);
 311     cv::Mat t_xg(1, n + 1, CV_32FC1, xg);
 312     cv::Mat t_xxg(1, n + 1, CV_32FC1, xxg);
 313
 314     optflow_farneback::g.upload(t_g);
 315     optflow_farneback::xg.upload(t_xg);
 316     optflow_farneback::xxg.upload(t_xxg);
 317
 318     optflow_farneback::ig[0] = static_cast<float>(ig11);
 319     optflow_farneback::ig[1] = static_cast<float>(ig03);
 320     optflow_farneback::ig[2] = static_cast<float>(ig33);
 321     optflow_farneback::ig[3] = static_cast<float>(ig55);
 322 }
 323
 324 void cv::ocl::FarnebackOpticalFlow::updateFlow_boxFilter(
 325     const oclMat& R0, const oclMat& R1, oclMat& flowx, oclMat &flowy,
 326     oclMat& M, oclMat &bufM, int blockSize, bool updateMatrices)
 327 {
 328     optflow_farneback::boxFilter5Ocl(M, blockSize/2, bufM);
 329
 330     swap(M, bufM);
 331
 332     finish();
 333
 334     optflow_farneback::updateFlowOcl(M, flowx, flowy);
 335
 336     if (updateMatrices)
 337         optflow_farneback::updateMatricesOcl(flowx, flowy, R0, R1, M);
 338 }
 339
 340
 341 void cv::ocl::FarnebackOpticalFlow::updateFlow_gaussianBlur(
 342     const oclMat& R0, const oclMat& R1, oclMat& flowx, oclMat& flowy,
 343     oclMat& M, oclMat &bufM, int blockSize, bool updateMatrices)
 344 {
 345     optflow_farneback::gaussianBlur5Ocl(M, blockSize/2, bufM);
 346
 347     swap(M, bufM);
 348
 349     optflow_farneback::updateFlowOcl(M, flowx, flowy);
 350
 351     if (updateMatrices)
 352         optflow_farneback::updateMatricesOcl(flowx, flowy, R0, R1, M);
 353 }
 354
 355
 356 void cv::ocl::FarnebackOpticalFlow::operator ()(
 357     const oclMat &frame0, const oclMat &frame1, oclMat &flowx, oclMat &flowy)
 358 {
 359     CV_Assert(frame0.channels() == 1 && frame1.channels() == 1);
 360     CV_Assert(frame0.size() == frame1.size());
 361     CV_Assert(polyN == 5 || polyN == 7);
 362     CV_Assert(!fastPyramids || std::abs(pyrScale - 0.5) < 1e-6);
 363
 364     Size size = frame0.size();
 365     oclMat prevFlowX, prevFlowY, curFlowX, curFlowY;
 366
 367     flowx.create(size, CV_32F);
 368     flowy.create(size, CV_32F);
 369     oclMat flowx0 = flowx;
 370     oclMat flowy0 = flowy;
 371
 372     // Crop unnecessary levels
 373     double scale = 1;
 374     int numLevelsCropped = 0;
 375     for (; numLevelsCropped < numLevels; numLevelsCropped++)
 376     {
 377         scale *= pyrScale;
 378         if (size.width*scale < MIN_SIZE || size.height*scale < MIN_SIZE)
 379             break;
 380     }
 381
 382     frame0.convertTo(frames_[0], CV_32F);
 383     frame1.convertTo(frames_[1], CV_32F);
 384
 385     if (fastPyramids)
 386     {
 387         // Build Gaussian pyramids using pyrDown()
 388         pyramid0_.resize(numLevelsCropped + 1);
 389         pyramid1_.resize(numLevelsCropped + 1);
 390         pyramid0_[0] = frames_[0];
 391         pyramid1_[0] = frames_[1];
 392         for (int i = 1; i <= numLevelsCropped; ++i)
 393         {
 394             pyrDown(pyramid0_[i - 1], pyramid0_[i]);
 395             pyrDown(pyramid1_[i - 1], pyramid1_[i]);
 396         }
 397     }
 398
 399     setPolynomialExpansionConsts(polyN, polySigma);
 400
 401     for (int k = numLevelsCropped; k >= 0; k--)
 402     {
 403         scale = 1;
 404         for (int i = 0; i < k; i++)
 405             scale *= pyrScale;
 406
 407         double sigma = (1./scale - 1) * 0.5;
 408         int smoothSize = cvRound(sigma*5) | 1;
 409         smoothSize = std::max(smoothSize, 3);
 410
 411         int width = cvRound(size.width*scale);
 412         int height = cvRound(size.height*scale);
 413
 414         if (fastPyramids)
 415         {
 416             width = pyramid0_[k].cols;
 417             height = pyramid0_[k].rows;
 418         }
 419
 420         if (k > 0)
 421         {
 422             curFlowX.create(height, width, CV_32F);
 423             curFlowY.create(height, width, CV_32F);
 424         }
 425         else
 426         {
 427             curFlowX = flowx0;
 428             curFlowY = flowy0;
 429         }
 430
 431         if (!prevFlowX.data)
 432         {
 433             if (flags & cv::OPTFLOW_USE_INITIAL_FLOW)
 434             {
 435                 resize(flowx0, curFlowX, Size(width, height), 0, 0, INTER_LINEAR);
 436                 resize(flowy0, curFlowY, Size(width, height), 0, 0, INTER_LINEAR);
 437                 multiply(scale, curFlowX, curFlowX);
 438                 multiply(scale, curFlowY, curFlowY);
 439             }
 440             else
 441             {
 442                 curFlowX.setTo(0);
 443                 curFlowY.setTo(0);
 444             }
 445         }
 446         else
 447         {
 448             resize(prevFlowX, curFlowX, Size(width, height), 0, 0, INTER_LINEAR);
 449             resize(prevFlowY, curFlowY, Size(width, height), 0, 0, INTER_LINEAR);
 450             multiply(1./pyrScale, curFlowX, curFlowX);
 451             multiply(1./pyrScale, curFlowY, curFlowY);
 452         }
 453
 454         oclMat M = allocMatFromBuf(5*height, width, CV_32F, M_);
 455         oclMat bufM = allocMatFromBuf(5*height, width, CV_32F, bufM_);
 456         oclMat R[2] =
 457         {
 458             allocMatFromBuf(5*height, width, CV_32F, R_[0]),
 459             allocMatFromBuf(5*height, width, CV_32F, R_[1])
 460         };
 461
 462         if (fastPyramids)
 463         {
 464             optflow_farneback::polynomialExpansionOcl(pyramid0_[k], polyN, R[0]);
 465             optflow_farneback::polynomialExpansionOcl(pyramid1_[k], polyN, R[1]);
 466         }
 467         else
 468         {
 469             oclMat blurredFrame[2] =
 470             {
 471                 allocMatFromBuf(size.height, size.width, CV_32F, blurredFrame_[0]),
 472                 allocMatFromBuf(size.height, size.width, CV_32F, blurredFrame_[1])
 473             };
 474             oclMat pyrLevel[2] =
 475             {
 476                 allocMatFromBuf(height, width, CV_32F, pyrLevel_[0]),
 477                 allocMatFromBuf(height, width, CV_32F, pyrLevel_[1])
 478             };
 479
 480             Mat g = getGaussianKernel(smoothSize, sigma, CV_32F);
 481             optflow_farneback::setGaussianBlurKernel(g.ptr<float>(smoothSize/2), smoothSize/2);
 482
 483             for (int i = 0; i < 2; i++)
 484             {
 485                 optflow_farneback::gaussianBlurOcl(frames_[i], smoothSize/2, blurredFrame[i]);
 486                 resize(blurredFrame[i], pyrLevel[i], Size(width, height), INTER_LINEAR);
 487                 optflow_farneback::polynomialExpansionOcl(pyrLevel[i], polyN, R[i]);
 488             }
 489         }
 490
 491         optflow_farneback::updateMatricesOcl(curFlowX, curFlowY, R[0], R[1], M);
 492
 493         if (flags & OPTFLOW_FARNEBACK_GAUSSIAN)
 494         {
 495             Mat g = getGaussianKernel(winSize, winSize/2*0.3f, CV_32F);
 496             optflow_farneback::setGaussianBlurKernel(g.ptr<float>(winSize/2), winSize/2);
 497         }
 498         for (int i = 0; i < numIters; i++)
 499         {
 500             if (flags & OPTFLOW_FARNEBACK_GAUSSIAN)
 501                 updateFlow_gaussianBlur(R[0], R[1], curFlowX, curFlowY, M, bufM, winSize, i < numIters-1);
 502             else
 503                 updateFlow_boxFilter(R[0], R[1], curFlowX, curFlowY, M, bufM, winSize, i < numIters-1);
 504         }
 505
 506         prevFlowX = curFlowX;
 507         prevFlowY = curFlowY;
 508     }
 509
 510     flowx = curFlowX;
 511     flowy = curFlowY;
 512 }