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 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 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
