opencv/src/cv/cvlkpyramid.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 #include <float.h>
  43
  44 #if 1
  45 static void
  46 intersect( CvPoint2D32f pt, CvSize win_size, CvSize img_size,
  47            CvPoint * min_pt, CvPoint * max_pt )
  48 {
  49     CvPoint ipt;
  50
  51     ipt.x = cvFloor( pt.x );
  52     ipt.y = cvFloor( pt.y );
  53
  54     ipt.x -= win_size.width;
  55     ipt.y -= win_size.height;
  56
  57     win_size.width = win_size.width * 2 + 1;
  58     win_size.height = win_size.height * 2 + 1;
  59
  60     min_pt->x = MAX( 0, -ipt.x );
  61     min_pt->y = MAX( 0, -ipt.y );
  62     max_pt->x = MIN( win_size.width, img_size.width - ipt.x );
  63     max_pt->y = MIN( win_size.height, img_size.height - ipt.y );
  64 }
  65 #endif
  66
  67 static CvStatus
  68 icvInitPyramidalAlgorithm( uchar * imgA, uchar * imgB,
  69                            int imgStep, CvSize imgSize,
  70                            uchar * pyrA, uchar * pyrB,
  71                            int level,
  72                            CvTermCriteria * criteria,
  73                            int max_iters, int flags,
  74                            uchar *** imgI, uchar *** imgJ,
  75                            int **step, CvSize** size,
  76                            double **scale, uchar ** buffer )
  77 {
  78     uchar *pyr_down_temp_buffer = 0;
  79     CvStatus result = CV_OK;
  80     int pyrBytes, bufferBytes = 0;
  81     int level1 = level + 1;
  82
  83     int i;
  84     CvSize levelSize;
  85
  86     *buffer = 0;
  87     *imgI = *imgJ = 0;
  88     *step = 0;
  89     *scale = 0;
  90     *size = 0;
  91
  92     /* check input arguments */
  93     if( !imgA || !imgB )
  94         return CV_NULLPTR_ERR;
  95
  96     if( (flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA ||
  97         (flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB )
  98         return CV_BADFLAG_ERR;
  99
 100     if( level < 0 )
 101         return CV_BADRANGE_ERR;
 102
 103     /*if( imgSize.width % (1 << level) != 0 ||
 104         imgSize.height % (1 << level) != 0 ||
 105         (imgSize.width >> level) == 0 || (imgSize.height >> level) == 0 )
 106         return CV_BADSIZE_ERR;*/
 107
 108     switch (criteria->type)
 109     {
 110     case CV_TERMCRIT_ITER:
 111         criteria->epsilon = 0.f;
 112         break;
 113     case CV_TERMCRIT_EPS:
 114         criteria->maxIter = max_iters;
 115         break;
 116     case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
 117         break;
 118     default:
 119         assert( 0 );
 120         return CV_BADFLAG_ERR;
 121     }
 122
 123     /* compare squared values */
 124     criteria->epsilon *= criteria->epsilon;
 125
 126     /* set pointers and step for every level */
 127     pyrBytes = 0;
 128
 129 #define ALIGN 8
 130
 131     levelSize = imgSize;
 132
 133     for( i = 1; i < level1; i++ )
 134     {
 135         levelSize.width = (levelSize.width + 1) >> 1;
 136         levelSize.height = (levelSize.height + 1) >> 1;
 137
 138         int tstep = icvAlign(levelSize.width,ALIGN) * sizeof( imgA[0] );
 139         pyrBytes += tstep * levelSize.height;
 140     }
 141
 142     assert( pyrBytes <= imgSize.width * imgSize.height * (int) sizeof( imgA[0] ) * 4 / 3 );
 143
 144     /* buffer_size = <size for patches> + <size for pyramids> */
 145     bufferBytes = (level1 >= 0) * ((pyrA == 0) + (pyrB == 0)) * pyrBytes +
 146         (sizeof( imgI[0][0] ) * 2 + sizeof( step[0][0] ) +
 147          sizeof(size[0][0]) + sizeof( scale[0][0] )) * level1;
 148
 149     *buffer = (uchar *) icvAlloc( bufferBytes );
 150     if( !buffer[0] )
 151         return CV_OUTOFMEM_ERR;
 152
 153     *imgI = (uchar **) buffer[0];
 154     *imgJ = *imgI + level1;
 155     *step = (int *) (*imgJ + level1);
 156     *scale = (double *) (*step + level1);
 157     *size = (CvSize *)(*scale + level1);
 158
 159     imgI[0][0] = imgA;
 160     imgJ[0][0] = imgB;
 161     step[0][0] = imgStep;
 162     scale[0][0] = 1;
 163     size[0][0] = imgSize;
 164
 165     if( level > 0 )
 166     {
 167         uchar *bufPtr = (uchar *) (*size + level1);
 168         uchar *ptrA = pyrA;
 169         uchar *ptrB = pyrB;
 170         int pyr_down_buffer_size = 0;
 171
 172         if( !ptrA )
 173         {
 174             ptrA = bufPtr;
 175             bufPtr += pyrBytes;
 176         }
 177
 178         if( !ptrB )
 179             ptrB = bufPtr;
 180
 181         icvPyrDownGetBufSize_Gauss5x5( imgSize.width, cv8u, 1, &pyr_down_buffer_size );
 182         pyr_down_temp_buffer = (uchar *) icvAlloc( pyr_down_buffer_size );
 183
 184         levelSize = imgSize;
 185
 186         /* build pyramids for both frames */
 187         for( i = 1; i <= level; i++ )
 188         {
 189             int levelBytes;
 190             CvSize srcSize = levelSize;
 191
 192             levelSize.width = (levelSize.width + 1) >> 1;
 193             levelSize.height = (levelSize.height + 1) >> 1;
 194
 195             size[0][i] = levelSize;
 196             step[0][i] = icvAlign( levelSize.width, ALIGN ) * sizeof( imgA[0] );
 197             scale[0][i] = scale[0][i - 1] * 0.5;
 198
 199             levelBytes = step[0][i] * levelSize.height;
 200             imgI[0][i] = (uchar *) ptrA;
 201             ptrA += levelBytes;
 202
 203             srcSize.width &= -2;
 204             srcSize.height &= -2;
 205
 206             if( !(flags & CV_LKFLOW_PYR_A_READY) )
 207             {
 208                 result = icvPyrDown_Gauss5x5_8u_C1R( imgI[0][i - 1], step[0][i - 1],
 209                                                      imgI[0][i], step[0][i],
 210                                                      srcSize, pyr_down_temp_buffer );
 211                 if( result < 0 )
 212                     goto func_exit;
 213                 icvPyrDownBorder_8u_CnR( imgI[0][i - 1], step[0][i - 1], size[0][i-1],
 214                                          imgI[0][i], step[0][i], size[0][i], 1 );
 215             }
 216
 217             imgJ[0][i] = (uchar *) ptrB;
 218             ptrB += levelBytes;
 219
 220             if( !(flags & CV_LKFLOW_PYR_B_READY) )
 221             {
 222                 result = icvPyrDown_Gauss5x5_8u_C1R( imgJ[0][i - 1], step[0][i - 1],
 223                                                      imgJ[0][i], step[0][i],
 224                                                      srcSize, pyr_down_temp_buffer );
 225                 if( result < 0 )
 226                     goto func_exit;
 227                 icvPyrDownBorder_8u_CnR( imgJ[0][i - 1], step[0][i - 1], size[0][i-1],
 228                                          imgJ[0][i], step[0][i], size[0][i], 1 );
 229             }
 230         }
 231     }
 232
 233   func_exit:
 234     icvFree( &pyr_down_temp_buffer );
 235
 236     return CV_OK;
 237 }
 238
 239
 240 /*F///////////////////////////////////////////////////////////////////////////////////////
 241 //    Name: icvCalcOpticalFlowPyrLK_8uC1R ( Lucas & Kanade method,
 242 //                                           modification that uses pyramids )
 243 //    Purpose:
 244 //      Calculates optical flow between two images for certain set of points.
 245 //    Context:
 246 //    Parameters:
 247 //            imgA     - pointer to first frame (time t)
 248 //            imgB     - pointer to second frame (time t+1)
 249 //            imgStep  - full width of the source images in bytes
 250 //            imgSize  - size of the source images
 251 //            pyrA     - buffer for pyramid for the first frame.
 252 //                       if the pointer is not NULL, the buffer must have size enough to
 253 //                       store pyramid (from level 1 to level #<level> (see below))
 254 //                       (imgSize.width*imgSize.height/3 will be enough)).
 255 //            pyrB     - similar to pyrA, but for the second frame.
 256 //
 257 //                       for both parameters above the following rules work:
 258 //                           If pointer is 0, the function allocates the buffer internally,
 259 //                           calculates pyramid and releases the buffer after processing.
 260 //                           Else (it should be large enough then) the function calculates
 261 //                           pyramid and stores it in the buffer unless the
 262 //                           CV_LKFLOW_PYR_A[B]_READY flag is set. In both cases
 263 //                           (flag is set or not) the subsequent calls may reuse the calculated
 264 //                           pyramid by setting CV_LKFLOW_PYR_A[B]_READY.
 265 //
 266 //            featuresA - array of points, for which the flow needs to be found
 267 //            count    - number of feature points
 268 //            winSize  - size of search window on each pyramid level
 269 //            level    - maximal pyramid level number
 270 //                         (if 0, pyramids are not used (single level),
 271 //                          if 1, two levels are used etc.)
 272 //
 273 //            next parameters are arrays of <count> elements.
 274 //            ------------------------------------------------------
 275 //            featuresB - array of 2D points, containing calculated
 276 //                       new positions of input features (in the second image).
 277 //            status   - array, every element of which will be set to 1 if the flow for the
 278 //                       corresponding feature has been found, 0 else.
 279 //            error    - array of double numbers, containing difference between
 280 //                       patches around the original and moved points
 281 //                       (it is optional parameter, can be NULL).
 282 //            ------------------------------------------------------
 283 //            criteria   - specifies when to stop the iteration process of finding flow
 284 //                         for each point on each pyramid level
 285 //
 286 //            flags      - miscellaneous flags:
 287 //                            CV_LKFLOW_PYR_A_READY - pyramid for the first frame
 288 //                                                      is precalculated before call
 289 //                            CV_LKFLOW_PYR_B_READY - pyramid for the second frame
 290 //                                                      is precalculated before call
 291 //                            CV_LKFLOW_INITIAL_GUESSES - featuresB array holds initial
 292 //                                                       guesses about new features'
 293 //                                                       locations before function call.
 294 //    Returns: CV_OK       - all ok
 295 //             CV_OUTOFMEM_ERR - insufficient memory for function work
 296 //             CV_NULLPTR_ERR  - if one of input pointers is NULL
 297 //             CV_BADSIZE_ERR  - wrong input sizes interrelation
 298 //
 299 //    Notes:  For calculating spatial derivatives 3x3 Sobel operator is used.
 300 //            The values of pixels beyond the image are determined using replication mode.
 301 //F*/
 302 static  CvStatus  icvCalcOpticalFlowPyrLK_8uC1R( uchar * imgA,
 303                                                  uchar * imgB,
 304                                                  int imgStep,
 305                                                  CvSize imgSize,
 306                                                  uchar * pyrA,
 307                                                  uchar * pyrB,
 308                                                  CvPoint2D32f * featuresA,
 309                                                  CvPoint2D32f * featuresB,
 310                                                  int count,
 311                                                  CvSize winSize,
 312                                                  int level,
 313                                                  char *status,
 314                                                  float *error,
 315                                                  CvTermCriteria criteria, int flags )
 316 #if 1
 317 {
 318 #define MAX_LEVEL 10
 319 #define MAX_ITERS 100
 320
 321     static const float kerX[] = { -1, 0, 1 }, kerY[] =
 322     {
 323     0.09375, 0.3125, 0.09375};  /* 3/32, 10/32, 3/32 */
 324
 325     uchar *pyr_buffer = 0;
 326     uchar *buffer = 0;
 327     int bufferBytes = 0;
 328
 329     uchar **imgI = 0;
 330     uchar **imgJ = 0;
 331     int *step = 0;
 332     double *scale = 0;
 333     CvSize* size = 0;
 334
 335     float *patchI;
 336     float *patchJ;
 337     float *Ix;
 338     float *Iy;
 339
 340     int i, j, k;
 341     int x, y;
 342
 343     CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
 344     int patchLen = patchSize.width * patchSize.height;
 345     int patchStep = patchSize.width * sizeof( patchI[0] );
 346
 347     CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
 348     int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
 349     int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );
 350
 351     CvStatus result = CV_OK;
 352
 353     /* check input arguments */
 354     if( !featuresA || !featuresB )
 355         return CV_NULLPTR_ERR;
 356     if( winSize.width <= 1 || winSize.height <= 1 )
 357         return CV_BADSIZE_ERR;
 358
 359     if( (flags & ~7) != 0 )
 360         return CV_BADFLAG_ERR;
 361     if( count <= 0 )
 362         return CV_BADRANGE_ERR;
 363
 364     result = icvInitPyramidalAlgorithm( imgA, imgB, imgStep, imgSize,
 365                                         pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
 366                                         &imgI, &imgJ, &step, &size, &scale, &pyr_buffer );
 367
 368     if( result < 0 )
 369         goto func_exit;
 370
 371     /* buffer_size = <size for patches> + <size for pyramids> */
 372     bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( patchI[0] );
 373
 374     buffer = (uchar *) icvAlloc( bufferBytes );
 375     if( !buffer )
 376     {
 377         result = CV_OUTOFMEM_ERR;
 378         goto func_exit;
 379     }
 380
 381     patchI = (float *) buffer;
 382     patchJ = patchI + srcPatchLen;
 383     Ix = patchJ + patchLen;
 384     Iy = Ix + patchLen;
 385
 386     memset( status, 1, count );
 387
 388     if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
 389     {
 390         memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
 391     }
 392
 393     /* find flow for each given point */
 394     for( i = 0; i < count; i++ )
 395     {
 396         CvPoint2D32f v;
 397         CvPoint minI, maxI, minJ, maxJ;
 398         int l;
 399         int pt_status = 1;
 400
 401         minI = maxI = minJ = maxJ = cvPoint( 0, 0 );
 402
 403         v.x = (float) (featuresB[i].x * scale[level] * 0.5);
 404         v.y = (float) (featuresB[i].y * scale[level] * 0.5);
 405
 406         /* do processing from top pyramid level (smallest image)
 407            to the bottom (original image) */
 408         for( l = level; l >= 0; l-- )
 409         {
 410             CvPoint2D32f u;
 411             CvSize levelSize = size[l];
 412
 413             v.x += v.x;
 414             v.y += v.y;
 415
 416             u.x = (float) (featuresA[i].x * scale[l]);
 417             u.y = (float) (featuresA[i].y * scale[l]);
 418
 419             if( icvGetRectSubPix_8u32f_C1R( imgI[l], step[l], levelSize,
 420                                             patchI, srcPatchStep, srcPatchSize, u ) < 0 )
 421             {
 422                 /* point is outside the image. take the next */
 423                 pt_status = 0;
 424                 break;
 425             }
 426
 427             /* calc Ix */
 428             icvSepConvSmall3_32f( patchI, srcPatchStep, Ix, patchStep,
 429                                   srcPatchSize, kerX, kerY, patchJ );
 430
 431             /* calc Iy */
 432             icvSepConvSmall3_32f( patchI, srcPatchStep, Iy, patchStep,
 433                                   srcPatchSize, kerY, kerX, patchJ );
 434
 435             /* repack patchI (remove borders) */
 436             for( k = 0; k < patchSize.height; k++ )
 437                 memcpy( patchI + k * patchSize.width,
 438                         patchI + (k + 1) * srcPatchSize.width + 1, patchStep );
 439
 440             intersect( u, winSize, levelSize, &minI, &maxI );
 441
 442             for( j = 0; j < criteria.maxIter; j++ )
 443             {
 444                 double bx = 0, by = 0;
 445                 float mx, my;
 446                 double Gxx = 0, Gxy = 0, Gyy = 0;
 447                 double D;
 448
 449                 if( icvGetRectSubPix_8u32f_C1R( imgJ[l], step[l], levelSize,
 450                                                 patchJ, patchStep, patchSize, v ) < 0 )
 451                 {
 452                     /* point is outside image. take the next */
 453                     pt_status = 0;
 454                     break;
 455                 }
 456
 457                 intersect( v, winSize, levelSize, &minJ, &maxJ );
 458
 459                 minJ.x = MAX( minJ.x, minI.x );
 460                 minJ.y = MAX( minJ.y, minI.y );
 461
 462                 maxJ.x = MIN( maxJ.x, maxI.x );
 463                 maxJ.y = MIN( maxJ.y, maxI.y );
 464
 465                 for( y = minJ.y; y < maxJ.y; y++ )
 466                 {
 467                     for( x = minJ.x; x < maxJ.x; x++ )
 468                     {
 469                         int idx = y * (winSize.width * 2 + 1) + x;
 470                         double t = patchI[idx] - patchJ[idx];
 471
 472                         bx += (double) (t * Ix[idx]);
 473                         by += (double) (t * Iy[idx]);
 474                         Gxx += Ix[idx] * Ix[idx];
 475                         Gxy += Ix[idx] * Iy[idx];
 476                         Gyy += Iy[idx] * Iy[idx];
 477                     }
 478                 }
 479
 480                 D = Gxx * Gyy - Gxy * Gxy;
 481                 if( D < DBL_EPSILON )
 482                 {
 483                     pt_status = 0;
 484                     break;
 485                 }
 486
 487                 D = 1. / D;
 488
 489                 mx = (float) ((Gyy * bx - Gxy * by) * D);
 490                 my = (float) ((Gxx * by - Gxy * bx) * D);
 491
 492                 v.x += mx;
 493                 v.y += my;
 494
 495                 if( mx * mx + my * my < criteria.epsilon )
 496                     break;
 497             }
 498
 499             if( pt_status == 0 )
 500                 break;
 501         }
 502
 503         if( pt_status )
 504         {
 505             featuresB[i] = v;
 506
 507             if( error )
 508             {
 509                 /* calc error */
 510                 double err = 0;
 511
 512                 for( y = minJ.y; y < maxJ.y; y++ )
 513                 {
 514                     for( x = minJ.x; x < maxJ.x; x++ )
 515                     {
 516                         int idx = y * (winSize.width * 2 + 1) + x;
 517                         double t = patchI[idx] - patchJ[idx];
 518
 519                         err += t * t;
 520                     }
 521                 }
 522                 error[i] = (float) sqrt( err );
 523             }
 524         }
 525
 526         if( status )
 527             status[i] = (char) pt_status;
 528     }
 529
 530   func_exit:
 531
 532     icvFree( &pyr_buffer );
 533     icvFree( &buffer );
 534
 535     return result;
 536 #undef MAX_LEVEL
 537 }
 538 #else
 539 {
 540 #define MAX_LEVEL 10
 541 #define MAX_ITERS 100
 542
 543     static const float kerX[] = { -1, 0, 1 }, kerY[] =
 544     {
 545     0.09375, 0.3125, 0.09375};  /* 3/32, 10/32, 3/32 */
 546
 547     uchar *pyr_buffer = 0;
 548     uchar *buffer = 0;
 549     int bufferBytes = 0;
 550
 551     uchar **imgI = 0;
 552     uchar **imgJ = 0;
 553     int *step = 0;
 554     double *scale = 0;
 555     CvSize* size = 0;
 556
 557     float *patchI;
 558     float *patchJ;
 559     float *Ix;
 560     float *Iy;
 561
 562     int i, j, k;
 563     int x, y;
 564
 565     CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
 566     int patchLen = patchSize.width * patchSize.height;
 567     int patchStep = patchSize.width * sizeof( patchI[0] );
 568
 569     CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
 570     int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
 571     int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );
 572
 573     CvStatus result = CV_OK;
 574
 575     /* check input arguments */
 576     if( !featuresA || !featuresB )
 577         return CV_NULLPTR_ERR;
 578     if( winSize.width <= 1 || winSize.height <= 1 )
 579         return CV_BADSIZE_ERR;
 580
 581     if( (flags & ~7) != 0 )
 582         return CV_BADFLAG_ERR;
 583     if( count <= 0 )
 584         return CV_BADRANGE_ERR;
 585
 586     result = icvInitPyramidalAlgorithm( imgA, imgB, imgStep, imgSize,
 587                                         pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
 588                                         &imgI, &imgJ, &step, &size, &scale, &pyr_buffer );
 589
 590     if( result < 0 )
 591         goto func_exit;
 592
 593     /* buffer_size = <size for patches> + <size for pyramids> */
 594     bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( patchI[0] );
 595
 596     buffer = (uchar *) icvAlloc( bufferBytes );
 597     if( !buffer )
 598     {
 599         result = CV_OUTOFMEM_ERR;
 600         goto func_exit;
 601     }
 602
 603     patchI = (float *) buffer;
 604     patchJ = patchI + srcPatchLen;
 605     Ix = patchJ + patchLen;
 606     Iy = Ix + patchLen;
 607
 608     if( status )
 609         memset( status, 1, count );
 610
 611     if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
 612     {
 613         memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
 614     }
 615
 616     /* find flow for each given point */
 617     for( i = 0; i < count; i++ )
 618     {
 619         CvPoint2D32f v;
 620         double t;
 621
 622         //CvPoint minI, maxI, minJ, maxJ;
 623         int l;
 624         int pt_status = 1;
 625
 626         //minI = maxI = minJ = maxJ = cvPoint(0,0);
 627
 628         v.x = (float) (featuresB[i].x * scale[level] * 0.5);
 629         v.y = (float) (featuresB[i].y * scale[level] * 0.5);
 630
 631         /* do processing from top pyramid level (smallest image)
 632            to the bottom (original image) */
 633         for( l = level; l >= 0; l-- )
 634         {
 635             CvPoint2D32f u;
 636             CvSize levelSize = size[l];
 637             double Gxx, Gxy, Gyy, D;
 638
 639             Gxx = Gxy = Gyy = 0;
 640
 641             v.x += v.x;
 642             v.y += v.y;
 643
 644             u.x = (float) (featuresA[i].x * scale[l]);
 645             u.y = (float) (featuresA[i].y * scale[l]);
 646
 647             if( icvGetRectSubPix_8u32f_C1R( imgI[l], step[l], levelSize,
 648                                              patchI, srcPatchStep, srcPatchSize, u ) < 0 )
 649             {
 650                 /* point is outside the image. take the next */
 651                 pt_status = 0;
 652                 break;
 653             }
 654
 655             /* calc Ix */
 656             icvSepConvSmall3_32f( patchI, srcPatchStep, Ix, patchStep,
 657                                   srcPatchSize, kerX, kerY, patchJ );
 658
 659             /* calc Iy */
 660             icvSepConvSmall3_32f( patchI, srcPatchStep, Iy, patchStep,
 661                                   srcPatchSize, kerY, kerX, patchJ );
 662
 663             /* repack patchI (remove borders) */
 664             for( k = 0; k < patchSize.height; k++ )
 665                 memcpy( patchI + k * patchSize.width,
 666                         patchI + (k + 1) * srcPatchSize.width + 1, patchStep );
 667
 668             for( y = 0, k = 0; y < patchSize.height; y++ )
 669             {
 670                 for( x = 0; x < patchSize.width; x++, k++ )
 671                 {
 672                     Gxx += Ix[k] * Ix[k];
 673                     Gxy += Ix[k] * Iy[k];
 674                     Gyy += Iy[k] * Iy[k];
 675                 }
 676             }
 677
 678             D = Gxx * Gyy - Gxy * Gxy;
 679             if( D < DBL_EPSILON )
 680             {
 681                 pt_status = 0;
 682                 break;
 683             }
 684
 685             D = 1. / D;
 686
 687             t = Gxx * D;
 688             Gxx = Gyy * D;
 689             Gyy = t;
 690             Gxy *= -D;
 691
 692             //intersect( u, winSize, levelSize, &minI, &maxI );
 693
 694             for( j = 0; j < criteria.maxIter; j++ )
 695             {
 696                 double bx = 0, by = 0;
 697                 float mx, my;
 698
 699                 if( icvGetRectSubPix_8u32f_C1R( imgJ[l], step[l], levelSize,
 700                                                  patchJ, patchStep, patchSize, v ) < 0 )
 701                 {
 702                     /* point is outside image. take the next */
 703                     pt_status = 0;
 704                     break;
 705                 }
 706
 707                 for( k = 0; k < patchLen; k++ )
 708                 {
 709                     double t = patchI[k] - patchJ[k];
 710
 711                     bx += (double) (t * Ix[k]);
 712                     by += (double) (t * Iy[k]);
 713                 }
 714
 715                 mx = (float) (Gxx * bx + Gxy * by);
 716                 my = (float) (Gxy * bx + Gyy * by);
 717
 718                 v.x += mx;
 719                 v.y += my;
 720
 721                 if( mx * mx + my * my < criteria.epsilon )
 722                     break;
 723             }
 724
 725             if( pt_status == 0 )
 726                 break;
 727         }
 728
 729         if( pt_status )
 730         {
 731             featuresB[i] = v;
 732
 733             if( error )
 734             {
 735                 /* calc error */
 736                 double err = 0;
 737
 738                 for( k = 0; k < patchLen; k++ )
 739                 {
 740                     double t = patchI[k] - patchJ[k];
 741
 742                     err += t * t;
 743                 }
 744                 error[i] = (float) sqrt( err );
 745             }
 746         }
 747
 748         if( status )
 749             status[i] = (char) pt_status;
 750     }
 751
 752   func_exit:
 753
 754     icvFree( &pyr_buffer );
 755     icvFree( &buffer );
 756
 757     return result;
 758 #undef MAX_LEVEL
 759 }
 760 #endif
 761
 762
 763 /* Affine tracking algorithm */
 764 static  CvStatus  icvCalcAffineFlowPyrLK_8uC1R( uchar * imgA, uchar * imgB,
 765                                                 int imgStep, CvSize imgSize,
 766                                                 uchar * pyrA, uchar * pyrB,
 767                                                 CvPoint2D32f * featuresA,
 768                                                 CvPoint2D32f * featuresB,
 769                                                 float *matrices, int count,
 770                                                 CvSize winSize, int level,
 771                                                 char *status, float *error,
 772                                                 CvTermCriteria criteria, int flags )
 773 {
 774 #define MAX_LEVEL 10
 775 #define MAX_ITERS 100
 776
 777     static const float kerX[] = { -1, 0, 1 }, kerY[] =
 778     {
 779     0.09375, 0.3125, 0.09375};  /* 3/32, 10/32, 3/32 */
 780
 781     uchar *buffer = 0;
 782     uchar *pyr_buffer = 0;
 783     int bufferBytes = 0;
 784
 785     uchar **imgI = 0;
 786     uchar **imgJ = 0;
 787     int *step = 0;
 788     double *scale = 0;
 789     CvSize* size = 0;
 790
 791     float *patchI;
 792     float *patchJ;
 793     float *Ix;
 794     float *Iy;
 795
 796     int i, j, k;
 797     int x, y;
 798
 799     CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
 800     int patchLen = patchSize.width * patchSize.height;
 801     int patchStep = patchSize.width * sizeof( patchI[0] );
 802
 803     CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
 804     int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
 805     int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );
 806
 807     CvStatus result = CV_OK;
 808
 809     /* check input arguments */
 810     if( !featuresA || !featuresB || !matrices )
 811         return CV_NULLPTR_ERR;
 812     if( winSize.width <= 1 || winSize.height <= 1 )
 813         return CV_BADSIZE_ERR;
 814
 815     if( (flags & ~7) != 0 )
 816         return CV_BADFLAG_ERR;
 817     if( count <= 0 )
 818         return CV_BADRANGE_ERR;
 819
 820     result = icvInitPyramidalAlgorithm( imgA, imgB, imgStep, imgSize,
 821                                         pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
 822                                         &imgI, &imgJ, &step, &size, &scale, &pyr_buffer );
 823
 824     if( result < 0 )
 825         goto func_exit;
 826
 827     /* buffer_size = <size for patches> + <size for pyramids> */
 828     bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( patchI[0] ) +
 829
 830         (36 * 2 + 6) * sizeof( double );
 831
 832     buffer = (uchar *) icvAlloc( bufferBytes );
 833     if( !buffer )
 834     {
 835         result = CV_OUTOFMEM_ERR;
 836         goto func_exit;
 837     }
 838
 839     patchI = (float *) buffer;
 840     patchJ = patchI + srcPatchLen;
 841     Ix = patchJ + patchLen;
 842     Iy = Ix + patchLen;
 843
 844     if( status )
 845         memset( status, 1, count );
 846
 847     if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
 848     {
 849         memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
 850         for( i = 0; i < count * 4; i += 4 )
 851         {
 852             matrices[i] = matrices[i + 2] = 1.f;
 853             matrices[i + 1] = matrices[i + 3] = 0.f;
 854         }
 855     }
 856
 857     /* find flow for each given point */
 858     for( i = 0; i < count; i++ )
 859     {
 860         CvPoint2D32f v;
 861         float A[4];
 862         double G[36];
 863         int l;
 864         int pt_status = 1;
 865
 866         memcpy( A, matrices + i * 4, sizeof( A ));
 867
 868         v.x = (float) (featuresB[i].x * scale[level] * 0.5);
 869         v.y = (float) (featuresB[i].y * scale[level] * 0.5);
 870
 871         /* do processing from top pyramid level (smallest image)
 872            to the bottom (original image) */
 873         for( l = level; l >= 0; l-- )
 874         {
 875             CvPoint2D32f u;
 876             CvSize levelSize = size[l];
 877             int x, y;
 878
 879             v.x += v.x;
 880             v.y += v.y;
 881
 882             u.x = (float) (featuresA[i].x * scale[l]);
 883             u.y = (float) (featuresA[i].y * scale[l]);
 884
 885             if( icvGetRectSubPix_8u32f_C1R( imgI[l], step[l], levelSize,
 886                                              patchI, srcPatchStep, srcPatchSize, u ) < 0 )
 887             {
 888                 /* point is outside the image. take the next */
 889                 pt_status = 0;
 890                 break;
 891             }
 892
 893             /* calc Ix */
 894             icvSepConvSmall3_32f( patchI, srcPatchStep, Ix, patchStep,
 895                                   srcPatchSize, kerX, kerY, patchJ );
 896
 897             /* calc Iy */
 898             icvSepConvSmall3_32f( patchI, srcPatchStep, Iy, patchStep,
 899                                   srcPatchSize, kerY, kerX, patchJ );
 900
 901             /* repack patchI (remove borders) */
 902             for( k = 0; k < patchSize.height; k++ )
 903                 memcpy( patchI + k * patchSize.width,
 904                         patchI + (k + 1) * srcPatchSize.width + 1, patchStep );
 905
 906             memset( G, 0, sizeof( G ));
 907
 908             /* calculate G matrix */
 909             for( y = -winSize.height, k = 0; y <= winSize.height; y++ )
 910             {
 911                 for( x = -winSize.width; x <= winSize.width; x++, k++ )
 912                 {
 913                     double ixix = ((double) Ix[k]) * Ix[k];
 914                     double ixiy = ((double) Ix[k]) * Iy[k];
 915                     double iyiy = ((double) Iy[k]) * Iy[k];
 916
 917                     double xx, xy, yy;
 918
 919                     G[0] += ixix;
 920                     G[1] += ixiy;
 921                     G[2] += x * ixix;
 922                     G[3] += y * ixix;
 923                     G[4] += x * ixiy;
 924                     G[5] += y * ixiy;
 925
 926                     // G[6] == G[1]
 927                     G[7] += iyiy;
 928                     // G[8] == G[4]
 929                     // G[9] == G[5]
 930                     G[10] += x * iyiy;
 931                     G[11] += y * iyiy;
 932
 933                     xx = x * x;
 934                     xy = x * y;
 935                     yy = y * y;
 936
 937                     // G[12] == G[2]
 938                     // G[13] == G[8] == G[4]
 939                     G[14] += xx * ixix;
 940                     G[15] += xy * ixix;
 941                     G[16] += xx * ixiy;
 942                     G[17] += xy * ixiy;
 943
 944                     // G[18] == G[3]
 945                     // G[19] == G[9]
 946                     // G[20] == G[15]
 947                     G[21] += yy * ixix;
 948                     // G[22] == G[17]
 949                     G[23] += yy * ixiy;
 950
 951                     // G[24] == G[4]
 952                     // G[25] == G[10]
 953                     // G[26] == G[16]
 954                     // G[27] == G[22]
 955                     G[28] += xx * iyiy;
 956                     G[29] += xy * iyiy;
 957
 958                     // G[30] == G[5]
 959                     // G[31] == G[11]
 960                     // G[32] == G[17]
 961                     // G[33] == G[23]
 962                     // G[34] == G[29]
 963                     G[35] += yy * iyiy;
 964                 }
 965             }
 966
 967             G[8] = G[4];
 968             G[9] = G[5];
 969             G[22] = G[17];
 970
 971             // fill part of G below its diagonal
 972             for( y = 1; y < 6; y++ )
 973                 for( x = 0; x < y; x++ )
 974                     G[y * 6 + x] = G[x * 6 + y];
 975
 976             CvMat mat;
 977             cvInitMatHeader( &mat, 6, 6, CV_64FC1, G );
 978
 979             if( cvInvert( &mat, &mat, CV_SVD ) < 1e-3 )
 980             {
 981                 /* bad matrix. take the next point */
 982                 pt_status = 0;
 983             }
 984             else
 985             {
 986                 for( j = 0; j < criteria.maxIter; j++ )
 987                 {
 988                     double b[6], eta[6];
 989                     double t0, t1, s = 0;
 990
 991                     if( icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], step[l], levelSize,
 992                                                           patchJ, patchStep, patchSize, A,
 993                                                           0, 0 ) < 0 )
 994                     {
 995                         pt_status = 0;
 996                         break;
 997                     }
 998
 999                     memset( b, 0, sizeof( b ));
1000
1001                     for( y = -winSize.height, k = 0; y <= winSize.height; y++ )
1002                     {
1003                         for( x = -winSize.width; x <= winSize.width; x++, k++ )
1004                         {
1005                             double t = patchI[k] - patchJ[k];
1006                             double ixt = Ix[k] * t;
1007                             double iyt = Iy[k] * t;
1008
1009                             s += t;
1010
1011                             b[0] += ixt;
1012                             b[1] += iyt;
1013                             b[2] += x * ixt;
1014                             b[3] += y * ixt;
1015                             b[4] += x * iyt;
1016                             b[5] += y * iyt;
1017                         }
1018                     }
1019
1020                     icvTransformVector_64d( G, b, eta, 6, 6 );
1021
1022                     t0 = v.x + A[0] * eta[0] + A[1] * eta[1];
1023                     t1 = v.y + A[2] * eta[0] + A[3] * eta[1];
1024
1025                     assert( fabs( t0 ) < levelSize.width * 2 );
1026                     assert( fabs( t1 ) < levelSize.height * 2 );
1027
1028                     v.x = (float) t0;
1029                     v.y = (float) t1;
1030
1031                     t0 = A[0] * (1 + eta[2]) + A[1] * eta[4];
1032                     t1 = A[0] * eta[3] + A[1] * (1 + eta[5]);
1033                     A[0] = (float) t0;
1034                     A[1] = (float) t1;
1035
1036                     t0 = A[2] * (1 + eta[2]) + A[3] * eta[4];
1037                     t1 = A[2] * eta[3] + A[3] * (1 + eta[5]);
1038                     A[2] = (float) t0;
1039                     A[3] = (float) t1;
1040
1041                     /*t0 = 4./(fabs(A[0]) + fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + DBL_EPSILON);
1042                        A[0] = (float)(A[0]*t0);
1043                        A[1] = (float)(A[1]*t0);
1044                        A[2] = (float)(A[2]*t0);
1045                        A[3] = (float)(A[3]*t0);
1046
1047                        t0 = fabs(A[0]*A[2] - A[1]*A[3]);
1048                        if( t0 >
1049                        A[0] = (float)(A[0]*t0);
1050                        A[1] = (float)(A[1]*t0);
1051                        A[2] = (float)(A[2]*t0);
1052                        A[3] = (float)(A[3]*t0); */
1053
1054                     if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon )
1055                         break;
1056                 }
1057             }
1058
1059             if( pt_status == 0 )
1060                 break;
1061         }
1062
1063         if( pt_status )
1064         {
1065             featuresB[i] = v;
1066             memcpy( matrices + i * 4, A, sizeof( A ));
1067
1068             if( error )
1069             {
1070                 /* calc error */
1071                 double err = 0;
1072
1073                 for( y = 0, k = 0; y < patchSize.height; y++ )
1074                 {
1075                     for( x = 0; x < patchSize.width; x++, k++ )
1076                     {
1077                         double t = patchI[k] - patchJ[k];
1078                         err += t * t;
1079                     }
1080                 }
1081                 error[i] = (float) sqrt( err );
1082             }
1083         }
1084
1085         if( status )
1086             status[i] = (char) pt_status;
1087     }
1088
1089   func_exit:
1090
1091     icvFree( &pyr_buffer );
1092     icvFree( &buffer );
1093
1094     return result;
1095 #undef MAX_LEVEL
1096 }
1097
1098
1099 static int icvMinimalPyramidSize( CvSize img_size )
1100 {
1101     return icvAlign(img_size.width,8) * img_size.height / 3;
1102 }
1103
1104
1105 CV_IMPL void
1106 cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB,
1107                         void* pyrarrA, void* pyrarrB,
1108                         CvPoint2D32f * featuresA,
1109                         CvPoint2D32f * featuresB,
1110                         int count, CvSize winSize, int level,
1111                         char *status, float *error,
1112                         CvTermCriteria criteria, int flags )
1113 {
1114     CV_FUNCNAME( "cvCalcOpticalFlowPyrLK" );
1115
1116     __BEGIN__;
1117
1118     CvMat stubA, *imgA = (CvMat*)arrA;
1119     CvMat stubB, *imgB = (CvMat*)arrB;
1120     CvMat pstubA, *pyrA = (CvMat*)pyrarrA;
1121     CvMat pstubB, *pyrB = (CvMat*)pyrarrB;
1122     CvSize img_size;
1123
1124     CV_CALL( imgA = cvGetMat( imgA, &stubA ));
1125     CV_CALL( imgB = cvGetMat( imgB, &stubB ));
1126
1127     if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 )
1128         CV_ERROR( CV_StsUnsupportedFormat, "" );
1129
1130     if( !CV_ARE_TYPES_EQ( imgA, imgB ))
1131         CV_ERROR( CV_StsUnmatchedFormats, "" );
1132
1133     if( !CV_ARE_SIZES_EQ( imgA, imgB ))
1134         CV_ERROR( CV_StsUnmatchedSizes, "" );
1135
1136     if( imgA->step != imgB->step )
1137         CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" );
1138
1139     img_size = icvGetMatSize( imgA );
1140
1141     if( pyrA )
1142     {
1143         CV_CALL( pyrA = cvGetMat( pyrA, &pstubA ));
1144
1145         if( pyrA->step*pyrA->height < icvMinimalPyramidSize( img_size ) )
1146             CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" );
1147     }
1148     else
1149     {
1150         pyrA = &pstubA;
1151         pyrA->data.ptr = 0;
1152     }
1153
1154
1155     if( pyrB )
1156     {
1157         CV_CALL( pyrB = cvGetMat( pyrB, &pstubB ));
1158
1159         if( pyrB->step*pyrB->height < icvMinimalPyramidSize( img_size ) )
1160             CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" );
1161     }
1162     else
1163     {
1164         pyrB = &pstubB;
1165         pyrB->data.ptr = 0;
1166     }
1167
1168     IPPI_CALL( icvCalcOpticalFlowPyrLK_8uC1R( imgA->data.ptr, imgB->data.ptr, imgA->step,
1169                                               img_size, pyrA->data.ptr, pyrB->data.ptr,
1170                                               featuresA, featuresB,
1171                                               count, winSize, level, status,
1172                                               error, criteria, flags ));
1173
1174     __END__;
1175 }
1176
1177
1178 CV_IMPL void
1179 cvCalcAffineFlowPyrLK( const void* arrA, const void* arrB,
1180                        void* pyrarrA, void* pyrarrB,
1181                        CvPoint2D32f * featuresA,
1182                        CvPoint2D32f * featuresB,
1183                        float *matrices, int count,
1184                        CvSize winSize, int level,
1185                        char *status, float *error,
1186                        CvTermCriteria criteria, int flags )
1187 {
1188     CV_FUNCNAME( "cvCalcAffineFlowPyrLK" );
1189
1190     __BEGIN__;
1191
1192     CvMat stubA, *imgA = (CvMat*)arrA;
1193     CvMat stubB, *imgB = (CvMat*)arrB;
1194     CvMat pstubA, *pyrA = (CvMat*)pyrarrA;
1195     CvMat pstubB, *pyrB = (CvMat*)pyrarrB;
1196     CvSize img_size;
1197
1198     CV_CALL( imgA = cvGetMat( imgA, &stubA ));
1199     CV_CALL( imgB = cvGetMat( imgB, &stubB ));
1200
1201     if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 )
1202         CV_ERROR( CV_StsUnsupportedFormat, "" );
1203
1204     if( !CV_ARE_TYPES_EQ( imgA, imgB ))
1205         CV_ERROR( CV_StsUnmatchedFormats, "" );
1206
1207     if( !CV_ARE_SIZES_EQ( imgA, imgB ))
1208         CV_ERROR( CV_StsUnmatchedSizes, "" );
1209
1210     if( imgA->step != imgB->step )
1211         CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" );
1212
1213     if( !matrices )
1214         CV_ERROR( CV_StsNullPtr, "" );
1215
1216     img_size = icvGetMatSize( imgA );
1217
1218     if( pyrA )
1219     {
1220         CV_CALL( pyrA = cvGetMat( pyrA, &pstubA ));
1221
1222         if( pyrA->step*pyrA->height < icvMinimalPyramidSize( img_size ) )
1223             CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" );
1224     }
1225     else
1226     {
1227         pyrA = &pstubA;
1228         pyrA->data.ptr = 0;
1229     }
1230
1231
1232     if( pyrB )
1233     {
1234         CV_CALL( pyrB = cvGetMat( pyrB, &pstubB ));
1235
1236         if( pyrB->step*pyrB->height < icvMinimalPyramidSize( img_size ) )
1237             CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" );
1238     }
1239     else
1240     {
1241         pyrB = &pstubB;
1242         pyrB->data.ptr = 0;
1243     }
1244
1245     IPPI_CALL( icvCalcAffineFlowPyrLK_8uC1R( imgA->data.ptr, imgB->data.ptr, imgA->step,
1246                                              img_size, pyrA->data.ptr, pyrB->data.ptr,
1247                                              featuresA, featuresB, matrices,
1248                                              count, winSize, level, status,
1249                                              error, criteria, flags ));
1250
1251     __END__;
1252 }
1253
1254
1255
1256 /* End of file. */