git.maemo.org Git - opencv/blob - cv/src/cvoptflowhs.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 #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. */