git.maemo.org Git - opencv/blob - cv/src/cvoptflowlk.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 #include "_cv.h"
  42
  43 typedef struct
  44 {
  45     float xx;
  46     float xy;
  47     float yy;
  48     float xt;
  49     float yt;
  50 }
  51 icvDerProduct;
  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*/
  82 static CvStatus CV_STDCALL
  83 icvCalcOpticalFlowLK_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*/
 566 CV_IMPL void
 567 cvCalcOpticalFlowLK( 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. */