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 #define CONV( A, B, C) ( (float)( A + (B<<1) + C ) )
44
45 typedef struct
46 {
47 float xx;
48 float xy;
49 float yy;
50 float xt;
51 float yt;
52 float alpha; /* alpha = 1 / ( 1/lambda + xx + yy ) */
53 }
54 icvDerProductEx;
55
56 /*F///////////////////////////////////////////////////////////////////////////////////////
57 // Name: icvCalcOpticalFlowHS_8u32fR (Horn & Schunck method )
58 // Purpose: calculate Optical flow for 2 images using Horn & Schunck algorithm
59 // Context:
60 // Parameters:
61 // imgA - pointer to first frame ROI
62 // imgB - pointer to second frame ROI
63 // imgStep - width of single row of source images in bytes
64 // imgSize - size of the source image ROI
65 // usePrevious - use previous (input) velocity field.
66 // velocityX - pointer to horizontal and
67 // velocityY - vertical components of optical flow ROI
68 // velStep - width of single row of velocity frames in bytes
69 // lambda - Lagrangian multiplier
70 // criteria - criteria of termination processmaximum number of iterations
71 //
72 // Returns: CV_OK - all ok
73 // CV_OUTOFMEM_ERR - insufficient memory for function work
74 // CV_NULLPTR_ERR - if one of input pointers is NULL
75 // CV_BADSIZE_ERR - wrong input sizes interrelation
76 //
77 // Notes: 1.Optical flow to be computed for every pixel in ROI
78 // 2.For calculating spatial derivatives we use 3x3 Sobel operator.
79 // 3.We use the following border mode.
80 // The last row or column is replicated for the border
81 // ( IPL_BORDER_REPLICATE in IPL ).
82 //
83 //
84 //F*/
85 static CvStatus CV_STDCALL
icvCalcOpticalFlowHS_8u32fR(uchar * imgA,uchar * imgB,int imgStep,CvSize imgSize,int usePrevious,float * velocityX,float * velocityY,int velStep,float lambda,CvTermCriteria criteria)86 icvCalcOpticalFlowHS_8u32fR( uchar* imgA,
87 uchar* imgB,
88 int imgStep,
89 CvSize imgSize,
90 int usePrevious,
91 float* velocityX,
92 float* velocityY,
93 int velStep,
94 float lambda,
95 CvTermCriteria criteria )
96 {
97 /* Loops indexes */
98 int i, j, k, address;
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 imageWidth = imgSize.width;
108 int imageHeight = imgSize.height;
109
110 int ConvLine;
111 int LastLine;
112
113 int BufferSize;
114
115 float Ilambda = 1 / lambda;
116 int iter = 0;
117 int Stop;
118
119 /* buffers derivatives product */
120 icvDerProductEx *II;
121
122 float *VelBufX[2];
123 float *VelBufY[2];
124
125 /* variables for storing number of first pixel of image line */
126 int Line1;
127 int Line2;
128 int Line3;
129
130 int pixNumber;
131
132 /* auxiliary */
133 int NoMem = 0;
134
135 /* Checking bad arguments */
136 if( imgA == NULL )
137 return CV_NULLPTR_ERR;
138 if( imgB == NULL )
139 return CV_NULLPTR_ERR;
140
141 if( imgSize.width <= 0 )
142 return CV_BADSIZE_ERR;
143 if( imgSize.height <= 0 )
144 return CV_BADSIZE_ERR;
145 if( imgSize.width > imgStep )
146 return CV_BADSIZE_ERR;
147
148 if( (velStep & 3) != 0 )
149 return CV_BADSIZE_ERR;
150
151 velStep /= 4;
152
153 /****************************************************************************************/
154 /* Allocating memory for all buffers */
155 /****************************************************************************************/
156 for( k = 0; k < 2; k++ )
157 {
158 MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float ));
159
160 if( MemX[k] == NULL )
161 NoMem = 1;
162 MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float ));
163
164 if( MemY[k] == NULL )
165 NoMem = 1;
166
167 VelBufX[k] = (float *) cvAlloc( imageWidth * sizeof( float ));
168
169 if( VelBufX[k] == NULL )
170 NoMem = 1;
171 VelBufY[k] = (float *) cvAlloc( imageWidth * sizeof( float ));
172
173 if( VelBufY[k] == NULL )
174 NoMem = 1;
175 }
176
177 BufferSize = imageHeight * imageWidth;
178
179 II = (icvDerProductEx *) cvAlloc( BufferSize * sizeof( icvDerProductEx ));
180 if( (II == NULL) )
181 NoMem = 1;
182
183 if( NoMem )
184 {
185 for( k = 0; k < 2; k++ )
186 {
187 if( MemX[k] )
188 cvFree( &MemX[k] );
189
190 if( MemY[k] )
191 cvFree( &MemY[k] );
192
193 if( VelBufX[k] )
194 cvFree( &VelBufX[k] );
195
196 if( VelBufY[k] )
197 cvFree( &VelBufY[k] );
198 }
199 if( II )
200 cvFree( &II );
201 return CV_OUTOFMEM_ERR;
202 }
203 /****************************************************************************************\
204 * Calculate first line of memX and memY *
205 \****************************************************************************************/
206 MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] );
207 MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] );
208
209 for( j = 1; j < imageWidth - 1; j++ )
210 {
211 MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] );
212 }
213
214 pixNumber = imgStep;
215 for( i = 1; i < imageHeight - 1; i++ )
216 {
217 MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep],
218 imgA[pixNumber], imgA[pixNumber + imgStep] );
219 pixNumber += imgStep;
220 }
221
222 MemY[0][imageWidth - 1] =
223 MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2],
224 imgA[imageWidth - 1], imgA[imageWidth - 1] );
225
226 MemX[0][imageHeight - 1] =
227 MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep],
228 imgA[pixNumber], imgA[pixNumber] );
229
230
231 /****************************************************************************************\
232 * begin scan image, calc derivatives *
233 \****************************************************************************************/
234
235 ConvLine = 0;
236 Line2 = -imgStep;
237 address = 0;
238 LastLine = imgStep * (imageHeight - 1);
239 while( ConvLine < imageHeight )
240 {
241 /*Here we calculate derivatives for line of image */
242 int memYline = (ConvLine + 1) & 1;
243
244 Line2 += imgStep;
245 Line1 = Line2 - ((Line2 == 0) ? 0 : imgStep);
246 Line3 = Line2 + ((Line2 == LastLine) ? 0 : imgStep);
247
248 /* Process first pixel */
249 ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] );
250 ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] );
251
252 GradY = (ConvY - MemY[memYline][0]) * 0.125f;
253 GradX = (ConvX - MemX[1][ConvLine]) * 0.125f;
254
255 MemY[memYline][0] = ConvY;
256 MemX[1][ConvLine] = ConvX;
257
258 GradT = (float) (imgB[Line2] - imgA[Line2]);
259
260 II[address].xx = GradX * GradX;
261 II[address].xy = GradX * GradY;
262 II[address].yy = GradY * GradY;
263 II[address].xt = GradX * GradT;
264 II[address].yt = GradY * GradT;
265
266 II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy);
267 address++;
268
269 /* Process middle of line */
270 for( j = 1; j < imageWidth - 1; j++ )
271 {
272 ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] );
273 ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] );
274
275 GradY = (ConvY - MemY[memYline][j]) * 0.125f;
276 GradX = (ConvX - MemX[(j - 1) & 1][ConvLine]) * 0.125f;
277
278 MemY[memYline][j] = ConvY;
279 MemX[(j - 1) & 1][ConvLine] = ConvX;
280
281 GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]);
282
283 II[address].xx = GradX * GradX;
284 II[address].xy = GradX * GradY;
285 II[address].yy = GradY * GradY;
286 II[address].xt = GradX * GradT;
287 II[address].yt = GradY * GradT;
288
289 II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy);
290 address++;
291 }
292 /* Process last pixel of line */
293 ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1],
294 imgA[Line3 + imageWidth - 1] );
295
296 ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1],
297 imgA[Line3 + imageWidth - 1] );
298
299
300 GradY = (ConvY - MemY[memYline][imageWidth - 1]) * 0.125f;
301 GradX = (ConvX - MemX[(imageWidth - 2) & 1][ConvLine]) * 0.125f;
302
303 MemY[memYline][imageWidth - 1] = ConvY;
304
305 GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]);
306
307 II[address].xx = GradX * GradX;
308 II[address].xy = GradX * GradY;
309 II[address].yy = GradY * GradY;
310 II[address].xt = GradX * GradT;
311 II[address].yt = GradY * GradT;
312
313 II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy);
314 address++;
315
316 ConvLine++;
317 }
318 /****************************************************************************************\
319 * Prepare initial approximation *
320 \****************************************************************************************/
321 if( !usePrevious )
322 {
323 float *vx = velocityX;
324 float *vy = velocityY;
325
326 for( i = 0; i < imageHeight; i++ )
327 {
328 memset( vx, 0, imageWidth * sizeof( float ));
329 memset( vy, 0, imageWidth * sizeof( float ));
330
331 vx += velStep;
332 vy += velStep;
333 }
334 }
335 /****************************************************************************************\
336 * Perform iterations *
337 \****************************************************************************************/
338 iter = 0;
339 Stop = 0;
340 LastLine = velStep * (imageHeight - 1);
341 while( !Stop )
342 {
343 float Eps = 0;
344 address = 0;
345
346 iter++;
347 /****************************************************************************************\
348 * begin scan velocity and update it *
349 \****************************************************************************************/
350 Line2 = -velStep;
351 for( i = 0; i < imageHeight; i++ )
352 {
353 /* Here average velocity */
354
355 float averageX;
356 float averageY;
357 float tmp;
358
359 Line2 += velStep;
360 Line1 = Line2 - ((Line2 == 0) ? 0 : velStep);
361 Line3 = Line2 + ((Line2 == LastLine) ? 0 : velStep);
362 /* Process first pixel */
363 averageX = (velocityX[Line2] +
364 velocityX[Line2 + 1] + velocityX[Line1] + velocityX[Line3]) / 4;
365
366 averageY = (velocityY[Line2] +
367 velocityY[Line2 + 1] + velocityY[Line1] + velocityY[Line3]) / 4;
368
369 VelBufX[i & 1][0] = averageX -
370 (II[address].xx * averageX +
371 II[address].xy * averageY + II[address].xt) * II[address].alpha;
372
373 VelBufY[i & 1][0] = averageY -
374 (II[address].xy * averageX +
375 II[address].yy * averageY + II[address].yt) * II[address].alpha;
376
377 /* update Epsilon */
378 if( criteria.type & CV_TERMCRIT_EPS )
379 {
380 tmp = (float)fabs(velocityX[Line2] - VelBufX[i & 1][0]);
381 Eps = MAX( tmp, Eps );
382 tmp = (float)fabs(velocityY[Line2] - VelBufY[i & 1][0]);
383 Eps = MAX( tmp, Eps );
384 }
385 address++;
386 /* Process middle of line */
387 for( j = 1; j < imageWidth - 1; j++ )
388 {
389 averageX = (velocityX[Line2 + j - 1] +
390 velocityX[Line2 + j + 1] +
391 velocityX[Line1 + j] + velocityX[Line3 + j]) / 4;
392 averageY = (velocityY[Line2 + j - 1] +
393 velocityY[Line2 + j + 1] +
394 velocityY[Line1 + j] + velocityY[Line3 + j]) / 4;
395
396 VelBufX[i & 1][j] = averageX -
397 (II[address].xx * averageX +
398 II[address].xy * averageY + II[address].xt) * II[address].alpha;
399
400 VelBufY[i & 1][j] = averageY -
401 (II[address].xy * averageX +
402 II[address].yy * averageY + II[address].yt) * II[address].alpha;
403 /* update Epsilon */
404 if( criteria.type & CV_TERMCRIT_EPS )
405 {
406 tmp = (float)fabs(velocityX[Line2 + j] - VelBufX[i & 1][j]);
407 Eps = MAX( tmp, Eps );
408 tmp = (float)fabs(velocityY[Line2 + j] - VelBufY[i & 1][j]);
409 Eps = MAX( tmp, Eps );
410 }
411 address++;
412 }
413 /* Process last pixel of line */
414 averageX = (velocityX[Line2 + imageWidth - 2] +
415 velocityX[Line2 + imageWidth - 1] +
416 velocityX[Line1 + imageWidth - 1] +
417 velocityX[Line3 + imageWidth - 1]) / 4;
418
419 averageY = (velocityY[Line2 + imageWidth - 2] +
420 velocityY[Line2 + imageWidth - 1] +
421 velocityY[Line1 + imageWidth - 1] +
422 velocityY[Line3 + imageWidth - 1]) / 4;
423
424
425 VelBufX[i & 1][imageWidth - 1] = averageX -
426 (II[address].xx * averageX +
427 II[address].xy * averageY + II[address].xt) * II[address].alpha;
428
429 VelBufY[i & 1][imageWidth - 1] = averageY -
430 (II[address].xy * averageX +
431 II[address].yy * averageY + II[address].yt) * II[address].alpha;
432
433 /* update Epsilon */
434 if( criteria.type & CV_TERMCRIT_EPS )
435 {
436 tmp = (float)fabs(velocityX[Line2 + imageWidth - 1] -
437 VelBufX[i & 1][imageWidth - 1]);
438 Eps = MAX( tmp, Eps );
439 tmp = (float)fabs(velocityY[Line2 + imageWidth - 1] -
440 VelBufY[i & 1][imageWidth - 1]);
441 Eps = MAX( tmp, Eps );
442 }
443 address++;
444
445 /* store new velocity from old buffer to velocity frame */
446 if( i > 0 )
447 {
448 memcpy( &velocityX[Line1], VelBufX[(i - 1) & 1], imageWidth * sizeof( float ));
449 memcpy( &velocityY[Line1], VelBufY[(i - 1) & 1], imageWidth * sizeof( float ));
450 }
451 } /*for */
452 /* store new velocity from old buffer to velocity frame */
453 memcpy( &velocityX[imageWidth * (imageHeight - 1)],
454 VelBufX[(imageHeight - 1) & 1], imageWidth * sizeof( float ));
455
456 memcpy( &velocityY[imageWidth * (imageHeight - 1)],
457 VelBufY[(imageHeight - 1) & 1], imageWidth * sizeof( float ));
458
459 if( (criteria.type & CV_TERMCRIT_ITER) && (iter == criteria.max_iter) )
460 Stop = 1;
461 if( (criteria.type & CV_TERMCRIT_EPS) && (Eps < criteria.epsilon) )
462 Stop = 1;
463 }
464 /* Free memory */
465 for( k = 0; k < 2; k++ )
466 {
467 cvFree( &MemX[k] );
468 cvFree( &MemY[k] );
469 cvFree( &VelBufX[k] );
470 cvFree( &VelBufY[k] );
471 }
472 cvFree( &II );
473
474 return CV_OK;
475 } /*icvCalcOpticalFlowHS_8u32fR*/
476
477
478 /*F///////////////////////////////////////////////////////////////////////////////////////
479 // Name: cvCalcOpticalFlowHS
480 // Purpose: Optical flow implementation
481 // Context:
482 // Parameters:
483 // srcA, srcB - source image
484 // velx, vely - destination image
485 // Returns:
486 //
487 // Notes:
488 //F*/
489 CV_IMPL void
cvCalcOpticalFlowHS(const void * srcarrA,const void * srcarrB,int usePrevious,void * velarrx,void * velarry,double lambda,CvTermCriteria criteria)490 cvCalcOpticalFlowHS( const void* srcarrA, const void* srcarrB, int usePrevious,
491 void* velarrx, void* velarry,
492 double lambda, CvTermCriteria criteria )
493 {
494 CV_FUNCNAME( "cvCalcOpticalFlowHS" );
495
496 __BEGIN__;
497
498 CvMat stubA, *srcA = (CvMat*)srcarrA;
499 CvMat stubB, *srcB = (CvMat*)srcarrB;
500 CvMat stubx, *velx = (CvMat*)velarrx;
501 CvMat stuby, *vely = (CvMat*)velarry;
502
503 CV_CALL( srcA = cvGetMat( srcA, &stubA ));
504 CV_CALL( srcB = cvGetMat( srcB, &stubB ));
505
506 CV_CALL( velx = cvGetMat( velx, &stubx ));
507 CV_CALL( vely = cvGetMat( vely, &stuby ));
508
509 if( !CV_ARE_TYPES_EQ( srcA, srcB ))
510 CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" );
511
512 if( !CV_ARE_TYPES_EQ( velx, vely ))
513 CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );
514
515 if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
516 !CV_ARE_SIZES_EQ( velx, vely ) ||
517 !CV_ARE_SIZES_EQ( srcA, velx ))
518 CV_ERROR( CV_StsUnmatchedSizes, "" );
519
520 if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
521 CV_MAT_TYPE( velx->type ) != CV_32FC1 )
522 CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
523 "destination images must have 32fC1 type" );
524
525 if( srcA->step != srcB->step || velx->step != vely->step )
526 CV_ERROR( CV_BadStep, "source and destination images have different step" );
527
528 IPPI_CALL( icvCalcOpticalFlowHS_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
529 srcA->step, cvGetMatSize( srcA ), usePrevious,
530 velx->data.fl, vely->data.fl,
531 velx->step, (float)lambda, criteria ));
532 __END__;
533 }
534
535 /* End of file. */
536