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*/ 47static int 48icvCmpBlocksL1_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 72static void 73icvCopyBM_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 103static CvStatus CV_STDCALL 104icvCalcOpticalFlowBM_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*/ 561CV_IMPL void 562cvCalcOpticalFlowBM( 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