git.maemo.org Git - opencv/blob - cv/src/cvoptflowbm.cpp

   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
  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
  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
 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
 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. */