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 43typedef struct 44{ 45 float xx; 46 float xy; 47 float yy; 48 float xt; 49 float yt; 50} 51icvDerProduct; 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*/ 82static CvStatus CV_STDCALL 83icvCalcOpticalFlowLK_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*/ 566CV_IMPL void 567cvCalcOpticalFlowLK( 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