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
42 #include "_cv.h"
43
44 /*
45 Finds L1 norm between two blocks.
46 */
47 static int
icvCmpBlocksL1_8u_C1(const uchar * vec1,const uchar * vec2,int len)48 icvCmpBlocksL1_8u_C1( const uchar * vec1, const uchar * vec2, int len )
49 {
50 int i, sum = 0;
51
52 for( i = 0; i <= len - 4; i += 4 )
53 {
54 int t0 = abs(vec1[i] - vec2[i]);
55 int t1 = abs(vec1[i + 1] - vec2[i + 1]);
56 int t2 = abs(vec1[i + 2] - vec2[i + 2]);
57 int t3 = abs(vec1[i + 3] - vec2[i + 3]);
58
59 sum += t0 + t1 + t2 + t3;
60 }
61
62 for( ; i < len; i++ )
63 {
64 int t0 = abs(vec1[i] - vec2[i]);
65 sum += t0;
66 }
67
68 return sum;
69 }
70
71
72 static void
icvCopyBM_8u_C1R(const uchar * src,int src_step,uchar * dst,int dst_step,CvSize size)73 icvCopyBM_8u_C1R( const uchar* src, int src_step,
74 uchar* dst, int dst_step, CvSize size )
75 {
76 for( ; size.height--; src += src_step, dst += dst_step )
77 memcpy( dst, src, size.width );
78 }
79
80
81 /*F///////////////////////////////////////////////////////////////////////////////////////
82 // Name: icvCalcOpticalFlowBM_8u32fR
83 // Purpose: calculate Optical flow for 2 images using block matching algorithm
84 // Context:
85 // Parameters:
86 // imgA, // pointer to first frame ROI
87 // imgB, // pointer to second frame ROI
88 // imgStep, // full width of input images in bytes
89 // imgSize, // size of the image
90 // blockSize, // size of basic blocks which are compared
91 // shiftSize, // coordinates increments.
92 // maxRange, // size of the scanned neighborhood.
93 // usePrevious, // flag of using previous velocity field
94 // velocityX, // pointer to ROI of horizontal and
95 // velocityY, // vertical components of optical flow
96 // velStep); // full width of velocity frames in bytes
97 // Returns: CV_OK or error code
98 // Notes:
99 //F*/
100 #define SMALL_DIFF 2
101 #define BIG_DIFF 128
102
103 static CvStatus CV_STDCALL
icvCalcOpticalFlowBM_8u32fR(uchar * imgA,uchar * imgB,int imgStep,CvSize imgSize,CvSize blockSize,CvSize shiftSize,CvSize maxRange,int usePrev,float * velocityX,float * velocityY,int velStep)104 icvCalcOpticalFlowBM_8u32fR( uchar * imgA, uchar * imgB,
105 int imgStep, CvSize imgSize,
106 CvSize blockSize, CvSize shiftSize,
107 CvSize maxRange, int usePrev,
108 float *velocityX, float *velocityY,
109 int velStep )
110 {
111 const float back = 1.f / (float) (1 << 16);
112
113 /* scanning scheme coordinates */
114
115 CvPoint *ss = 0;
116 int ss_count = 0;
117
118 int stand_accept_level = blockSize.height * blockSize.width * SMALL_DIFF;
119 int stand_escape_level = blockSize.height * blockSize.width * BIG_DIFF;
120
121 int i, j;
122
123 int *int_velocityX = (int *) velocityX;
124 int *int_velocityY = (int *) velocityY;
125
126 /* if image sizes can't be divided by block sizes then right blocks will */
127 /* have not full width - BorderWidth */
128 /* and bottom blocks will */
129 /* have not full height - BorderHeight */
130 int BorderWidth;
131 int BorderHeight;
132
133 int CurrentWidth;
134 int CurrentHeight;
135
136 int NumberBlocksX;
137 int NumberBlocksY;
138
139 int Y1 = 0;
140 int X1 = 0;
141
142 int DownStep = blockSize.height * imgStep;
143
144 uchar *blockA = 0;
145 uchar *blockB = 0;
146 uchar *blockZ = 0;
147 int blSize = blockSize.width * blockSize.height;
148 int bufferSize = cvAlign(blSize + 9,16);
149 int cmpSize = cvAlign(blSize,4);
150 int patch_ofs = blSize & -8;
151 int64 patch_mask = (((int64) 1) << (blSize - patch_ofs * 8)) - 1;
152
153 velStep /= sizeof(velocityX[0]);
154
155 if( patch_ofs == blSize )
156 patch_mask = (int64) - 1;
157
158 /****************************************************************************************\
159 * Checking bad arguments *
160 \****************************************************************************************/
161 if( imgA == NULL )
162 return CV_NULLPTR_ERR;
163 if( imgB == NULL )
164 return CV_NULLPTR_ERR;
165
166 /****************************************************************************************\
167 * Allocate buffers *
168 \****************************************************************************************/
169 blockA = (uchar *) cvAlloc( bufferSize * 3 );
170 if( !blockA )
171 return CV_OUTOFMEM_ERR;
172
173 blockB = blockA + bufferSize;
174 blockZ = blockB + bufferSize;
175
176 memset( blockZ, 0, bufferSize );
177
178 ss = (CvPoint *) cvAlloc( (2 * maxRange.width + 1) * (2 * maxRange.height + 1) *
179 sizeof( CvPoint ));
180 if( !ss )
181 {
182 cvFree( &blockA );
183 return CV_OUTOFMEM_ERR;
184 }
185
186 /****************************************************************************************\
187 * Calculate scanning scheme *
188 \****************************************************************************************/
189 {
190 int X_shift_count = maxRange.width / shiftSize.width;
191 int Y_shift_count = maxRange.height / shiftSize.height;
192 int min_count = MIN( X_shift_count, Y_shift_count );
193
194 /* cycle by neighborhood rings */
195 /* scanning scheme is
196
197 . 9 10 11 12
198 . 8 1 2 13
199 . 7 * 3 14
200 . 6 5 4 15
201 20 19 18 17 16
202 */
203
204 for( i = 0; i < min_count; i++ )
205 {
206 /* four cycles along sides */
207 int y = -(i + 1) * shiftSize.height;
208 int x = -(i + 1) * shiftSize.width;
209
210 /* upper side */
211 for( j = -i; j <= i + 1; j++, ss_count++ )
212 {
213 x += shiftSize.width;
214 ss[ss_count].x = x;
215 ss[ss_count].y = y;
216 }
217
218 /* right side */
219 for( j = -i; j <= i + 1; j++, ss_count++ )
220 {
221 y += shiftSize.height;
222 ss[ss_count].x = x;
223 ss[ss_count].y = y;
224 }
225
226 /* bottom side */
227 for( j = -i; j <= i + 1; j++, ss_count++ )
228 {
229 x -= shiftSize.width;
230 ss[ss_count].x = x;
231 ss[ss_count].y = y;
232 }
233
234 /* left side */
235 for( j = -i; j <= i + 1; j++, ss_count++ )
236 {
237 y -= shiftSize.height;
238 ss[ss_count].x = x;
239 ss[ss_count].y = y;
240 }
241 }
242
243 /* the rest part */
244 if( X_shift_count < Y_shift_count )
245 {
246 int xleft = -min_count * shiftSize.width;
247
248 /* cycle by neighbor rings */
249 for( i = min_count; i < Y_shift_count; i++ )
250 {
251 /* two cycles by x */
252 int y = -(i + 1) * shiftSize.height;
253 int x = xleft;
254
255 /* upper side */
256 for( j = -X_shift_count; j <= X_shift_count; j++, ss_count++ )
257 {
258 ss[ss_count].x = x;
259 ss[ss_count].y = y;
260 x += shiftSize.width;
261 }
262
263 x = xleft;
264 y = -y;
265 /* bottom side */
266 for( j = -X_shift_count; j <= X_shift_count; j++, ss_count++ )
267 {
268 ss[ss_count].x = x;
269 ss[ss_count].y = y;
270 x += shiftSize.width;
271 }
272 }
273 }
274 else if( X_shift_count > Y_shift_count )
275 {
276 int yupper = -min_count * shiftSize.height;
277
278 /* cycle by neighbor rings */
279 for( i = min_count; i < X_shift_count; i++ )
280 {
281 /* two cycles by y */
282 int x = -(i + 1) * shiftSize.width;
283 int y = yupper;
284
285 /* left side */
286 for( j = -Y_shift_count; j <= Y_shift_count; j++, ss_count++ )
287 {
288 ss[ss_count].x = x;
289 ss[ss_count].y = y;
290 y += shiftSize.height;
291 }
292
293 y = yupper;
294 x = -x;
295 /* right side */
296 for( j = -Y_shift_count; j <= Y_shift_count; j++, ss_count++ )
297 {
298 ss[ss_count].x = x;
299 ss[ss_count].y = y;
300 y += shiftSize.height;
301 }
302 }
303 }
304
305 }
306
307 /****************************************************************************************\
308 * Calculate some neeeded variables *
309 \****************************************************************************************/
310 /* Calculate number of full blocks */
311 NumberBlocksX = (int) imgSize.width / blockSize.width;
312 NumberBlocksY = (int) imgSize.height / blockSize.height;
313
314 /* add 1 if not full border blocks exist */
315 BorderWidth = imgSize.width % blockSize.width;
316 if( BorderWidth )
317 NumberBlocksX++;
318 else
319 BorderWidth = blockSize.width;
320
321 BorderHeight = imgSize.height % blockSize.height;
322 if( BorderHeight )
323 NumberBlocksY++;
324 else
325 BorderHeight = blockSize.height;
326
327 /****************************************************************************************\
328 * Round input velocities integer searching area center position *
329 \****************************************************************************************/
330 if( usePrev )
331 {
332 float *velxf = velocityX, *velyf = velocityY;
333 int* velx = (int*)velocityX, *vely = (int*)velocityY;
334
335 for( i = 0; i < NumberBlocksY; i++, velxf += velStep, velyf += velStep,
336 velx += velStep, vely += velStep )
337 {
338 for( j = 0; j < NumberBlocksX; j++ )
339 {
340 int vx = cvRound( velxf[j] ), vy = cvRound( velyf[j] );
341 velx[j] = vx; vely[j] = vy;
342 }
343 }
344 }
345 /****************************************************************************************\
346 * Main loop *
347 \****************************************************************************************/
348 Y1 = 0;
349 for( i = 0; i < NumberBlocksY; i++ )
350 {
351 /* calculate height of current block */
352 CurrentHeight = (i == NumberBlocksY - 1) ? BorderHeight : blockSize.height;
353 X1 = 0;
354
355 for( j = 0; j < NumberBlocksX; j++ )
356 {
357 int accept_level;
358 int escape_level;
359
360 int blDist;
361
362 int VelocityX = 0;
363 int VelocityY = 0;
364
365 int offX = 0, offY = 0;
366
367 int CountDirection = 1;
368
369 int main_flag = i < NumberBlocksY - 1 && j < NumberBlocksX - 1;
370 CvSize CurSize;
371
372 /* calculate width of current block */
373 CurrentWidth = (j == NumberBlocksX - 1) ? BorderWidth : blockSize.width;
374
375 /* compute initial offset */
376 if( usePrev )
377 {
378 offX = int_velocityX[j];
379 offY = int_velocityY[j];
380 }
381
382 CurSize.width = CurrentWidth;
383 CurSize.height = CurrentHeight;
384
385 if( main_flag )
386 {
387 icvCopyBM_8u_C1R( imgA + X1, imgStep, blockA,
388 CurSize.width, CurSize );
389 icvCopyBM_8u_C1R( imgB + (Y1 + offY)*imgStep + (X1 + offX),
390 imgStep, blockB, CurSize.width, CurSize );
391
392 *((int64 *) (blockA + patch_ofs)) &= patch_mask;
393 *((int64 *) (blockB + patch_ofs)) &= patch_mask;
394 }
395 else
396 {
397 memset( blockA, 0, bufferSize );
398 memset( blockB, 0, bufferSize );
399
400 icvCopyBM_8u_C1R( imgA + X1, imgStep, blockA, blockSize.width, CurSize );
401 icvCopyBM_8u_C1R( imgB + (Y1 + offY) * imgStep + (X1 + offX), imgStep,
402 blockB, blockSize.width, CurSize );
403 }
404
405 if( !main_flag )
406 {
407 int tmp = CurSize.width * CurSize.height;
408
409 accept_level = tmp * SMALL_DIFF;
410 escape_level = tmp * BIG_DIFF;
411 }
412 else
413 {
414 accept_level = stand_accept_level;
415 escape_level = stand_escape_level;
416 }
417
418 blDist = icvCmpBlocksL1_8u_C1( blockA, blockB, cmpSize );
419
420 if( blDist > accept_level )
421 {
422 int k;
423 int VelX = 0;
424 int VelY = 0;
425
426 /* walk around basic block */
427
428 /* cycle for neighborhood */
429 for( k = 0; k < ss_count; k++ )
430 {
431 int tmpDist;
432
433 int Y2 = Y1 + offY + ss[k].y;
434 int X2 = X1 + offX + ss[k].x;
435
436 /* if we break upper border */
437 if( Y2 < 0 )
438 {
439 continue;
440 }
441 /* if we break bottom border */
442 if( Y2 + CurrentHeight >= imgSize.height )
443 {
444 continue;
445 }
446 /* if we break left border */
447 if( X2 < 0 )
448 {
449 continue;
450 }
451 /* if we break right border */
452 if( X2 + CurrentWidth >= imgSize.width )
453 {
454 continue;
455 }
456
457 if( main_flag )
458 {
459 icvCopyBM_8u_C1R( imgB + Y2 * imgStep + X2,
460 imgStep, blockB, CurSize.width, CurSize );
461
462 *((int64 *) (blockB + patch_ofs)) &= patch_mask;
463 }
464 else
465 {
466 memset( blockB, 0, bufferSize );
467 icvCopyBM_8u_C1R( imgB + Y1 * imgStep + X1, imgStep,
468 blockB, blockSize.width, CurSize );
469 }
470
471 tmpDist = icvCmpBlocksL1_8u_C1( blockA, blockB, cmpSize );
472
473 if( tmpDist < accept_level )
474 {
475 VelX = ss[k].x;
476 VelY = ss[k].y;
477 break; /*for */
478 }
479 else if( tmpDist < blDist )
480 {
481 blDist = tmpDist;
482 VelX = ss[k].x;
483 VelY = ss[k].y;
484 CountDirection = 1;
485 }
486 else if( tmpDist == blDist )
487 {
488 VelX += ss[k].x;
489 VelY += ss[k].y;
490 CountDirection++;
491 }
492 }
493 if( blDist > escape_level )
494 {
495 VelX = VelY = 0;
496 CountDirection = 1;
497 }
498 if( CountDirection > 1 )
499 {
500 int temp = CountDirection == 2 ? 1 << 15 : ((1 << 16) / CountDirection);
501
502 VelocityX = VelX * temp;
503 VelocityY = VelY * temp;
504 }
505 else
506 {
507 VelocityX = VelX << 16;
508 VelocityY = VelY << 16;
509 }
510 } /*if */
511
512 int_velocityX[j] = VelocityX + (offX << 16);
513 int_velocityY[j] = VelocityY + (offY << 16);
514
515 X1 += blockSize.width;
516
517 } /*for */
518 int_velocityX += velStep;
519 int_velocityY += velStep;
520
521 imgA += DownStep;
522 Y1 += blockSize.height;
523 } /*for */
524
525 /****************************************************************************************\
526 * Converting fixed point velocities to floating point *
527 \****************************************************************************************/
528 {
529 float *velxf = velocityX, *velyf = velocityY;
530 int* velx = (int*)velocityX, *vely = (int*)velocityY;
531
532 for( i = 0; i < NumberBlocksY; i++, velxf += velStep, velyf += velStep,
533 velx += velStep, vely += velStep )
534 {
535 for( j = 0; j < NumberBlocksX; j++ )
536 {
537 float vx = (float)velx[j]*back, vy = (float)vely[j]*back;
538 velxf[j] = vx; velyf[j] = vy;
539 }
540 }
541 }
542
543 cvFree( &ss );
544 cvFree( &blockA );
545
546 return CV_OK;
547 } /*cvCalcOpticalFlowBM_8u */
548
549
550 /*F///////////////////////////////////////////////////////////////////////////////////////
551 // Name: cvCalcOpticalFlowBM
552 // Purpose: Optical flow implementation
553 // Context:
554 // Parameters:
555 // srcA, srcB - source image
556 // velx, vely - destination image
557 // Returns:
558 //
559 // Notes:
560 //F*/
561 CV_IMPL void
cvCalcOpticalFlowBM(const void * srcarrA,const void * srcarrB,CvSize blockSize,CvSize shiftSize,CvSize maxRange,int usePrevious,void * velarrx,void * velarry)562 cvCalcOpticalFlowBM( const void* srcarrA, const void* srcarrB,
563 CvSize blockSize, CvSize shiftSize,
564 CvSize maxRange, int usePrevious,
565 void* velarrx, void* velarry )
566 {
567 CV_FUNCNAME( "cvCalcOpticalFlowBM" );
568
569 __BEGIN__;
570
571 CvMat stubA, *srcA = (CvMat*)srcarrA;
572 CvMat stubB, *srcB = (CvMat*)srcarrB;
573 CvMat stubx, *velx = (CvMat*)velarrx;
574 CvMat stuby, *vely = (CvMat*)velarry;
575
576 CV_CALL( srcA = cvGetMat( srcA, &stubA ));
577 CV_CALL( srcB = cvGetMat( srcB, &stubB ));
578
579 CV_CALL( velx = cvGetMat( velx, &stubx ));
580 CV_CALL( vely = cvGetMat( vely, &stuby ));
581
582 if( !CV_ARE_TYPES_EQ( srcA, srcB ))
583 CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" );
584
585 if( !CV_ARE_TYPES_EQ( velx, vely ))
586 CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );
587
588 if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
589 !CV_ARE_SIZES_EQ( velx, vely ) ||
590 (unsigned)(velx->width*blockSize.width - srcA->width) >= (unsigned)blockSize.width ||
591 (unsigned)(velx->height*blockSize.height - srcA->height) >= (unsigned)blockSize.height )
592 CV_ERROR( CV_StsUnmatchedSizes, "" );
593
594 if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
595 CV_MAT_TYPE( velx->type ) != CV_32FC1 )
596 CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
597 "destination images must have 32fC1 type" );
598
599 if( srcA->step != srcB->step || velx->step != vely->step )
600 CV_ERROR( CV_BadStep, "two source or two destination images have different steps" );
601
602 IPPI_CALL( icvCalcOpticalFlowBM_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
603 srcA->step, cvGetMatSize( srcA ), blockSize,
604 shiftSize, maxRange, usePrevious,
605 velx->data.fl, vely->data.fl, velx->step ));
606 __END__;
607 }
608
609
610 /* End of file. */
611