Merge pull request #1263 from abidrahmank:pyCLAHE_24
[profile/ivi/opencv.git] / 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 cv::ocl::FarnebackOpticalFlow::FarnebackOpticalFlow()
245 {
246     numLevels = 5;
247     pyrScale = 0.5;
248     fastPyramids = false;
249     winSize = 13;
250     numIters = 10;
251     polyN = 5;
252     polySigma = 1.1;
253     flags = 0;
254 }
255
256 void cv::ocl::FarnebackOpticalFlow::releaseMemory()
257 {
258     frames_[0].release();
259     frames_[1].release();
260     pyrLevel_[0].release();
261     pyrLevel_[1].release();
262     M_.release();
263     bufM_.release();
264     R_[0].release();
265     R_[1].release();
266     blurredFrame_[0].release();
267     blurredFrame_[1].release();
268     pyramid0_.clear();
269     pyramid1_.clear();
270 }
271
272 void cv::ocl::FarnebackOpticalFlow::prepareGaussian(
273     int n, double sigma, float *g, float *xg, float *xxg,
274     double &ig11, double &ig03, double &ig33, double &ig55)
275 {
276     double s = 0.;
277     for (int x = -n; x <= n; x++)
278     {
279         g[x] = (float)std::exp(-x*x/(2*sigma*sigma));
280         s += g[x];
281     }
282
283     s = 1./s;
284     for (int x = -n; x <= n; x++)
285     {
286         g[x] = (float)(g[x]*s);
287         xg[x] = (float)(x*g[x]);
288         xxg[x] = (float)(x*x*g[x]);
289     }
290
291     Mat_<double> G(6, 6);
292     G.setTo(0);
293
294     for (int y = -n; y <= n; y++)
295     {
296         for (int x = -n; x <= n; x++)
297         {
298             G(0,0) += g[y]*g[x];
299             G(1,1) += g[y]*g[x]*x*x;
300             G(3,3) += g[y]*g[x]*x*x*x*x;
301             G(5,5) += g[y]*g[x]*x*x*y*y;
302         }
303     }
304
305     //G[0][0] = 1.;
306     G(2,2) = G(0,3) = G(0,4) = G(3,0) = G(4,0) = G(1,1);
307     G(4,4) = G(3,3);
308     G(3,4) = G(4,3) = G(5,5);
309
310     // invG:
311     // [ x        e  e    ]
312     // [    y             ]
313     // [       y          ]
314     // [ e        z       ]
315     // [ e           z    ]
316     // [                u ]
317     Mat_<double> invG = G.inv(DECOMP_CHOLESKY);
318
319     ig11 = invG(1,1);
320     ig03 = invG(0,3);
321     ig33 = invG(3,3);
322     ig55 = invG(5,5);
323 }
324
325 void cv::ocl::FarnebackOpticalFlow::setPolynomialExpansionConsts(int n, double sigma)
326 {
327     vector<float> buf(n*6 + 3);
328     float* g = &buf[0] + n;
329     float* xg = g + n*2 + 1;
330     float* xxg = xg + n*2 + 1;
331
332     if (sigma < FLT_EPSILON)
333         sigma = n*0.3;
334
335     double ig11, ig03, ig33, ig55;
336     prepareGaussian(n, sigma, g, xg, xxg, ig11, ig03, ig33, ig55);
337
338     cv::Mat t_g(1, n + 1, CV_32FC1, g);
339     cv::Mat t_xg(1, n + 1, CV_32FC1, xg);
340     cv::Mat t_xxg(1, n + 1, CV_32FC1, xxg);
341
342     optflow_farneback::g.upload(t_g);
343     optflow_farneback::xg.upload(t_xg);
344     optflow_farneback::xxg.upload(t_xxg);
345
346     optflow_farneback::ig[0] = static_cast<float>(ig11);
347     optflow_farneback::ig[1] = static_cast<float>(ig03);
348     optflow_farneback::ig[2] = static_cast<float>(ig33);
349     optflow_farneback::ig[3] = static_cast<float>(ig55);
350 }
351
352 void cv::ocl::FarnebackOpticalFlow::updateFlow_boxFilter(
353     const oclMat& R0, const oclMat& R1, oclMat& flowx, oclMat &flowy,
354     oclMat& M, oclMat &bufM, int blockSize, bool updateMatrices)
355 {
356     optflow_farneback::boxFilter5Ocl(M, blockSize/2, bufM);
357
358     swap(M, bufM);
359
360     finish();
361
362     optflow_farneback::updateFlowOcl(M, flowx, flowy);
363
364     if (updateMatrices)
365         optflow_farneback::updateMatricesOcl(flowx, flowy, R0, R1, M);
366 }
367
368
369 void cv::ocl::FarnebackOpticalFlow::updateFlow_gaussianBlur(
370     const oclMat& R0, const oclMat& R1, oclMat& flowx, oclMat& flowy,
371     oclMat& M, oclMat &bufM, int blockSize, bool updateMatrices)
372 {
373     optflow_farneback::gaussianBlur5Ocl(M, blockSize/2, bufM);
374
375     swap(M, bufM);
376
377     optflow_farneback::updateFlowOcl(M, flowx, flowy);
378
379     if (updateMatrices)
380         optflow_farneback::updateMatricesOcl(flowx, flowy, R0, R1, M);
381 }
382
383
384 void cv::ocl::FarnebackOpticalFlow::operator ()(
385     const oclMat &frame0, const oclMat &frame1, oclMat &flowx, oclMat &flowy)
386 {
387     CV_Assert(frame0.channels() == 1 && frame1.channels() == 1);
388     CV_Assert(frame0.size() == frame1.size());
389     CV_Assert(polyN == 5 || polyN == 7);
390     CV_Assert(!fastPyramids || std::abs(pyrScale - 0.5) < 1e-6);
391
392     Size size = frame0.size();
393     oclMat prevFlowX, prevFlowY, curFlowX, curFlowY;
394
395     flowx.create(size, CV_32F);
396     flowy.create(size, CV_32F);
397     oclMat flowx0 = flowx;
398     oclMat flowy0 = flowy;
399
400     // Crop unnecessary levels
401     double scale = 1;
402     int numLevelsCropped = 0;
403     for (; numLevelsCropped < numLevels; numLevelsCropped++)
404     {
405         scale *= pyrScale;
406         if (size.width*scale < MIN_SIZE || size.height*scale < MIN_SIZE)
407             break;
408     }
409
410     frame0.convertTo(frames_[0], CV_32F);
411     frame1.convertTo(frames_[1], CV_32F);
412
413     if (fastPyramids)
414     {
415         // Build Gaussian pyramids using pyrDown()
416         pyramid0_.resize(numLevelsCropped + 1);
417         pyramid1_.resize(numLevelsCropped + 1);
418         pyramid0_[0] = frames_[0];
419         pyramid1_[0] = frames_[1];
420         for (int i = 1; i <= numLevelsCropped; ++i)
421         {
422             pyrDown(pyramid0_[i - 1], pyramid0_[i]);
423             pyrDown(pyramid1_[i - 1], pyramid1_[i]);
424         }
425     }
426
427     setPolynomialExpansionConsts(polyN, polySigma);
428
429     for (int k = numLevelsCropped; k >= 0; k--)
430     {
431         scale = 1;
432         for (int i = 0; i < k; i++)
433             scale *= pyrScale;
434
435         double sigma = (1./scale - 1) * 0.5;
436         int smoothSize = cvRound(sigma*5) | 1;
437         smoothSize = std::max(smoothSize, 3);
438
439         int width = cvRound(size.width*scale);
440         int height = cvRound(size.height*scale);
441
442         if (fastPyramids)
443         {
444             width = pyramid0_[k].cols;
445             height = pyramid0_[k].rows;
446         }
447
448         if (k > 0)
449         {
450             curFlowX.create(height, width, CV_32F);
451             curFlowY.create(height, width, CV_32F);
452         }
453         else
454         {
455             curFlowX = flowx0;
456             curFlowY = flowy0;
457         }
458
459         if (!prevFlowX.data)
460         {
461             if (flags & cv::OPTFLOW_USE_INITIAL_FLOW)
462             {
463                 resize(flowx0, curFlowX, Size(width, height), 0, 0, INTER_LINEAR);
464                 resize(flowy0, curFlowY, Size(width, height), 0, 0, INTER_LINEAR);
465                 multiply(scale, curFlowX, curFlowX);
466                 multiply(scale, curFlowY, curFlowY);
467             }
468             else
469             {
470                 curFlowX.setTo(0);
471                 curFlowY.setTo(0);
472             }
473         }
474         else
475         {
476             resize(prevFlowX, curFlowX, Size(width, height), 0, 0, INTER_LINEAR);
477             resize(prevFlowY, curFlowY, Size(width, height), 0, 0, INTER_LINEAR);
478             multiply(1./pyrScale, curFlowX, curFlowX);
479             multiply(1./pyrScale, curFlowY, curFlowY);
480         }
481
482         oclMat M = allocMatFromBuf(5*height, width, CV_32F, M_);
483         oclMat bufM = allocMatFromBuf(5*height, width, CV_32F, bufM_);
484         oclMat R[2] =
485         {
486             allocMatFromBuf(5*height, width, CV_32F, R_[0]),
487             allocMatFromBuf(5*height, width, CV_32F, R_[1])
488         };
489
490         if (fastPyramids)
491         {
492             optflow_farneback::polynomialExpansionOcl(pyramid0_[k], polyN, R[0]);
493             optflow_farneback::polynomialExpansionOcl(pyramid1_[k], polyN, R[1]);
494         }
495         else
496         {
497             oclMat blurredFrame[2] =
498             {
499                 allocMatFromBuf(size.height, size.width, CV_32F, blurredFrame_[0]),
500                 allocMatFromBuf(size.height, size.width, CV_32F, blurredFrame_[1])
501             };
502             oclMat pyrLevel[2] =
503             {
504                 allocMatFromBuf(height, width, CV_32F, pyrLevel_[0]),
505                 allocMatFromBuf(height, width, CV_32F, pyrLevel_[1])
506             };
507
508             Mat g = getGaussianKernel(smoothSize, sigma, CV_32F);
509             optflow_farneback::setGaussianBlurKernel(g.ptr<float>(smoothSize/2), smoothSize/2);
510
511             for (int i = 0; i < 2; i++)
512             {
513                 optflow_farneback::gaussianBlurOcl(frames_[i], smoothSize/2, blurredFrame[i]);
514                 resize(blurredFrame[i], pyrLevel[i], Size(width, height), INTER_LINEAR);
515                 optflow_farneback::polynomialExpansionOcl(pyrLevel[i], polyN, R[i]);
516             }
517         }
518
519         optflow_farneback::updateMatricesOcl(curFlowX, curFlowY, R[0], R[1], M);
520
521         if (flags & OPTFLOW_FARNEBACK_GAUSSIAN)
522         {
523             Mat g = getGaussianKernel(winSize, winSize/2*0.3f, CV_32F);
524             optflow_farneback::setGaussianBlurKernel(g.ptr<float>(winSize/2), winSize/2);
525         }
526         for (int i = 0; i < numIters; i++)
527         {
528             if (flags & OPTFLOW_FARNEBACK_GAUSSIAN)
529                 updateFlow_gaussianBlur(R[0], R[1], curFlowX, curFlowY, M, bufM, winSize, i < numIters-1);
530             else
531                 updateFlow_boxFilter(R[0], R[1], curFlowX, curFlowY, M, bufM, winSize, i < numIters-1);
532         }
533
534         prevFlowX = curFlowX;
535         prevFlowY = curFlowY;
536     }
537
538     flowx = curFlowX;
539     flowy = curFlowY;
540 }