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 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41 #include "_cv.h"
42
43 typedef struct
44 {
45 float xx;
46 float xy;
47 float yy;
48 float xt;
49 float yt;
50 }
51 icvDerProduct;
52
53
54 #define CONV( A, B, C) ((float)( A + (B<<1) + C ))
55 /*F///////////////////////////////////////////////////////////////////////////////////////
56 // Name: icvCalcOpticalFlowLK_8u32fR ( Lucas & Kanade method )
57 // Purpose: calculate Optical flow for 2 images using Lucas & Kanade algorithm
58 // Context:
59 // Parameters:
60 // imgA, // pointer to first frame ROI
61 // imgB, // pointer to second frame ROI
62 // imgStep, // width of single row of source images in bytes
63 // imgSize, // size of the source image ROI
64 // winSize, // size of the averaging window used for grouping
65 // velocityX, // pointer to horizontal and
66 // velocityY, // vertical components of optical flow ROI
67 // velStep // width of single row of velocity frames in bytes
68 //
69 // Returns: CV_OK - all ok
70 // CV_OUTOFMEM_ERR - insufficient memory for function work
71 // CV_NULLPTR_ERR - if one of input pointers is NULL
72 // CV_BADSIZE_ERR - wrong input sizes interrelation
73 //
74 // Notes: 1.Optical flow to be computed for every pixel in ROI
75 // 2.For calculating spatial derivatives we use 3x3 Sobel operator.
76 // 3.We use the following border mode.
77 // The last row or column is replicated for the border
78 // ( IPL_BORDER_REPLICATE in IPL ).
79 //
80 //
81 //F*/
82 static CvStatus CV_STDCALL
icvCalcOpticalFlowLK_8u32fR(uchar * imgA,uchar * imgB,int imgStep,CvSize imgSize,CvSize winSize,float * velocityX,float * velocityY,int velStep)83 icvCalcOpticalFlowLK_8u32fR( uchar * imgA,
84 uchar * imgB,
85 int imgStep,
86 CvSize imgSize,
87 CvSize winSize,
88 float *velocityX,
89 float *velocityY, int velStep )
90 {
91 /* Loops indexes */
92 int i, j, k;
93
94 /* Gaussian separable kernels */
95 float GaussX[16];
96 float GaussY[16];
97 float *KerX;
98 float *KerY;
99
100 /* Buffers for Sobel calculations */
101 float *MemX[2];
102 float *MemY[2];
103
104 float ConvX, ConvY;
105 float GradX, GradY, GradT;
106
107 int winWidth = winSize.width;
108 int winHeight = winSize.height;
109
110 int imageWidth = imgSize.width;
111 int imageHeight = imgSize.height;
112
113 int HorRadius = (winWidth - 1) >> 1;
114 int VerRadius = (winHeight - 1) >> 1;
115
116 int PixelLine;
117 int ConvLine;
118
119 int BufferAddress;
120
121 int BufferHeight = 0;
122 int BufferWidth;
123 int BufferSize;
124
125 /* buffers derivatives product */
126 icvDerProduct *II;
127
128 /* buffers for gaussian horisontal convolution */
129 icvDerProduct *WII;
130
131 /* variables for storing number of first pixel of image line */
132 int Line1;
133 int Line2;
134 int Line3;
135
136 /* we must have 2*2 linear system coeffs
137 | A1B2 B1 | {u} {C1} {0}
138 | | { } + { } = { }
139 | A2 A1B2 | {v} {C2} {0}
140 */
141 float A1B2, A2, B1, C1, C2;
142
143 int pixNumber;
144
145 /* auxiliary */
146 int NoMem = 0;
147
148 velStep /= sizeof(velocityX[0]);
149
150 /* Checking bad arguments */
151 if( imgA == NULL )
152 return CV_NULLPTR_ERR;
153 if( imgB == NULL )
154 return CV_NULLPTR_ERR;
155
156 if( imageHeight < winHeight )
157 return CV_BADSIZE_ERR;
158 if( imageWidth < winWidth )
159 return CV_BADSIZE_ERR;
160
161 if( winHeight >= 16 )
162 return CV_BADSIZE_ERR;
163 if( winWidth >= 16 )
164 return CV_BADSIZE_ERR;
165
166 if( !(winHeight & 1) )
167 return CV_BADSIZE_ERR;
168 if( !(winWidth & 1) )
169 return CV_BADSIZE_ERR;
170
171 BufferHeight = winHeight;
172 BufferWidth = imageWidth;
173
174 /****************************************************************************************/
175 /* Computing Gaussian coeffs */
176 /****************************************************************************************/
177 GaussX[0] = 1;
178 GaussY[0] = 1;
179 for( i = 1; i < winWidth; i++ )
180 {
181 GaussX[i] = 1;
182 for( j = i - 1; j > 0; j-- )
183 {
184 GaussX[j] += GaussX[j - 1];
185 }
186 }
187 for( i = 1; i < winHeight; i++ )
188 {
189 GaussY[i] = 1;
190 for( j = i - 1; j > 0; j-- )
191 {
192 GaussY[j] += GaussY[j - 1];
193 }
194 }
195 KerX = &GaussX[HorRadius];
196 KerY = &GaussY[VerRadius];
197
198 /****************************************************************************************/
199 /* Allocating memory for all buffers */
200 /****************************************************************************************/
201 for( k = 0; k < 2; k++ )
202 {
203 MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float ));
204
205 if( MemX[k] == NULL )
206 NoMem = 1;
207 MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float ));
208
209 if( MemY[k] == NULL )
210 NoMem = 1;
211 }
212
213 BufferSize = BufferHeight * BufferWidth;
214
215 II = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct ));
216 WII = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct ));
217
218
219 if( (II == NULL) || (WII == NULL) )
220 NoMem = 1;
221
222 if( NoMem )
223 {
224 for( k = 0; k < 2; k++ )
225 {
226 if( MemX[k] )
227 cvFree( &MemX[k] );
228
229 if( MemY[k] )
230 cvFree( &MemY[k] );
231 }
232 if( II )
233 cvFree( &II );
234 if( WII )
235 cvFree( &WII );
236
237 return CV_OUTOFMEM_ERR;
238 }
239
240 /****************************************************************************************/
241 /* Calculate first line of memX and memY */
242 /****************************************************************************************/
243 MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] );
244 MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] );
245
246 for( j = 1; j < imageWidth - 1; j++ )
247 {
248 MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] );
249 }
250
251 pixNumber = imgStep;
252 for( i = 1; i < imageHeight - 1; i++ )
253 {
254 MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep],
255 imgA[pixNumber], imgA[pixNumber + imgStep] );
256 pixNumber += imgStep;
257 }
258
259 MemY[0][imageWidth - 1] =
260 MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2],
261 imgA[imageWidth - 1], imgA[imageWidth - 1] );
262
263 MemX[0][imageHeight - 1] =
264 MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep],
265 imgA[pixNumber], imgA[pixNumber] );
266
267
268 /****************************************************************************************/
269 /* begin scan image, calc derivatives and solve system */
270 /****************************************************************************************/
271
272 PixelLine = -VerRadius;
273 ConvLine = 0;
274 BufferAddress = -BufferWidth;
275
276 while( PixelLine < imageHeight )
277 {
278 if( ConvLine < imageHeight )
279 {
280 /*Here we calculate derivatives for line of image */
281 int address;
282
283 i = ConvLine;
284 int L1 = i - 1;
285 int L2 = i;
286 int L3 = i + 1;
287
288 int memYline = L3 & 1;
289
290 if( L1 < 0 )
291 L1 = 0;
292 if( L3 >= imageHeight )
293 L3 = imageHeight - 1;
294
295 BufferAddress += BufferWidth;
296 BufferAddress -= ((BufferAddress >= BufferSize) ? 0xffffffff : 0) & BufferSize;
297
298 address = BufferAddress;
299
300 Line1 = L1 * imgStep;
301 Line2 = L2 * imgStep;
302 Line3 = L3 * imgStep;
303
304 /* Process first pixel */
305 ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] );
306 ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] );
307
308 GradY = ConvY - MemY[memYline][0];
309 GradX = ConvX - MemX[1][L2];
310
311 MemY[memYline][0] = ConvY;
312 MemX[1][L2] = ConvX;
313
314 GradT = (float) (imgB[Line2] - imgA[Line2]);
315
316 II[address].xx = GradX * GradX;
317 II[address].xy = GradX * GradY;
318 II[address].yy = GradY * GradY;
319 II[address].xt = GradX * GradT;
320 II[address].yt = GradY * GradT;
321 address++;
322 /* Process middle of line */
323 for( j = 1; j < imageWidth - 1; j++ )
324 {
325 ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] );
326 ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] );
327
328 GradY = ConvY - MemY[memYline][j];
329 GradX = ConvX - MemX[(j - 1) & 1][L2];
330
331 MemY[memYline][j] = ConvY;
332 MemX[(j - 1) & 1][L2] = ConvX;
333
334 GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]);
335
336 II[address].xx = GradX * GradX;
337 II[address].xy = GradX * GradY;
338 II[address].yy = GradY * GradY;
339 II[address].xt = GradX * GradT;
340 II[address].yt = GradY * GradT;
341
342 address++;
343 }
344 /* Process last pixel of line */
345 ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1],
346 imgA[Line3 + imageWidth - 1] );
347
348 ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1],
349 imgA[Line3 + imageWidth - 1] );
350
351
352 GradY = ConvY - MemY[memYline][imageWidth - 1];
353 GradX = ConvX - MemX[(imageWidth - 2) & 1][L2];
354
355 MemY[memYline][imageWidth - 1] = ConvY;
356
357 GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]);
358
359 II[address].xx = GradX * GradX;
360 II[address].xy = GradX * GradY;
361 II[address].yy = GradY * GradY;
362 II[address].xt = GradX * GradT;
363 II[address].yt = GradY * GradT;
364 address++;
365
366 /* End of derivatives for line */
367
368 /****************************************************************************************/
369 /* ---------Calculating horizontal convolution of processed line----------------------- */
370 /****************************************************************************************/
371 address -= BufferWidth;
372 /* process first HorRadius pixels */
373 for( j = 0; j < HorRadius; j++ )
374 {
375 int jj;
376
377 WII[address].xx = 0;
378 WII[address].xy = 0;
379 WII[address].yy = 0;
380 WII[address].xt = 0;
381 WII[address].yt = 0;
382
383 for( jj = -j; jj <= HorRadius; jj++ )
384 {
385 float Ker = KerX[jj];
386
387 WII[address].xx += II[address + jj].xx * Ker;
388 WII[address].xy += II[address + jj].xy * Ker;
389 WII[address].yy += II[address + jj].yy * Ker;
390 WII[address].xt += II[address + jj].xt * Ker;
391 WII[address].yt += II[address + jj].yt * Ker;
392 }
393 address++;
394 }
395 /* process inner part of line */
396 for( j = HorRadius; j < imageWidth - HorRadius; j++ )
397 {
398 int jj;
399 float Ker0 = KerX[0];
400
401 WII[address].xx = 0;
402 WII[address].xy = 0;
403 WII[address].yy = 0;
404 WII[address].xt = 0;
405 WII[address].yt = 0;
406
407 for( jj = 1; jj <= HorRadius; jj++ )
408 {
409 float Ker = KerX[jj];
410
411 WII[address].xx += (II[address - jj].xx + II[address + jj].xx) * Ker;
412 WII[address].xy += (II[address - jj].xy + II[address + jj].xy) * Ker;
413 WII[address].yy += (II[address - jj].yy + II[address + jj].yy) * Ker;
414 WII[address].xt += (II[address - jj].xt + II[address + jj].xt) * Ker;
415 WII[address].yt += (II[address - jj].yt + II[address + jj].yt) * Ker;
416 }
417 WII[address].xx += II[address].xx * Ker0;
418 WII[address].xy += II[address].xy * Ker0;
419 WII[address].yy += II[address].yy * Ker0;
420 WII[address].xt += II[address].xt * Ker0;
421 WII[address].yt += II[address].yt * Ker0;
422
423 address++;
424 }
425 /* process right side */
426 for( j = imageWidth - HorRadius; j < imageWidth; j++ )
427 {
428 int jj;
429
430 WII[address].xx = 0;
431 WII[address].xy = 0;
432 WII[address].yy = 0;
433 WII[address].xt = 0;
434 WII[address].yt = 0;
435
436 for( jj = -HorRadius; jj < imageWidth - j; jj++ )
437 {
438 float Ker = KerX[jj];
439
440 WII[address].xx += II[address + jj].xx * Ker;
441 WII[address].xy += II[address + jj].xy * Ker;
442 WII[address].yy += II[address + jj].yy * Ker;
443 WII[address].xt += II[address + jj].xt * Ker;
444 WII[address].yt += II[address + jj].yt * Ker;
445 }
446 address++;
447 }
448 }
449
450 /****************************************************************************************/
451 /* Calculating velocity line */
452 /****************************************************************************************/
453 if( PixelLine >= 0 )
454 {
455 int USpace;
456 int BSpace;
457 int address;
458
459 if( PixelLine < VerRadius )
460 USpace = PixelLine;
461 else
462 USpace = VerRadius;
463
464 if( PixelLine >= imageHeight - VerRadius )
465 BSpace = imageHeight - PixelLine - 1;
466 else
467 BSpace = VerRadius;
468
469 address = ((PixelLine - USpace) % BufferHeight) * BufferWidth;
470 for( j = 0; j < imageWidth; j++ )
471 {
472 int addr = address;
473
474 A1B2 = 0;
475 A2 = 0;
476 B1 = 0;
477 C1 = 0;
478 C2 = 0;
479
480 for( i = -USpace; i <= BSpace; i++ )
481 {
482 A2 += WII[addr + j].xx * KerY[i];
483 A1B2 += WII[addr + j].xy * KerY[i];
484 B1 += WII[addr + j].yy * KerY[i];
485 C2 += WII[addr + j].xt * KerY[i];
486 C1 += WII[addr + j].yt * KerY[i];
487
488 addr += BufferWidth;
489 addr -= ((addr >= BufferSize) ? 0xffffffff : 0) & BufferSize;
490 }
491 /****************************************************************************************\
492 * Solve Linear System *
493 \****************************************************************************************/
494 {
495 float delta = (A1B2 * A1B2 - A2 * B1);
496
497 if( delta )
498 {
499 /* system is not singular - solving by Kramer method */
500 float deltaX;
501 float deltaY;
502 float Idelta = 8 / delta;
503
504 deltaX = -(C1 * A1B2 - C2 * B1);
505 deltaY = -(A1B2 * C2 - A2 * C1);
506
507 velocityX[j] = deltaX * Idelta;
508 velocityY[j] = deltaY * Idelta;
509 }
510 else
511 {
512 /* singular system - find optical flow in gradient direction */
513 float Norm = (A1B2 + A2) * (A1B2 + A2) + (B1 + A1B2) * (B1 + A1B2);
514
515 if( Norm )
516 {
517 float IGradNorm = 8 / Norm;
518 float temp = -(C1 + C2) * IGradNorm;
519
520 velocityX[j] = (A1B2 + A2) * temp;
521 velocityY[j] = (B1 + A1B2) * temp;
522
523 }
524 else
525 {
526 velocityX[j] = 0;
527 velocityY[j] = 0;
528 }
529 }
530 }
531 /****************************************************************************************\
532 * End of Solving Linear System *
533 \****************************************************************************************/
534 } /*for */
535 velocityX += velStep;
536 velocityY += velStep;
537 } /*for */
538 PixelLine++;
539 ConvLine++;
540 }
541
542 /* Free memory */
543 for( k = 0; k < 2; k++ )
544 {
545 cvFree( &MemX[k] );
546 cvFree( &MemY[k] );
547 }
548 cvFree( &II );
549 cvFree( &WII );
550
551 return CV_OK;
552 } /*icvCalcOpticalFlowLK_8u32fR*/
553
554
555 /*F///////////////////////////////////////////////////////////////////////////////////////
556 // Name: cvCalcOpticalFlowLK
557 // Purpose: Optical flow implementation
558 // Context:
559 // Parameters:
560 // srcA, srcB - source image
561 // velx, vely - destination image
562 // Returns:
563 //
564 // Notes:
565 //F*/
566 CV_IMPL void
cvCalcOpticalFlowLK(const void * srcarrA,const void * srcarrB,CvSize winSize,void * velarrx,void * velarry)567 cvCalcOpticalFlowLK( const void* srcarrA, const void* srcarrB, CvSize winSize,
568 void* velarrx, void* velarry )
569 {
570 CV_FUNCNAME( "cvCalcOpticalFlowLK" );
571
572 __BEGIN__;
573
574 CvMat stubA, *srcA = (CvMat*)srcarrA;
575 CvMat stubB, *srcB = (CvMat*)srcarrB;
576 CvMat stubx, *velx = (CvMat*)velarrx;
577 CvMat stuby, *vely = (CvMat*)velarry;
578
579 CV_CALL( srcA = cvGetMat( srcA, &stubA ));
580 CV_CALL( srcB = cvGetMat( srcB, &stubB ));
581
582 CV_CALL( velx = cvGetMat( velx, &stubx ));
583 CV_CALL( vely = cvGetMat( vely, &stuby ));
584
585 if( !CV_ARE_TYPES_EQ( srcA, srcB ))
586 CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" );
587
588 if( !CV_ARE_TYPES_EQ( velx, vely ))
589 CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );
590
591 if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
592 !CV_ARE_SIZES_EQ( velx, vely ) ||
593 !CV_ARE_SIZES_EQ( srcA, velx ))
594 CV_ERROR( CV_StsUnmatchedSizes, "" );
595
596 if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
597 CV_MAT_TYPE( velx->type ) != CV_32FC1 )
598 CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
599 "destination images must have 32fC1 type" );
600
601 if( srcA->step != srcB->step || velx->step != vely->step )
602 CV_ERROR( CV_BadStep, "source and destination images have different step" );
603
604 IPPI_CALL( icvCalcOpticalFlowLK_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
605 srcA->step, cvGetMatSize( srcA ), winSize,
606 velx->data.fl, vely->data.fl, velx->step ));
607
608 __END__;
609 }
610
611 /* End of file. */
612