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