git.maemo.org Git - opencv/blob - cv/src/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 #include <stdio.h>
  44
  45 static void
  46 intersect( CvPoint2D32f pt, CvSize win_size, CvSize imgSize,
  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, imgSize.width - ipt.x );
  63     max_pt->y = MIN( win_size.height, imgSize.height - ipt.y );
  64 }
  65
  66
  67 static int icvMinimalPyramidSize( CvSize imgSize )
  68 {
  69     return cvAlign(imgSize.width,8) * imgSize.height / 3;
  70 }
  71
  72
  73 static void
  74 icvInitPyramidalAlgorithm( const CvMat* imgA, const CvMat* imgB,
  75                            CvMat* pyrA, CvMat* pyrB,
  76                            int level, CvTermCriteria * criteria,
  77                            int max_iters, int flags,
  78                            uchar *** imgI, uchar *** imgJ,
  79                            int **step, CvSize** size,
  80                            double **scale, uchar ** buffer )
  81 {
  82     CV_FUNCNAME( "icvInitPyramidalAlgorithm" );
  83
  84     __BEGIN__;
  85
  86     const int ALIGN = 8;
  87     int pyrBytes, bufferBytes = 0, elem_size;
  88     int level1 = level + 1;
  89
  90     int i;
  91     CvSize imgSize, levelSize;
  92
  93     *buffer = 0;
  94     *imgI = *imgJ = 0;
  95     *step = 0;
  96     *scale = 0;
  97     *size = 0;
  98
  99     /* check input arguments */
 100     if( (flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA ||
 101         (flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB )
 102         CV_ERROR( CV_StsNullPtr, "Some of the precomputed pyramids are missing" );
 103
 104     if( level < 0 )
 105         CV_ERROR( CV_StsOutOfRange, "The number of pyramid layers is negative" );
 106
 107     switch( criteria->type )
 108     {
 109     case CV_TERMCRIT_ITER:
 110         criteria->epsilon = 0.f;
 111         break;
 112     case CV_TERMCRIT_EPS:
 113         criteria->max_iter = max_iters;
 114         break;
 115     case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
 116         break;
 117     default:
 118         assert( 0 );
 119         CV_ERROR( CV_StsBadArg, "Invalid termination criteria" );
 120     }
 121
 122     /* compare squared values */
 123     criteria->epsilon *= criteria->epsilon;
 124
 125     /* set pointers and step for every level */
 126     pyrBytes = 0;
 127
 128     imgSize = cvGetSize(imgA);
 129     elem_size = CV_ELEM_SIZE(imgA->type);
 130     levelSize = imgSize;
 131
 132     for( i = 1; i < level1; i++ )
 133     {
 134         levelSize.width = (levelSize.width + 1) >> 1;
 135         levelSize.height = (levelSize.height + 1) >> 1;
 136
 137         int tstep = cvAlign(levelSize.width,ALIGN) * elem_size;
 138         pyrBytes += tstep * levelSize.height;
 139     }
 140
 141     assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 );
 142
 143     /* buffer_size = <size for patches> + <size for pyramids> */
 144     bufferBytes = (int)((level1 >= 0) * ((pyrA->data.ptr == 0) +
 145         (pyrB->data.ptr == 0)) * pyrBytes +
 146         (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) +
 147          sizeof(size[0][0]) + sizeof(scale[0][0])) * level1);
 148
 149     CV_CALL( *buffer = (uchar *)cvAlloc( bufferBytes ));
 150
 151     *imgI = (uchar **) buffer[0];
 152     *imgJ = *imgI + level1;
 153     *step = (int *) (*imgJ + level1);
 154     *scale = (double *) (*step + level1);
 155     *size = (CvSize *)(*scale + level1);
 156
 157     imgI[0][0] = imgA->data.ptr;
 158     imgJ[0][0] = imgB->data.ptr;
 159     step[0][0] = imgA->step;
 160     scale[0][0] = 1;
 161     size[0][0] = imgSize;
 162
 163     if( level > 0 )
 164     {
 165         uchar *bufPtr = (uchar *) (*size + level1);
 166         uchar *ptrA = pyrA->data.ptr;
 167         uchar *ptrB = pyrB->data.ptr;
 168
 169         if( !ptrA )
 170         {
 171             ptrA = bufPtr;
 172             bufPtr += pyrBytes;
 173         }
 174
 175         if( !ptrB )
 176             ptrB = bufPtr;
 177
 178         levelSize = imgSize;
 179
 180         /* build pyramids for both frames */
 181         for( i = 1; i <= level; i++ )
 182         {
 183             int levelBytes;
 184             CvMat prev_level, next_level;
 185
 186             levelSize.width = (levelSize.width + 1) >> 1;
 187             levelSize.height = (levelSize.height + 1) >> 1;
 188
 189             size[0][i] = levelSize;
 190             step[0][i] = cvAlign( levelSize.width, ALIGN ) * elem_size;
 191             scale[0][i] = scale[0][i - 1] * 0.5;
 192
 193             levelBytes = step[0][i] * levelSize.height;
 194             imgI[0][i] = (uchar *) ptrA;
 195             ptrA += levelBytes;
 196
 197             if( !(flags & CV_LKFLOW_PYR_A_READY) )
 198             {
 199                 prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 );
 200                 next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 );
 201                 cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] );
 202                 cvSetData( &next_level, imgI[0][i], step[0][i] );
 203                 cvPyrDown( &prev_level, &next_level );
 204             }
 205
 206             imgJ[0][i] = (uchar *) ptrB;
 207             ptrB += levelBytes;
 208
 209             if( !(flags & CV_LKFLOW_PYR_B_READY) )
 210             {
 211                 prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 );
 212                 next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 );
 213                 cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] );
 214                 cvSetData( &next_level, imgJ[0][i], step[0][i] );
 215                 cvPyrDown( &prev_level, &next_level );
 216             }
 217         }
 218     }
 219
 220     __END__;
 221 }
 222
 223
 224 /* compute dI/dx and dI/dy */
 225 static void
 226 icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step,
 227                  CvSize src_size, const float* smooth_k, float* buffer0 )
 228 {
 229     int src_width = src_size.width, dst_width = src_size.width-2;
 230     int x, height = src_size.height - 2;
 231     float* buffer1 = buffer0 + src_width;
 232
 233     src_step /= sizeof(src[0]);
 234     dst_step /= sizeof(dstX[0]);
 235
 236     for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step )
 237     {
 238         const float* src2 = src + src_step;
 239         const float* src3 = src + src_step*2;
 240
 241         for( x = 0; x < src_width; x++ )
 242         {
 243             float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1];
 244             float t1 = src3[x] - src[x];
 245             buffer0[x] = t0; buffer1[x] = t1;
 246         }
 247
 248         for( x = 0; x < dst_width; x++ )
 249         {
 250             float t0 = buffer0[x+2] - buffer0[x];
 251             float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1];
 252             dstX[x] = t0; dstY[x] = t1;
 253         }
 254     }
 255 }
 256
 257
 258 icvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0;
 259 icvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0;
 260 icvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0;
 261
 262
 263 CV_IMPL void
 264 cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB,
 265                         void* pyrarrA, void* pyrarrB,
 266                         const CvPoint2D32f * featuresA,
 267                         CvPoint2D32f * featuresB,
 268                         int count, CvSize winSize, int level,
 269                         char *status, float *error,
 270                         CvTermCriteria criteria, int flags )
 271 {
 272     uchar *pyrBuffer = 0;
 273     uchar *buffer = 0;
 274     float* _error = 0;
 275     char* _status = 0;
 276
 277     void* ipp_optflow_state = 0;
 278
 279     CV_FUNCNAME( "cvCalcOpticalFlowPyrLK" );
 280
 281     __BEGIN__;
 282
 283     const int MAX_ITERS = 100;
 284
 285     CvMat stubA, *imgA = (CvMat*)arrA;
 286     CvMat stubB, *imgB = (CvMat*)arrB;
 287     CvMat pstubA, *pyrA = (CvMat*)pyrarrA;
 288     CvMat pstubB, *pyrB = (CvMat*)pyrarrB;
 289     CvSize imgSize;
 290     static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 };  /* 3/32, 10/32, 3/32 */
 291
 292     int bufferBytes = 0;
 293     uchar **imgI = 0;
 294     uchar **imgJ = 0;
 295     int *step = 0;
 296     double *scale = 0;
 297     CvSize* size = 0;
 298
 299     int threadCount = cvGetNumThreads();
 300     float* _patchI[CV_MAX_THREADS];
 301     float* _patchJ[CV_MAX_THREADS];
 302     float* _Ix[CV_MAX_THREADS];
 303     float* _Iy[CV_MAX_THREADS];
 304
 305     int i, l;
 306
 307     CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
 308     int patchLen = patchSize.width * patchSize.height;
 309     int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2);
 310
 311     CV_CALL( imgA = cvGetMat( imgA, &stubA ));
 312     CV_CALL( imgB = cvGetMat( imgB, &stubB ));
 313
 314     if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 )
 315         CV_ERROR( CV_StsUnsupportedFormat, "" );
 316
 317     if( !CV_ARE_TYPES_EQ( imgA, imgB ))
 318         CV_ERROR( CV_StsUnmatchedFormats, "" );
 319
 320     if( !CV_ARE_SIZES_EQ( imgA, imgB ))
 321         CV_ERROR( CV_StsUnmatchedSizes, "" );
 322
 323     if( imgA->step != imgB->step )
 324         CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" );
 325
 326     imgSize = cvGetMatSize( imgA );
 327
 328     if( pyrA )
 329     {
 330         CV_CALL( pyrA = cvGetMat( pyrA, &pstubA ));
 331
 332         if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) )
 333             CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" );
 334     }
 335     else
 336     {
 337         pyrA = &pstubA;
 338         pyrA->data.ptr = 0;
 339     }
 340
 341     if( pyrB )
 342     {
 343         CV_CALL( pyrB = cvGetMat( pyrB, &pstubB ));
 344
 345         if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) )
 346             CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" );
 347     }
 348     else
 349     {
 350         pyrB = &pstubB;
 351         pyrB->data.ptr = 0;
 352     }
 353
 354     if( count == 0 )
 355         EXIT;
 356
 357     if( !featuresA || !featuresB )
 358         CV_ERROR( CV_StsNullPtr, "Some of arrays of point coordinates are missing" );
 359
 360     if( count < 0 )
 361         CV_ERROR( CV_StsOutOfRange, "The number of tracked points is negative or zero" );
 362
 363     if( winSize.width <= 1 || winSize.height <= 1 )
 364         CV_ERROR( CV_StsBadSize, "Invalid search window size" );
 365
 366     for( i = 0; i < threadCount; i++ )
 367         _patchI[i] = _patchJ[i] = _Ix[i] = _Iy[i] = 0;
 368
 369     CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, pyrA, pyrB,
 370         level, &criteria, MAX_ITERS, flags,
 371         &imgI, &imgJ, &step, &size, &scale, &pyrBuffer ));
 372
 373     if( !status )
 374         CV_CALL( status = _status = (char*)cvAlloc( count*sizeof(_status[0]) ));
 375
 376 #if 0
 377     if( icvOpticalFlowPyrLKInitAlloc_8u_C1R_p &&
 378         icvOpticalFlowPyrLKFree_8u_C1R_p &&
 379         icvOpticalFlowPyrLK_8u_C1R_p &&
 380         winSize.width == winSize.height &&
 381         icvOpticalFlowPyrLKInitAlloc_8u_C1R_p( &ipp_optflow_state, imgSize,
 382                                                winSize.width*2+1, cvAlgHintAccurate ) >= 0 )
 383     {
 384         CvPyramid ipp_pyrA, ipp_pyrB;
 385         static const double rate[] = { 1, 0.5, 0.25, 0.125, 0.0625, 0.03125, 0.015625, 0.0078125,
 386                                        0.00390625, 0.001953125, 0.0009765625, 0.00048828125, 0.000244140625,
 387                                        0.0001220703125 };
 388         // initialize pyramid structures
 389         assert( level < 14 );
 390         ipp_pyrA.ptr = imgI;
 391         ipp_pyrB.ptr = imgJ;
 392         ipp_pyrA.sz = ipp_pyrB.sz = size;
 393         ipp_pyrA.rate = ipp_pyrB.rate = (double*)rate;
 394         ipp_pyrA.step = ipp_pyrB.step = step;
 395         ipp_pyrA.state = ipp_pyrB.state = 0;
 396         ipp_pyrA.level = ipp_pyrB.level = level;
 397
 398         if( !error )
 399             CV_CALL( error = _error = (float*)cvAlloc( count*sizeof(_error[0]) ));
 400
 401         for( i = 0; i < count; i++ )
 402             featuresB[i] = featuresA[i];
 403
 404         if( icvOpticalFlowPyrLK_8u_C1R_p( &ipp_pyrA, &ipp_pyrB,
 405             (const float*)featuresA, (float*)featuresB, status, error, count,
 406             winSize.width*2 + 1, level, criteria.max_iter,
 407             (float)criteria.epsilon, ipp_optflow_state ) >= 0 )
 408         {
 409             for( i = 0; i < count; i++ )
 410                 status[i] = status[i] == 0;
 411             EXIT;
 412         }
 413     }
 414 #endif
 415
 416     /* buffer_size = <size for patches> + <size for pyramids> */
 417     bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( _patchI[0][0] ) * threadCount;
 418     CV_CALL( buffer = (uchar*)cvAlloc( bufferBytes ));
 419
 420     for( i = 0; i < threadCount; i++ )
 421     {
 422         _patchI[i] = i == 0 ? (float*)buffer : _Iy[i-1] + patchLen;
 423         _patchJ[i] = _patchI[i] + srcPatchLen;
 424         _Ix[i] = _patchJ[i] + patchLen;
 425         _Iy[i] = _Ix[i] + patchLen;
 426     }
 427
 428     memset( status, 1, count );
 429     if( error )
 430         memset( error, 0, count*sizeof(error[0]) );
 431
 432     if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
 433         memcpy( featuresB, featuresA, count*sizeof(featuresA[0]));
 434
 435     /* do processing from top pyramid level (smallest image)
 436        to the bottom (original image) */
 437     for( l = level; l >= 0; l-- )
 438     {
 439         CvSize levelSize = size[l];
 440         int levelStep = step[l];
 441
 442         {
 443 #ifdef _OPENMP
 444         #pragma omp parallel for num_threads(threadCount), schedule(dynamic)
 445 #endif // _OPENMP
 446         /* find flow for each given point */
 447         for( i = 0; i < count; i++ )
 448         {
 449             CvPoint2D32f v;
 450             CvPoint minI, maxI, minJ, maxJ;
 451             CvSize isz, jsz;
 452             int pt_status;
 453             CvPoint2D32f u;
 454             CvPoint prev_minJ = { -1, -1 }, prev_maxJ = { -1, -1 };
 455             double Gxx = 0, Gxy = 0, Gyy = 0, D = 0, minEig = 0;
 456             float prev_mx = 0, prev_my = 0;
 457             int j, x, y;
 458             int threadIdx = cvGetThreadNum();
 459             float* patchI = _patchI[threadIdx];
 460             float* patchJ = _patchJ[threadIdx];
 461             float* Ix = _Ix[threadIdx];
 462             float* Iy = _Iy[threadIdx];
 463
 464             v.x = featuresB[i].x;
 465             v.y = featuresB[i].y;
 466             if( l < level )
 467             {
 468                 v.x += v.x;
 469                 v.y += v.y;
 470             }
 471             else
 472             {
 473                 v.x = (float)(v.x * scale[l]);
 474                 v.y = (float)(v.y * scale[l]);
 475             }
 476
 477             pt_status = status[i];
 478             if( !pt_status )
 479                 continue;
 480
 481             minI = maxI = minJ = maxJ = cvPoint( 0, 0 );
 482
 483             u.x = (float) (featuresA[i].x * scale[l]);
 484             u.y = (float) (featuresA[i].y * scale[l]);
 485
 486             intersect( u, winSize, levelSize, &minI, &maxI );
 487             isz = jsz = cvSize(maxI.x - minI.x + 2, maxI.y - minI.y + 2);
 488             u.x += (minI.x - (patchSize.width - maxI.x + 1))*0.5f;
 489             u.y += (minI.y - (patchSize.height - maxI.y + 1))*0.5f;
 490
 491             if( isz.width < 3 || isz.height < 3 ||
 492                 icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, levelSize,
 493                     patchI, isz.width*sizeof(patchI[0]), isz, u ) < 0 )
 494             {
 495                 /* point is outside the image. take the next */
 496                 status[i] = 0;
 497                 continue;
 498             }
 499
 500             icvCalcIxIy_32f( patchI, isz.width*sizeof(patchI[0]), Ix, Iy,
 501                 (isz.width-2)*sizeof(patchI[0]), isz, smoothKernel, patchJ );
 502
 503             for( j = 0; j < criteria.max_iter; j++ )
 504             {
 505                 double bx = 0, by = 0;
 506                 float mx, my;
 507                 CvPoint2D32f _v;
 508
 509                 intersect( v, winSize, levelSize, &minJ, &maxJ );
 510
 511                 minJ.x = MAX( minJ.x, minI.x );
 512                 minJ.y = MAX( minJ.y, minI.y );
 513
 514                 maxJ.x = MIN( maxJ.x, maxI.x );
 515                 maxJ.y = MIN( maxJ.y, maxI.y );
 516
 517                 jsz = cvSize(maxJ.x - minJ.x, maxJ.y - minJ.y);
 518
 519                 _v.x = v.x + (minJ.x - (patchSize.width - maxJ.x + 1))*0.5f;
 520                 _v.y = v.y + (minJ.y - (patchSize.height - maxJ.y + 1))*0.5f;
 521
 522                 if( jsz.width < 1 || jsz.height < 1 ||
 523                     icvGetRectSubPix_8u32f_C1R( imgJ[l], levelStep, levelSize, patchJ,
 524                                                 jsz.width*sizeof(patchJ[0]), jsz, _v ) < 0 )
 525                 {
 526                     /* point is outside image. take the next */
 527                     pt_status = 0;
 528                     break;
 529                 }
 530
 531                 if( maxJ.x == prev_maxJ.x && maxJ.y == prev_maxJ.y &&
 532                     minJ.x == prev_minJ.x && minJ.y == prev_minJ.y )
 533                 {
 534                     for( y = 0; y < jsz.height; y++ )
 535                     {
 536                         const float* pi = patchI +
 537                             (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
 538                         const float* pj = patchJ + y*jsz.width;
 539                         const float* ix = Ix +
 540                             (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x;
 541                         const float* iy = Iy + (ix - Ix);
 542
 543                         for( x = 0; x < jsz.width; x++ )
 544                         {
 545                             double t0 = pi[x] - pj[x];
 546                             bx += t0 * ix[x];
 547                             by += t0 * iy[x];
 548                         }
 549                     }
 550                 }
 551                 else
 552                 {
 553                     Gxx = Gyy = Gxy = 0;
 554                     for( y = 0; y < jsz.height; y++ )
 555                     {
 556                         const float* pi = patchI +
 557                             (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
 558                         const float* pj = patchJ + y*jsz.width;
 559                         const float* ix = Ix +
 560                             (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x;
 561                         const float* iy = Iy + (ix - Ix);
 562
 563                         for( x = 0; x < jsz.width; x++ )
 564                         {
 565                             double t = pi[x] - pj[x];
 566                             bx += (double) (t * ix[x]);
 567                             by += (double) (t * iy[x]);
 568                             Gxx += ix[x] * ix[x];
 569                             Gxy += ix[x] * iy[x];
 570                             Gyy += iy[x] * iy[x];
 571                         }
 572                     }
 573
 574                     D = Gxx * Gyy - Gxy * Gxy;
 575                     if( D < DBL_EPSILON )
 576                     {
 577                         pt_status = 0;
 578                         break;
 579                     }
 580
 581                     // Adi Shavit - 2008.05
 582                     if( flags & CV_LKFLOW_GET_MIN_EIGENVALS )
 583                         minEig = (Gyy + Gxx - sqrt((Gxx-Gyy)*(Gxx-Gyy) + 4.*Gxy*Gxy))/(2*jsz.height*jsz.width);
 584
 585                     D = 1. / D;
 586
 587                     prev_minJ = minJ;
 588                     prev_maxJ = maxJ;
 589                 }
 590
 591                 mx = (float) ((Gyy * bx - Gxy * by) * D);
 592                 my = (float) ((Gxx * by - Gxy * bx) * D);
 593
 594                 v.x += mx;
 595                 v.y += my;
 596
 597                 if( mx * mx + my * my < criteria.epsilon )
 598                     break;
 599
 600                 if( j > 0 && fabs(mx + prev_mx) < 0.01 && fabs(my + prev_my) < 0.01 )
 601                 {
 602                     v.x -= mx*0.5f;
 603                     v.y -= my*0.5f;
 604                     break;
 605                 }
 606                 prev_mx = mx;
 607                 prev_my = my;
 608             }
 609
 610             featuresB[i] = v;
 611             status[i] = (char)pt_status;
 612             if( l == 0 && error && pt_status )
 613             {
 614                 /* calc error */
 615                 double err = 0;
 616                 if( flags & CV_LKFLOW_GET_MIN_EIGENVALS )
 617                     err = minEig;
 618                 else
 619                 {
 620                     for( y = 0; y < jsz.height; y++ )
 621                     {
 622                         const float* pi = patchI +
 623                             (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
 624                         const float* pj = patchJ + y*jsz.width;
 625
 626                         for( x = 0; x < jsz.width; x++ )
 627                         {
 628                             double t = pi[x] - pj[x];
 629                             err += t * t;
 630                         }
 631                     }
 632                     err = sqrt(err);
 633                 }
 634                 error[i] = (float)err;
 635             }
 636         } // end of point processing loop (i)
 637         }
 638     } // end of pyramid levels loop (l)
 639
 640     __END__;
 641
 642     if( ipp_optflow_state )
 643         icvOpticalFlowPyrLKFree_8u_C1R_p( ipp_optflow_state );
 644
 645     cvFree( &pyrBuffer );
 646     cvFree( &buffer );
 647     cvFree( &_error );
 648     cvFree( &_status );
 649 }
 650
 651
 652 /* Affine tracking algorithm */
 653
 654 CV_IMPL void
 655 cvCalcAffineFlowPyrLK( const void* arrA, const void* arrB,
 656                        void* pyrarrA, void* pyrarrB,
 657                        const CvPoint2D32f * featuresA,
 658                        CvPoint2D32f * featuresB,
 659                        float *matrices, int count,
 660                        CvSize winSize, int level,
 661                        char *status, float *error,
 662                        CvTermCriteria criteria, int flags )
 663 {
 664     const int MAX_ITERS = 100;
 665
 666     char* _status = 0;
 667     uchar *buffer = 0;
 668     uchar *pyr_buffer = 0;
 669
 670     CV_FUNCNAME( "cvCalcAffineFlowPyrLK" );
 671
 672     __BEGIN__;
 673
 674     CvMat stubA, *imgA = (CvMat*)arrA;
 675     CvMat stubB, *imgB = (CvMat*)arrB;
 676     CvMat pstubA, *pyrA = (CvMat*)pyrarrA;
 677     CvMat pstubB, *pyrB = (CvMat*)pyrarrB;
 678
 679     static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 };  /* 3/32, 10/32, 3/32 */
 680
 681     int bufferBytes = 0;
 682
 683     uchar **imgI = 0;
 684     uchar **imgJ = 0;
 685     int *step = 0;
 686     double *scale = 0;
 687     CvSize* size = 0;
 688
 689     float *patchI;
 690     float *patchJ;
 691     float *Ix;
 692     float *Iy;
 693
 694     int i, j, k, l;
 695
 696     CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
 697     int patchLen = patchSize.width * patchSize.height;
 698     int patchStep = patchSize.width * sizeof( patchI[0] );
 699
 700     CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
 701     int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
 702     int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );
 703     CvSize imgSize;
 704     float eps = (float)MIN(winSize.width, winSize.height);
 705
 706     CV_CALL( imgA = cvGetMat( imgA, &stubA ));
 707     CV_CALL( imgB = cvGetMat( imgB, &stubB ));
 708
 709     if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 )
 710         CV_ERROR( CV_StsUnsupportedFormat, "" );
 711
 712     if( !CV_ARE_TYPES_EQ( imgA, imgB ))
 713         CV_ERROR( CV_StsUnmatchedFormats, "" );
 714
 715     if( !CV_ARE_SIZES_EQ( imgA, imgB ))
 716         CV_ERROR( CV_StsUnmatchedSizes, "" );
 717
 718     if( imgA->step != imgB->step )
 719         CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" );
 720
 721     if( !matrices )
 722         CV_ERROR( CV_StsNullPtr, "" );
 723
 724     imgSize = cvGetMatSize( imgA );
 725
 726     if( pyrA )
 727     {
 728         CV_CALL( pyrA = cvGetMat( pyrA, &pstubA ));
 729
 730         if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) )
 731             CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" );
 732     }
 733     else
 734     {
 735         pyrA = &pstubA;
 736         pyrA->data.ptr = 0;
 737     }
 738
 739     if( pyrB )
 740     {
 741         CV_CALL( pyrB = cvGetMat( pyrB, &pstubB ));
 742
 743         if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) )
 744             CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" );
 745     }
 746     else
 747     {
 748         pyrB = &pstubB;
 749         pyrB->data.ptr = 0;
 750     }
 751
 752     if( count == 0 )
 753         EXIT;
 754
 755     /* check input arguments */
 756     if( !featuresA || !featuresB || !matrices )
 757         CV_ERROR( CV_StsNullPtr, "" );
 758
 759     if( winSize.width <= 1 || winSize.height <= 1 )
 760         CV_ERROR( CV_StsOutOfRange, "the search window is too small" );
 761
 762     if( count < 0 )
 763         CV_ERROR( CV_StsOutOfRange, "" );
 764
 765     CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB,
 766         pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
 767         &imgI, &imgJ, &step, &size, &scale, &pyr_buffer ));
 768
 769     /* buffer_size = <size for patches> + <size for pyramids> */
 770     bufferBytes = (srcPatchLen + patchLen*3)*sizeof(patchI[0]) + (36*2 + 6)*sizeof(double);
 771
 772     CV_CALL( buffer = (uchar*)cvAlloc(bufferBytes));
 773
 774     if( !status )
 775         CV_CALL( status = _status = (char*)cvAlloc(count) );
 776
 777     patchI = (float *) buffer;
 778     patchJ = patchI + srcPatchLen;
 779     Ix = patchJ + patchLen;
 780     Iy = Ix + patchLen;
 781
 782     if( status )
 783         memset( status, 1, count );
 784
 785     if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
 786     {
 787         memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
 788         for( i = 0; i < count * 4; i += 4 )
 789         {
 790             matrices[i] = matrices[i + 3] = 1.f;
 791             matrices[i + 1] = matrices[i + 2] = 0.f;
 792         }
 793     }
 794
 795     for( i = 0; i < count; i++ )
 796     {
 797         featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5);
 798         featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5);
 799     }
 800
 801     /* do processing from top pyramid level (smallest image)
 802        to the bottom (original image) */
 803     for( l = level; l >= 0; l-- )
 804     {
 805         CvSize levelSize = size[l];
 806         int levelStep = step[l];
 807
 808         /* find flow for each given point at the particular level */
 809         for( i = 0; i < count; i++ )
 810         {
 811             CvPoint2D32f u;
 812             float Av[6];
 813             double G[36];
 814             double meanI = 0, meanJ = 0;
 815             int x, y;
 816             int pt_status = status[i];
 817             CvMat mat;
 818
 819             if( !pt_status )
 820                 continue;
 821
 822             Av[0] = matrices[i*4];
 823             Av[1] = matrices[i*4+1];
 824             Av[3] = matrices[i*4+2];
 825             Av[4] = matrices[i*4+3];
 826
 827             Av[2] = featuresB[i].x += featuresB[i].x;
 828             Av[5] = featuresB[i].y += featuresB[i].y;
 829
 830             u.x = (float) (featuresA[i].x * scale[l]);
 831             u.y = (float) (featuresA[i].y * scale[l]);
 832
 833             if( u.x < -eps || u.x >= levelSize.width+eps ||
 834                 u.y < -eps || u.y >= levelSize.height+eps ||
 835                 icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep,
 836                 levelSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 )
 837             {
 838                 /* point is outside the image. take the next */
 839                 if( l == 0 )
 840                     status[i] = 0;
 841                 continue;
 842             }
 843
 844             icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy,
 845                 (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize,
 846                 smoothKernel, patchJ );
 847
 848             /* repack patchI (remove borders) */
 849             for( k = 0; k < patchSize.height; k++ )
 850                 memcpy( patchI + k * patchSize.width,
 851                         patchI + (k + 1) * srcPatchSize.width + 1, patchStep );
 852
 853             memset( G, 0, sizeof( G ));
 854
 855             /* calculate G matrix */
 856             for( y = -winSize.height, k = 0; y <= winSize.height; y++ )
 857             {
 858                 for( x = -winSize.width; x <= winSize.width; x++, k++ )
 859                 {
 860                     double ixix = ((double) Ix[k]) * Ix[k];
 861                     double ixiy = ((double) Ix[k]) * Iy[k];
 862                     double iyiy = ((double) Iy[k]) * Iy[k];
 863
 864                     double xx, xy, yy;
 865
 866                     G[0] += ixix;
 867                     G[1] += ixiy;
 868                     G[2] += x * ixix;
 869                     G[3] += y * ixix;
 870                     G[4] += x * ixiy;
 871                     G[5] += y * ixiy;
 872
 873                     // G[6] == G[1]
 874                     G[7] += iyiy;
 875                     // G[8] == G[4]
 876                     // G[9] == G[5]
 877                     G[10] += x * iyiy;
 878                     G[11] += y * iyiy;
 879
 880                     xx = x * x;
 881                     xy = x * y;
 882                     yy = y * y;
 883
 884                     // G[12] == G[2]
 885                     // G[13] == G[8] == G[4]
 886                     G[14] += xx * ixix;
 887                     G[15] += xy * ixix;
 888                     G[16] += xx * ixiy;
 889                     G[17] += xy * ixiy;
 890
 891                     // G[18] == G[3]
 892                     // G[19] == G[9]
 893                     // G[20] == G[15]
 894                     G[21] += yy * ixix;
 895                     // G[22] == G[17]
 896                     G[23] += yy * ixiy;
 897
 898                     // G[24] == G[4]
 899                     // G[25] == G[10]
 900                     // G[26] == G[16]
 901                     // G[27] == G[22]
 902                     G[28] += xx * iyiy;
 903                     G[29] += xy * iyiy;
 904
 905                     // G[30] == G[5]
 906                     // G[31] == G[11]
 907                     // G[32] == G[17]
 908                     // G[33] == G[23]
 909                     // G[34] == G[29]
 910                     G[35] += yy * iyiy;
 911
 912                     meanI += patchI[k];
 913                 }
 914             }
 915
 916             meanI /= patchSize.width*patchSize.height;
 917
 918             G[8] = G[4];
 919             G[9] = G[5];
 920             G[22] = G[17];
 921
 922             // fill part of G below its diagonal
 923             for( y = 1; y < 6; y++ )
 924                 for( x = 0; x < y; x++ )
 925                     G[y * 6 + x] = G[x * 6 + y];
 926
 927             cvInitMatHeader( &mat, 6, 6, CV_64FC1, G );
 928
 929             if( cvInvert( &mat, &mat, CV_SVD ) < 1e-4 )
 930             {
 931                 /* bad matrix. take the next point */
 932                 if( l == 0 )
 933                     status[i] = 0;
 934                 continue;
 935             }
 936
 937             for( j = 0; j < criteria.max_iter; j++ )
 938             {
 939                 double b[6] = {0,0,0,0,0,0}, eta[6];
 940                 double t0, t1, s = 0;
 941
 942                 if( Av[2] < -eps || Av[2] >= levelSize.width+eps ||
 943                     Av[5] < -eps || Av[5] >= levelSize.height+eps ||
 944                     icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], levelStep,
 945                     levelSize, patchJ, patchStep, patchSize, Av ) < 0 )
 946                 {
 947                     pt_status = 0;
 948                     break;
 949                 }
 950
 951                 for( y = -winSize.height, k = 0, meanJ = 0; y <= winSize.height; y++ )
 952                     for( x = -winSize.width; x <= winSize.width; x++, k++ )
 953                         meanJ += patchJ[k];
 954
 955                 meanJ = meanJ / (patchSize.width * patchSize.height) - meanI;
 956
 957                 for( y = -winSize.height, k = 0; y <= winSize.height; y++ )
 958                 {
 959                     for( x = -winSize.width; x <= winSize.width; x++, k++ )
 960                     {
 961                         double t = patchI[k] - patchJ[k] + meanJ;
 962                         double ixt = Ix[k] * t;
 963                         double iyt = Iy[k] * t;
 964
 965                         s += t;
 966
 967                         b[0] += ixt;
 968                         b[1] += iyt;
 969                         b[2] += x * ixt;
 970                         b[3] += y * ixt;
 971                         b[4] += x * iyt;
 972                         b[5] += y * iyt;
 973                     }
 974                 }
 975
 976                 icvTransformVector_64d( G, b, eta, 6, 6 );
 977
 978                 Av[2] = (float)(Av[2] + Av[0] * eta[0] + Av[1] * eta[1]);
 979                 Av[5] = (float)(Av[5] + Av[3] * eta[0] + Av[4] * eta[1]);
 980
 981                 t0 = Av[0] * (1 + eta[2]) + Av[1] * eta[4];
 982                 t1 = Av[0] * eta[3] + Av[1] * (1 + eta[5]);
 983                 Av[0] = (float)t0;
 984                 Av[1] = (float)t1;
 985
 986                 t0 = Av[3] * (1 + eta[2]) + Av[4] * eta[4];
 987                 t1 = Av[3] * eta[3] + Av[4] * (1 + eta[5]);
 988                 Av[3] = (float)t0;
 989                 Av[4] = (float)t1;
 990
 991                 if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon )
 992                     break;
 993             }
 994
 995             if( pt_status != 0 || l == 0 )
 996             {
 997                 status[i] = (char)pt_status;
 998                 featuresB[i].x = Av[2];
 999                 featuresB[i].y = Av[5];
1000
1001                 matrices[i*4] = Av[0];
1002                 matrices[i*4+1] = Av[1];
1003                 matrices[i*4+2] = Av[3];
1004                 matrices[i*4+3] = Av[4];
1005             }
1006
1007             if( pt_status && l == 0 && error )
1008             {
1009                 /* calc error */
1010                 double err = 0;
1011
1012                 for( y = 0, k = 0; y < patchSize.height; y++ )
1013                 {
1014                     for( x = 0; x < patchSize.width; x++, k++ )
1015                     {
1016                         double t = patchI[k] - patchJ[k] + meanJ;
1017                         err += t * t;
1018                     }
1019                 }
1020                 error[i] = (float)sqrt(err);
1021             }
1022         }
1023     }
1024
1025     __END__;
1026
1027     cvFree( &pyr_buffer );
1028     cvFree( &buffer );
1029     cvFree( &_status );
1030 }
1031
1032
1033
1034 static void
1035 icvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b,
1036                 int count, CvMat* M, int full_affine )
1037 {
1038     if( full_affine )
1039     {
1040         double sa[36], sb[6];
1041         CvMat A = cvMat( 6, 6, CV_64F, sa ), B = cvMat( 6, 1, CV_64F, sb );
1042         CvMat MM = cvMat( 6, 1, CV_64F, M->data.db );
1043
1044         int i;
1045
1046         memset( sa, 0, sizeof(sa) );
1047         memset( sb, 0, sizeof(sb) );
1048
1049         for( i = 0; i < count; i++ )
1050         {
1051             sa[0] += a[i].x*a[i].x;
1052             sa[1] += a[i].y*a[i].x;
1053             sa[2] += a[i].x;
1054
1055             sa[6] += a[i].x*a[i].y;
1056             sa[7] += a[i].y*a[i].y;
1057             sa[8] += a[i].y;
1058
1059             sa[12] += a[i].x;
1060             sa[13] += a[i].y;
1061             sa[14] += 1;
1062
1063             sb[0] += a[i].x*b[i].x;
1064             sb[1] += a[i].y*b[i].x;
1065             sb[2] += b[i].x;
1066             sb[3] += a[i].x*b[i].y;
1067             sb[4] += a[i].y*b[i].y;
1068             sb[5] += b[i].y;
1069         }
1070
1071         sa[21] = sa[0];
1072         sa[22] = sa[1];
1073         sa[23] = sa[2];
1074         sa[27] = sa[6];
1075         sa[28] = sa[7];
1076         sa[29] = sa[8];
1077         sa[33] = sa[12];
1078         sa[34] = sa[13];
1079         sa[35] = sa[14];
1080
1081         cvSolve( &A, &B, &MM, CV_SVD );
1082     }
1083     else
1084     {
1085         double sa[16], sb[4], m[4], *om = M->data.db;
1086         CvMat A = cvMat( 4, 4, CV_64F, sa ), B = cvMat( 4, 1, CV_64F, sb );
1087         CvMat MM = cvMat( 4, 1, CV_64F, m );
1088
1089         int i;
1090
1091         memset( sa, 0, sizeof(sa) );
1092         memset( sb, 0, sizeof(sb) );
1093
1094         for( i = 0; i < count; i++ )
1095         {
1096             sa[0] += a[i].x*a[i].x + a[i].y*a[i].y;
1097             sa[1] += 0;
1098             sa[2] += a[i].x;
1099             sa[3] += a[i].y;
1100
1101             sa[4] += 0;
1102             sa[5] += a[i].x*a[i].x + a[i].y*a[i].y;
1103             sa[6] += -a[i].y;
1104             sa[7] += a[i].x;
1105
1106             sa[8] += a[i].x;
1107             sa[9] += -a[i].y;
1108             sa[10] += 1;
1109             sa[11] += 0;
1110
1111             sa[12] += a[i].y;
1112             sa[13] += a[i].x;
1113             sa[14] += 0;
1114             sa[15] += 1;
1115
1116             sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;
1117             sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;
1118             sb[2] += b[i].x;
1119             sb[3] += b[i].y;
1120         }
1121
1122         cvSolve( &A, &B, &MM, CV_SVD );
1123
1124         om[0] = om[4] = m[0];
1125         om[1] = -m[1];
1126         om[3] = m[1];
1127         om[2] = m[2];
1128         om[5] = m[3];
1129     }
1130 }
1131
1132
1133 CV_IMPL int
1134 cvEstimateRigidTransform( const CvArr* _A, const CvArr* _B, CvMat* _M, int full_affine )
1135 {
1136     int result = 0;
1137
1138     const int COUNT = 15;
1139     const int WIDTH = 160, HEIGHT = 120;
1140     const int RANSAC_MAX_ITERS = 100;
1141     const int RANSAC_SIZE0 = 3;
1142     const double MIN_TRIANGLE_SIDE = 20;
1143     const double RANSAC_GOOD_RATIO = 0.5;
1144
1145     int allocated = 1;
1146     CvMat *sA = 0, *sB = 0;
1147     CvPoint2D32f *pA = 0, *pB = 0;
1148     int* good_idx = 0;
1149     char *status = 0;
1150     CvMat* gray = 0;
1151
1152     CV_FUNCNAME( "cvEstimateRigidTransform" );
1153
1154     __BEGIN__;
1155
1156     CvMat stubA, *A;
1157     CvMat stubB, *B;
1158     CvSize sz0, sz1;
1159     int cn, equal_sizes;
1160     int i, j, k, k1;
1161     int count_x, count_y, count;
1162     double scale = 1;
1163     CvRNG rng = cvRNG(-1);
1164     double m[6]={0};
1165     CvMat M = cvMat( 2, 3, CV_64F, m );
1166     int good_count = 0;
1167
1168     CV_CALL( A = cvGetMat( _A, &stubA ));
1169     CV_CALL( B = cvGetMat( _B, &stubB ));
1170
1171     if( !CV_IS_MAT(_M) )
1172         CV_ERROR( _M ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" );
1173
1174     if( !CV_ARE_SIZES_EQ( A, B ) )
1175         CV_ERROR( CV_StsUnmatchedSizes, "Both input images must have the same size" );
1176
1177     if( !CV_ARE_TYPES_EQ( A, B ) )
1178         CV_ERROR( CV_StsUnmatchedFormats, "Both input images must have the same data type" );
1179
1180     if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 )
1181     {
1182         cn = CV_MAT_CN(A->type);
1183         sz0 = cvGetSize(A);
1184         sz1 = cvSize(WIDTH, HEIGHT);
1185
1186         scale = MAX( (double)sz1.width/sz0.width, (double)sz1.height/sz0.height );
1187         scale = MIN( scale, 1. );
1188         sz1.width = cvRound( sz0.width * scale );
1189         sz1.height = cvRound( sz0.height * scale );
1190
1191         equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height;
1192
1193         if( !equal_sizes || cn != 1 )
1194         {
1195             CV_CALL( sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 ));
1196             CV_CALL( sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 ));
1197
1198             if( !equal_sizes && cn != 1 )
1199                 CV_CALL( gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 ));
1200
1201             if( gray )
1202             {
1203                 cvCvtColor( A, gray, CV_BGR2GRAY );
1204                 cvResize( gray, sA, CV_INTER_AREA );
1205                 cvCvtColor( B, gray, CV_BGR2GRAY );
1206                 cvResize( gray, sB, CV_INTER_AREA );
1207             }
1208             else if( cn == 1 )
1209             {
1210                 cvResize( gray, sA, CV_INTER_AREA );
1211                 cvResize( gray, sB, CV_INTER_AREA );
1212             }
1213             else
1214             {
1215                 cvCvtColor( A, gray, CV_BGR2GRAY );
1216                 cvResize( gray, sA, CV_INTER_AREA );
1217                 cvCvtColor( B, gray, CV_BGR2GRAY );
1218             }
1219
1220             cvReleaseMat( &gray );
1221             A = sA;
1222             B = sB;
1223         }
1224
1225         count_y = COUNT;
1226         count_x = cvRound((double)COUNT*sz1.width/sz1.height);
1227         count = count_x * count_y;
1228
1229         CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) ));
1230         CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) ));
1231         CV_CALL( status = (char*)cvAlloc( count*sizeof(status[0]) ));
1232
1233         for( i = 0, k = 0; i < count_y; i++ )
1234             for( j = 0; j < count_x; j++, k++ )
1235             {
1236                 pA[k].x = (j+0.5f)*sz1.width/count_x;
1237                 pA[k].y = (i+0.5f)*sz1.height/count_y;
1238             }
1239
1240         // find the corresponding points in B
1241         cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3,
1242                                 status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 );
1243
1244         // repack the remained points
1245         for( i = 0, k = 0; i < count; i++ )
1246             if( status[i] )
1247             {
1248                 if( i > k )
1249                 {
1250                     pA[k] = pA[i];
1251                     pB[k] = pB[i];
1252                 }
1253                 k++;
1254             }
1255
1256         count = k;
1257     }
1258     else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 )
1259     {
1260         count = A->cols*A->rows;
1261
1262         if( CV_IS_MAT_CONT(A->type & B->type) && CV_MAT_TYPE(A->type) == CV_32FC2 )
1263         {
1264             pA = (CvPoint2D32f*)A->data.ptr;
1265             pB = (CvPoint2D32f*)B->data.ptr;
1266             allocated = 0;
1267         }
1268         else
1269         {
1270             CvMat _pA, _pB;
1271
1272             CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) ));
1273             CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) ));
1274             _pA = cvMat( A->rows, A->cols, CV_32FC2, pA );
1275             _pB = cvMat( B->rows, B->cols, CV_32FC2, pB );
1276             cvConvert( A, &_pA );
1277             cvConvert( B, &_pB );
1278         }
1279     }
1280     else
1281         CV_ERROR( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" );
1282
1283     CV_CALL( good_idx = (int*)cvAlloc( count*sizeof(good_idx[0]) ));
1284
1285     if( count < RANSAC_SIZE0 )
1286         EXIT;
1287
1288     // RANSAC stuff:
1289     // 1. find the consensus
1290     for( k = 0; k < RANSAC_MAX_ITERS; k++ )
1291     {
1292         int idx[RANSAC_SIZE0];
1293         CvPoint2D32f a[3];
1294         CvPoint2D32f b[3];
1295
1296         memset( a, 0, sizeof(a) );
1297         memset( b, 0, sizeof(b) );
1298
1299         // choose random 3 non-complanar points from A & B
1300         for( i = 0; i < RANSAC_SIZE0; i++ )
1301         {
1302             for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ )
1303             {
1304                 idx[i] = cvRandInt(&rng) % count;
1305
1306                 for( j = 0; j < i; j++ )
1307                 {
1308                     if( idx[j] == idx[i] )
1309                         break;
1310                     // check that the points are not very close one each other
1311                     if( fabs(pA[idx[i]].x - pA[idx[j]].x) +
1312                         fabs(pA[idx[i]].y - pA[idx[j]].y) < MIN_TRIANGLE_SIDE )
1313                         break;
1314                     if( fabs(pB[idx[i]].x - pB[idx[j]].x) +
1315                         fabs(pB[idx[i]].y - pB[idx[j]].y) < MIN_TRIANGLE_SIDE )
1316                         break;
1317                 }
1318
1319                 if( j < i )
1320                     continue;
1321
1322                 if( i+1 == RANSAC_SIZE0 )
1323                 {
1324                     // additional check for non-complanar vectors
1325                     a[0] = pA[idx[0]];
1326                     a[1] = pA[idx[1]];
1327                     a[2] = pA[idx[2]];
1328
1329                     b[0] = pB[idx[0]];
1330                     b[1] = pB[idx[1]];
1331                     b[2] = pB[idx[2]];
1332
1333                     if( fabs((a[1].x - a[0].x)*(a[2].y - a[0].y) - (a[1].y - a[0].y)*(a[2].x - a[0].x)) < 1 ||
1334                         fabs((b[1].x - b[0].x)*(b[2].y - b[0].y) - (b[1].y - b[0].y)*(b[2].x - b[0].x)) < 1 )
1335                         continue;
1336                 }
1337                 break;
1338             }
1339
1340             if( k1 >= RANSAC_MAX_ITERS )
1341                 break;
1342         }
1343
1344         if( i < RANSAC_SIZE0 )
1345             continue;
1346
1347         // estimate the transformation using 3 points
1348         icvGetRTMatrix( a, b, 3, &M, full_affine );
1349
1350         for( i = 0, good_count = 0; i < count; i++ )
1351         {
1352             if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) +
1353                 fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < 8 )
1354                 good_idx[good_count++] = i;
1355         }
1356
1357         if( good_count >= count*RANSAC_GOOD_RATIO )
1358             break;
1359     }
1360
1361     if( k >= RANSAC_MAX_ITERS )
1362         EXIT;
1363
1364     if( good_count < count )
1365     {
1366         for( i = 0; i < good_count; i++ )
1367         {
1368             j = good_idx[i];
1369             pA[i] = pA[j];
1370             pB[i] = pB[j];
1371         }
1372     }
1373
1374     icvGetRTMatrix( pA, pB, good_count, &M, full_affine );
1375     m[2] /= scale;
1376     m[5] /= scale;
1377     CV_CALL( cvConvert( &M, _M ));
1378     result = 1;
1379
1380     __END__;
1381
1382     cvReleaseMat( &sA );
1383     cvReleaseMat( &sB );
1384     cvFree( &pA );
1385     cvFree( &pB );
1386     cvFree( &status );
1387     cvFree( &good_idx );
1388     cvReleaseMat( &gray );
1389
1390     return result;
1391 }
1392
1393
1394 /* End of file. */