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