added dual tvl1 optical flow gpu implementation
[profile/ivi/opencv.git] / modules / gpu / src / cuda / pyrlk.cu
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 // Copyright (c) 2010, Paul Furgale, Chi Hay Tong
42 //
43 // The original code was written by Paul Furgale and Chi Hay Tong
44 // and later optimized and prepared for integration into OpenCV by Itseez.
45 //
46 //M*/
47
48 #if !defined CUDA_DISABLER
49
50 #include "opencv2/gpu/device/common.hpp"
51 #include "opencv2/gpu/device/utility.hpp"
52 #include "opencv2/gpu/device/functional.hpp"
53 #include "opencv2/gpu/device/limits.hpp"
54 #include "opencv2/gpu/device/vec_math.hpp"
55 #include "opencv2/gpu/device/reduce.hpp"
56
57 using namespace cv::gpu;
58 using namespace cv::gpu::device;
59
60 namespace pyrlk
61 {
62     __constant__ int c_winSize_x;
63     __constant__ int c_winSize_y;
64     __constant__ int c_halfWin_x;
65     __constant__ int c_halfWin_y;
66     __constant__ int c_iters;
67
68     texture<float, cudaTextureType2D, cudaReadModeElementType> tex_If(false, cudaFilterModeLinear, cudaAddressModeClamp);
69     texture<float4, cudaTextureType2D, cudaReadModeElementType> tex_If4(false, cudaFilterModeLinear, cudaAddressModeClamp);
70     texture<uchar, cudaTextureType2D, cudaReadModeElementType> tex_Ib(false, cudaFilterModePoint, cudaAddressModeClamp);
71
72     texture<float, cudaTextureType2D, cudaReadModeElementType> tex_Jf(false, cudaFilterModeLinear, cudaAddressModeClamp);
73     texture<float4, cudaTextureType2D, cudaReadModeElementType> tex_Jf4(false, cudaFilterModeLinear, cudaAddressModeClamp);
74
75     template <int cn> struct Tex_I;
76     template <> struct Tex_I<1>
77     {
78         static __device__ __forceinline__ float read(float x, float y)
79         {
80             return tex2D(tex_If, x, y);
81         }
82     };
83     template <> struct Tex_I<4>
84     {
85         static __device__ __forceinline__ float4 read(float x, float y)
86         {
87             return tex2D(tex_If4, x, y);
88         }
89     };
90
91     template <int cn> struct Tex_J;
92     template <> struct Tex_J<1>
93     {
94         static __device__ __forceinline__ float read(float x, float y)
95         {
96             return tex2D(tex_Jf, x, y);
97         }
98     };
99     template <> struct Tex_J<4>
100     {
101         static __device__ __forceinline__ float4 read(float x, float y)
102         {
103             return tex2D(tex_Jf4, x, y);
104         }
105     };
106
107     __device__ __forceinline__ void accum(float& dst, float val)
108     {
109         dst += val;
110     }
111     __device__ __forceinline__ void accum(float& dst, const float4& val)
112     {
113         dst += val.x + val.y + val.z;
114     }
115
116     __device__ __forceinline__ float abs_(float a)
117     {
118         return ::fabsf(a);
119     }
120     __device__ __forceinline__ float4 abs_(const float4& a)
121     {
122         return abs(a);
123     }
124
125     template <int cn, int PATCH_X, int PATCH_Y, bool calcErr>
126     __global__ void sparseKernel(const float2* prevPts, float2* nextPts, uchar* status, float* err, const int level, const int rows, const int cols)
127     {
128     #if __CUDA_ARCH__ <= 110
129         const int BLOCK_SIZE = 128;
130     #else
131         const int BLOCK_SIZE = 256;
132     #endif
133
134         __shared__ float smem1[BLOCK_SIZE];
135         __shared__ float smem2[BLOCK_SIZE];
136         __shared__ float smem3[BLOCK_SIZE];
137
138         const unsigned int tid = threadIdx.y * blockDim.x + threadIdx.x;
139
140         float2 prevPt = prevPts[blockIdx.x];
141         prevPt.x *= (1.0f / (1 << level));
142         prevPt.y *= (1.0f / (1 << level));
143
144         if (prevPt.x < 0 || prevPt.x >= cols || prevPt.y < 0 || prevPt.y >= rows)
145         {
146             if (tid == 0 && level == 0)
147                 status[blockIdx.x] = 0;
148
149             return;
150         }
151
152         prevPt.x -= c_halfWin_x;
153         prevPt.y -= c_halfWin_y;
154
155         // extract the patch from the first image, compute covariation matrix of derivatives
156
157         float A11 = 0;
158         float A12 = 0;
159         float A22 = 0;
160
161         typedef typename TypeVec<float, cn>::vec_type work_type;
162
163         work_type I_patch   [PATCH_Y][PATCH_X];
164         work_type dIdx_patch[PATCH_Y][PATCH_X];
165         work_type dIdy_patch[PATCH_Y][PATCH_X];
166
167         for (int yBase = threadIdx.y, i = 0; yBase < c_winSize_y; yBase += blockDim.y, ++i)
168         {
169             for (int xBase = threadIdx.x, j = 0; xBase < c_winSize_x; xBase += blockDim.x, ++j)
170             {
171                 float x = prevPt.x + xBase + 0.5f;
172                 float y = prevPt.y + yBase + 0.5f;
173
174                 I_patch[i][j] = Tex_I<cn>::read(x, y);
175
176                 // Sharr Deriv
177
178                 work_type dIdx = 3.0f * Tex_I<cn>::read(x+1, y-1) + 10.0f * Tex_I<cn>::read(x+1, y) + 3.0f * Tex_I<cn>::read(x+1, y+1) -
179                                  (3.0f * Tex_I<cn>::read(x-1, y-1) + 10.0f * Tex_I<cn>::read(x-1, y) + 3.0f * Tex_I<cn>::read(x-1, y+1));
180
181                 work_type dIdy = 3.0f * Tex_I<cn>::read(x-1, y+1) + 10.0f * Tex_I<cn>::read(x, y+1) + 3.0f * Tex_I<cn>::read(x+1, y+1) -
182                                 (3.0f * Tex_I<cn>::read(x-1, y-1) + 10.0f * Tex_I<cn>::read(x, y-1) + 3.0f * Tex_I<cn>::read(x+1, y-1));
183
184                 dIdx_patch[i][j] = dIdx;
185                 dIdy_patch[i][j] = dIdy;
186
187                 accum(A11, dIdx * dIdx);
188                 accum(A12, dIdx * dIdy);
189                 accum(A22, dIdy * dIdy);
190             }
191         }
192
193         reduce<BLOCK_SIZE>(smem_tuple(smem1, smem2, smem3), thrust::tie(A11, A12, A22), tid, thrust::make_tuple(plus<float>(), plus<float>(), plus<float>()));
194
195     #if __CUDA_ARCH__ >= 300
196         if (tid == 0)
197         {
198             smem1[0] = A11;
199             smem2[0] = A12;
200             smem3[0] = A22;
201         }
202     #endif
203
204         __syncthreads();
205
206         A11 = smem1[0];
207         A12 = smem2[0];
208         A22 = smem3[0];
209
210         float D = A11 * A22 - A12 * A12;
211
212         if (D < numeric_limits<float>::epsilon())
213         {
214             if (tid == 0 && level == 0)
215                 status[blockIdx.x] = 0;
216
217             return;
218         }
219
220         D = 1.f / D;
221
222         A11 *= D;
223         A12 *= D;
224         A22 *= D;
225
226         float2 nextPt = nextPts[blockIdx.x];
227         nextPt.x *= 2.f;
228         nextPt.y *= 2.f;
229
230         nextPt.x -= c_halfWin_x;
231         nextPt.y -= c_halfWin_y;
232
233         for (int k = 0; k < c_iters; ++k)
234         {
235             if (nextPt.x < -c_halfWin_x || nextPt.x >= cols || nextPt.y < -c_halfWin_y || nextPt.y >= rows)
236             {
237                 if (tid == 0 && level == 0)
238                     status[blockIdx.x] = 0;
239
240                 return;
241             }
242
243             float b1 = 0;
244             float b2 = 0;
245
246             for (int y = threadIdx.y, i = 0; y < c_winSize_y; y += blockDim.y, ++i)
247             {
248                 for (int x = threadIdx.x, j = 0; x < c_winSize_x; x += blockDim.x, ++j)
249                 {
250                     work_type I_val = I_patch[i][j];
251                     work_type J_val = Tex_J<cn>::read(nextPt.x + x + 0.5f, nextPt.y + y + 0.5f);
252
253                     work_type diff = (J_val - I_val) * 32.0f;
254
255                     accum(b1, diff * dIdx_patch[i][j]);
256                     accum(b2, diff * dIdy_patch[i][j]);
257                 }
258             }
259
260             reduce<BLOCK_SIZE>(smem_tuple(smem1, smem2), thrust::tie(b1, b2), tid, thrust::make_tuple(plus<float>(), plus<float>()));
261
262         #if __CUDA_ARCH__ >= 300
263             if (tid == 0)
264             {
265                 smem1[0] = b1;
266                 smem2[0] = b2;
267             }
268         #endif
269
270             __syncthreads();
271
272             b1 = smem1[0];
273             b2 = smem2[0];
274
275             float2 delta;
276             delta.x = A12 * b2 - A22 * b1;
277             delta.y = A12 * b1 - A11 * b2;
278
279             nextPt.x += delta.x;
280             nextPt.y += delta.y;
281
282             if (::fabs(delta.x) < 0.01f && ::fabs(delta.y) < 0.01f)
283                 break;
284         }
285
286         float errval = 0;
287         if (calcErr)
288         {
289             for (int y = threadIdx.y, i = 0; y < c_winSize_y; y += blockDim.y, ++i)
290             {
291                 for (int x = threadIdx.x, j = 0; x < c_winSize_x; x += blockDim.x, ++j)
292                 {
293                     work_type I_val = I_patch[i][j];
294                     work_type J_val = Tex_J<cn>::read(nextPt.x + x + 0.5f, nextPt.y + y + 0.5f);
295
296                     work_type diff = J_val - I_val;
297
298                     accum(errval, abs_(diff));
299                 }
300             }
301
302             reduce<BLOCK_SIZE>(smem1, errval, tid, plus<float>());
303         }
304
305         if (tid == 0)
306         {
307             nextPt.x += c_halfWin_x;
308             nextPt.y += c_halfWin_y;
309
310             nextPts[blockIdx.x] = nextPt;
311
312             if (calcErr)
313                 err[blockIdx.x] = static_cast<float>(errval) / (cn * c_winSize_x * c_winSize_y);
314         }
315     }
316
317     template <int cn, int PATCH_X, int PATCH_Y>
318     void sparse_caller(int rows, int cols, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
319                        int level, dim3 block, cudaStream_t stream)
320     {
321         dim3 grid(ptcount);
322
323         if (level == 0 && err)
324             sparseKernel<cn, PATCH_X, PATCH_Y, true><<<grid, block>>>(prevPts, nextPts, status, err, level, rows, cols);
325         else
326             sparseKernel<cn, PATCH_X, PATCH_Y, false><<<grid, block>>>(prevPts, nextPts, status, err, level, rows, cols);
327
328         cudaSafeCall( cudaGetLastError() );
329
330         if (stream == 0)
331             cudaSafeCall( cudaDeviceSynchronize() );
332     }
333
334     template <bool calcErr>
335     __global__ void denseKernel(PtrStepf u, PtrStepf v, const PtrStepf prevU, const PtrStepf prevV, PtrStepf err, const int rows, const int cols)
336     {
337         extern __shared__ int smem[];
338
339         const int patchWidth  = blockDim.x + 2 * c_halfWin_x;
340         const int patchHeight = blockDim.y + 2 * c_halfWin_y;
341
342         int* I_patch = smem;
343         int* dIdx_patch = I_patch + patchWidth * patchHeight;
344         int* dIdy_patch = dIdx_patch + patchWidth * patchHeight;
345
346         const int xBase = blockIdx.x * blockDim.x;
347         const int yBase = blockIdx.y * blockDim.y;
348
349         for (int i = threadIdx.y; i < patchHeight; i += blockDim.y)
350         {
351             for (int j = threadIdx.x; j < patchWidth; j += blockDim.x)
352             {
353                 float x = xBase - c_halfWin_x + j + 0.5f;
354                 float y = yBase - c_halfWin_y + i + 0.5f;
355
356                 I_patch[i * patchWidth + j] = tex2D(tex_Ib, x, y);
357
358                 // Sharr Deriv
359
360                 dIdx_patch[i * patchWidth + j] = 3 * tex2D(tex_Ib, x+1, y-1) + 10 * tex2D(tex_Ib, x+1, y) + 3 * tex2D(tex_Ib, x+1, y+1) -
361                                                 (3 * tex2D(tex_Ib, x-1, y-1) + 10 * tex2D(tex_Ib, x-1, y) + 3 * tex2D(tex_Ib, x-1, y+1));
362
363                 dIdy_patch[i * patchWidth + j] = 3 * tex2D(tex_Ib, x-1, y+1) + 10 * tex2D(tex_Ib, x, y+1) + 3 * tex2D(tex_Ib, x+1, y+1) -
364                                                 (3 * tex2D(tex_Ib, x-1, y-1) + 10 * tex2D(tex_Ib, x, y-1) + 3 * tex2D(tex_Ib, x+1, y-1));
365             }
366         }
367
368         __syncthreads();
369
370         const int x = xBase + threadIdx.x;
371         const int y = yBase + threadIdx.y;
372
373         if (x >= cols || y >= rows)
374             return;
375
376         int A11i = 0;
377         int A12i = 0;
378         int A22i = 0;
379
380         for (int i = 0; i < c_winSize_y; ++i)
381         {
382             for (int j = 0; j < c_winSize_x; ++j)
383             {
384                 int dIdx = dIdx_patch[(threadIdx.y + i) * patchWidth + (threadIdx.x + j)];
385                 int dIdy = dIdy_patch[(threadIdx.y + i) * patchWidth + (threadIdx.x + j)];
386
387                 A11i += dIdx * dIdx;
388                 A12i += dIdx * dIdy;
389                 A22i += dIdy * dIdy;
390             }
391         }
392
393         float A11 = A11i;
394         float A12 = A12i;
395         float A22 = A22i;
396
397         float D = A11 * A22 - A12 * A12;
398
399         if (D < numeric_limits<float>::epsilon())
400         {
401             if (calcErr)
402                 err(y, x) = numeric_limits<float>::max();
403
404             return;
405         }
406
407         D = 1.f / D;
408
409         A11 *= D;
410         A12 *= D;
411         A22 *= D;
412
413         float2 nextPt;
414         nextPt.x = x + prevU(y/2, x/2) * 2.0f;
415         nextPt.y = y + prevV(y/2, x/2) * 2.0f;
416
417         for (int k = 0; k < c_iters; ++k)
418         {
419             if (nextPt.x < 0 || nextPt.x >= cols || nextPt.y < 0 || nextPt.y >= rows)
420             {
421                 if (calcErr)
422                     err(y, x) = numeric_limits<float>::max();
423
424                 return;
425             }
426
427             int b1 = 0;
428             int b2 = 0;
429
430             for (int i = 0; i < c_winSize_y; ++i)
431             {
432                 for (int j = 0; j < c_winSize_x; ++j)
433                 {
434                     int I = I_patch[(threadIdx.y + i) * patchWidth + threadIdx.x + j];
435                     int J = tex2D(tex_Jf, nextPt.x - c_halfWin_x + j + 0.5f, nextPt.y - c_halfWin_y + i + 0.5f);
436
437                     int diff = (J - I) * 32;
438
439                     int dIdx = dIdx_patch[(threadIdx.y + i) * patchWidth + (threadIdx.x + j)];
440                     int dIdy = dIdy_patch[(threadIdx.y + i) * patchWidth + (threadIdx.x + j)];
441
442                     b1 += diff * dIdx;
443                     b2 += diff * dIdy;
444                 }
445             }
446
447             float2 delta;
448             delta.x = A12 * b2 - A22 * b1;
449             delta.y = A12 * b1 - A11 * b2;
450
451             nextPt.x += delta.x;
452             nextPt.y += delta.y;
453
454             if (::fabs(delta.x) < 0.01f && ::fabs(delta.y) < 0.01f)
455                 break;
456         }
457
458         u(y, x) = nextPt.x - x;
459         v(y, x) = nextPt.y - y;
460
461         if (calcErr)
462         {
463             int errval = 0;
464
465             for (int i = 0; i < c_winSize_y; ++i)
466             {
467                 for (int j = 0; j < c_winSize_x; ++j)
468                 {
469                     int I = I_patch[(threadIdx.y + i) * patchWidth + threadIdx.x + j];
470                     int J = tex2D(tex_Jf, nextPt.x - c_halfWin_x + j + 0.5f, nextPt.y - c_halfWin_y + i + 0.5f);
471
472                     errval += ::abs(J - I);
473                 }
474             }
475
476             err(y, x) = static_cast<float>(errval) / (c_winSize_x * c_winSize_y);
477         }
478     }
479
480     void loadConstants(int2 winSize, int iters)
481     {
482         cudaSafeCall( cudaMemcpyToSymbol(c_winSize_x, &winSize.x, sizeof(int)) );
483         cudaSafeCall( cudaMemcpyToSymbol(c_winSize_y, &winSize.y, sizeof(int)) );
484
485         int2 halfWin = make_int2((winSize.x - 1) / 2, (winSize.y - 1) / 2);
486         cudaSafeCall( cudaMemcpyToSymbol(c_halfWin_x, &halfWin.x, sizeof(int)) );
487         cudaSafeCall( cudaMemcpyToSymbol(c_halfWin_y, &halfWin.y, sizeof(int)) );
488
489         cudaSafeCall( cudaMemcpyToSymbol(c_iters, &iters, sizeof(int)) );
490     }
491
492     void sparse1(PtrStepSzf I, PtrStepSzf J, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
493                  int level, dim3 block, dim3 patch, cudaStream_t stream)
494     {
495         typedef void (*func_t)(int rows, int cols, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
496                                int level, dim3 block, cudaStream_t stream);
497
498         static const func_t funcs[5][5] =
499         {
500             {sparse_caller<1, 1, 1>, sparse_caller<1, 2, 1>, sparse_caller<1, 3, 1>, sparse_caller<1, 4, 1>, sparse_caller<1, 5, 1>},
501             {sparse_caller<1, 1, 2>, sparse_caller<1, 2, 2>, sparse_caller<1, 3, 2>, sparse_caller<1, 4, 2>, sparse_caller<1, 5, 2>},
502             {sparse_caller<1, 1, 3>, sparse_caller<1, 2, 3>, sparse_caller<1, 3, 3>, sparse_caller<1, 4, 3>, sparse_caller<1, 5, 3>},
503             {sparse_caller<1, 1, 4>, sparse_caller<1, 2, 4>, sparse_caller<1, 3, 4>, sparse_caller<1, 4, 4>, sparse_caller<1, 5, 4>},
504             {sparse_caller<1, 1, 5>, sparse_caller<1, 2, 5>, sparse_caller<1, 3, 5>, sparse_caller<1, 4, 5>, sparse_caller<1, 5, 5>}
505         };
506
507         bindTexture(&tex_If, I);
508         bindTexture(&tex_Jf, J);
509
510         funcs[patch.y - 1][patch.x - 1](I.rows, I.cols, prevPts, nextPts, status, err, ptcount,
511             level, block, stream);
512     }
513
514     void sparse4(PtrStepSz<float4> I, PtrStepSz<float4> J, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
515                  int level, dim3 block, dim3 patch, cudaStream_t stream)
516     {
517         typedef void (*func_t)(int rows, int cols, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
518                                int level, dim3 block, cudaStream_t stream);
519
520         static const func_t funcs[5][5] =
521         {
522             {sparse_caller<4, 1, 1>, sparse_caller<4, 2, 1>, sparse_caller<4, 3, 1>, sparse_caller<4, 4, 1>, sparse_caller<4, 5, 1>},
523             {sparse_caller<4, 1, 2>, sparse_caller<4, 2, 2>, sparse_caller<4, 3, 2>, sparse_caller<4, 4, 2>, sparse_caller<4, 5, 2>},
524             {sparse_caller<4, 1, 3>, sparse_caller<4, 2, 3>, sparse_caller<4, 3, 3>, sparse_caller<4, 4, 3>, sparse_caller<4, 5, 3>},
525             {sparse_caller<4, 1, 4>, sparse_caller<4, 2, 4>, sparse_caller<4, 3, 4>, sparse_caller<4, 4, 4>, sparse_caller<4, 5, 4>},
526             {sparse_caller<4, 1, 5>, sparse_caller<4, 2, 5>, sparse_caller<4, 3, 5>, sparse_caller<4, 4, 5>, sparse_caller<4, 5, 5>}
527         };
528
529         bindTexture(&tex_If4, I);
530         bindTexture(&tex_Jf4, J);
531
532         funcs[patch.y - 1][patch.x - 1](I.rows, I.cols, prevPts, nextPts, status, err, ptcount,
533             level, block, stream);
534     }
535
536     void dense(PtrStepSzb I, PtrStepSzf J, PtrStepSzf u, PtrStepSzf v, PtrStepSzf prevU, PtrStepSzf prevV, PtrStepSzf err, int2 winSize, cudaStream_t stream)
537     {
538         dim3 block(16, 16);
539         dim3 grid(divUp(I.cols, block.x), divUp(I.rows, block.y));
540
541         bindTexture(&tex_Ib, I);
542         bindTexture(&tex_Jf, J);
543
544         int2 halfWin = make_int2((winSize.x - 1) / 2, (winSize.y - 1) / 2);
545         const int patchWidth  = block.x + 2 * halfWin.x;
546         const int patchHeight = block.y + 2 * halfWin.y;
547         size_t smem_size = 3 * patchWidth * patchHeight * sizeof(int);
548
549         if (err.data)
550         {
551             denseKernel<true><<<grid, block, smem_size, stream>>>(u, v, prevU, prevV, err, I.rows, I.cols);
552             cudaSafeCall( cudaGetLastError() );
553         }
554         else
555         {
556             denseKernel<false><<<grid, block, smem_size, stream>>>(u, v, prevU, prevV, PtrStepf(), I.rows, I.cols);
557             cudaSafeCall( cudaGetLastError() );
558         }
559
560         if (stream == 0)
561             cudaSafeCall( cudaDeviceSynchronize() );
562     }
563 }
564
565 #endif /* CUDA_DISABLER */