Index: opencv/samples/c/lkdemo.c =================================================================== --- opencv/samples/c/lkdemo.c (revision 3296) +++ opencv/samples/c/lkdemo.c (working copy) @@ -7,6 +7,8 @@ #define CV_NO_BACKWARD_COMPATIBILITY +#define USE_AFFINE + #ifndef _EiC #include "cv.h" #include "highgui.h" @@ -14,11 +16,36 @@ #include #endif +static void draw_affine_feature(CvArr *img, const float *A, CvPoint2D32f x, CvScalar color, float sz) +{ + CvPoint corners[4] = { + cvPoint(x.x + (A[0]+A[1])*sz, x.y + (A[2]+A[3])*sz), + cvPoint(x.x + (A[0]-A[1])*sz, x.y + (A[2]-A[3])*sz), + cvPoint(x.x + (-A[0]-A[1])*sz, x.y + (-A[2]-A[3])*sz), + cvPoint(x.x + (-A[0]+A[1])*sz, x.y + (-A[2]+A[3])*sz) + }; + + cvLine(img, corners[3], corners[0], color, 1, 8, 0); + cvLine(img, corners[0], corners[1], color, 1, 8, 0); + cvLine(img, corners[1], corners[2], color, 1, 8, 0); + cvLine(img, corners[2], corners[3], color, 1, 8, 0); +} + +static void draw_feature(CvArr *img, CvPoint2D32f x) +{ + cvCircle( img, cvPointFrom32f(x), 3, CV_RGB(0,255,0), -1, 8,0); +} + IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid = 0, *swap_temp; int win_size = 10; const int MAX_COUNT = 500; CvPoint2D32f* points[2] = {0,0}, *swap_points; +#ifdef USE_AFFINE +CvAffineConsistencyPatch* affpatch; +float *affmatrices; +float *error; +#endif char* status = 0; int count = 0; int need_to_init = 0; @@ -94,6 +121,17 @@ prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 ); points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + +#ifdef USE_AFFINE + affpatch = (CvAffineConsistencyPatch*)cvAlloc(MAX_COUNT*sizeof(affpatch[0])); + affmatrices = (float*) cvAlloc(MAX_COUNT*4*sizeof(affmatrices[0])); + error = (float*) cvAlloc(MAX_COUNT*sizeof(error[0])); + for(i=0; i #include +#define DO_SCALE + namespace cv { @@ -791,6 +793,237 @@ namespace cv { +struct AffineConsistencyCheckInvoker +{ + AffineConsistencyCheckInvoker( const CvMat* _imgJ, + const CvPoint2D32f* _featuresA, + const CvAffineConsistencyPatch* _affinePatchA, + CvPoint2D32f* _featuresB, + float* _matrixB, + char* _status, float* _error, + CvTermCriteria _criteria, + CvSize _winSize ) + { + imgJ = _imgJ; + featuresA = _featuresA; + affinePatchA = _affinePatchA; + featuresB = _featuresB; + matrixB = _matrixB; + status = _status; + error = _error; + criteria = _criteria; + winSize = _winSize; + } + + void operator()(const BlockedRange& range) const + { + int i, i1 = range.begin(), i2 = range.end(); + + CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); + int patchLen = patchSize.width * patchSize.height; + + AutoBuffer buf(patchLen); + float* patchJ = buf; + CvSize imgSize = cvGetMatSize(imgJ); + + // find flow for each given point + for( i = i1; i < i2; i++ ) + { + int pt_status; + //CvPoint2D32f u; + double D = 0; + const float *patchI = (const float* ) affinePatchA[i].f->data.ptr; + const float *Ix = (const float* ) affinePatchA[i].fx->data.ptr; + const float *Iy = (const float* ) affinePatchA[i].fy->data.ptr; + const double *invG = (const double* ) affinePatchA[i].invG->data.ptr; + float Av[6]; + float alpha = 1, beta = 0; + int j, x, y; + + pt_status = status[i]; + if( !pt_status ) + continue; + + CvSize psz = cvGetSize(affinePatchA[i].f); + + Av[0] = matrixB[4*i + 0]; + Av[1] = matrixB[4*i + 1]; + Av[3] = matrixB[4*i + 2]; + Av[4] = matrixB[4*i + 3]; + Av[2] = featuresB[i].x; + Av[5] = featuresB[i].y; + + for( j = 0; j < criteria.max_iter; j++ ) + { + double g[8] = { 0 }; + + for(int k=0; kdata.ptr, imgJ->step, imgSize, + patchJ, patchSize.width*sizeof(float), patchSize, Av ) < 0 ) + { + // point is outside of the second image. take the next + status[i] = 0; + continue; + } + + int yy = -winSize.height; + for( y = 0; y < patchSize.height; y++, yy++ ) + { + // Here as well, scale pi, pj and ix and iy + const float* pi = patchI + y*patchSize.width; + const float* pj = patchJ + y*patchSize.width; + const float* ix = Ix + y*patchSize.width; + const float* iy = Iy + y*patchSize.width; + + int xx = -winSize.width; + for( x = 0; x < patchSize.width; x++, xx++ ) + { +#ifdef DO_SCALE + g[7] += pj[x] / 255; + g[6] += pi[x] * pj[x] / (255 * 255); + g[5] += iy[x] * pj[x] / (255 * 255); + g[4] += ix[x] * pj[x] / (255 * 255); + g[3] += yy * iy[x] * pj[x] / (255 * 255); + g[2] += xx * iy[x] * pj[x] / (255 * 255); + g[1] += yy * ix[x] * pj[x] / (255 * 255); + g[0] += xx * ix[x] * pj[x] / (255 * 255); +#else + g[7] += pj[x] ; + g[6] += pi[x] * pj[x] ; + g[5] += iy[x] * pj[x] ; + g[4] += ix[x] * pj[x] ; + g[3] += yy * iy[x] * pj[x] ; + g[2] += xx * iy[x] * pj[x] ; + g[1] += yy * ix[x] * pj[x] ; + g[0] += xx * ix[x] * pj[x] ; +#endif + } + } + + double dp[8] = {}; + // Compute dA = G^-1 g + for(int i=0; i<8; ++i) + { + const double *pG = invG + 8*i; + dp[i] = 0; + + for(int j=0; j<8; ++j) + { + dp[i] += pG[j] * g[j]; + } + } + + if(fabs(dp[6]) < 1e-5) + { + pt_status = 0; + break; + } + + for(int i=0; i<5; ++i) + dp[i] /= dp[6]; + + // Update Av. + // I( (I + dA)x + dv ) - J( A x + v ) + // let y = (I + dA) x + dv, then x = (I + dA)^-1 (y - dv) + // => I( y ) - J( A (I + dA)^-1 y + (v - A (I+dA)^-1 dv)) + // Compute determinant of (I+dA) + + D = 1.0/((1+dp[0])*(1+dp[3]) - dp[1]*dp[2]); + + // Inverse of (I+dA) is 1/det(I+dA) * [ 1+dp[3] , -dp[1] ; -dp[2] , 1+dp[0] ] + + float _Av[6]; + _Av[0] = (float) ( (Av[0]*(1+dp[3]) - Av[1]*dp[2] ) * D ); + _Av[1] = (float) ( (Av[1]*(1+dp[0]) - Av[0]*dp[1] ) * D ); + _Av[2] = Av[2] - ( _Av[0] * dp[4] + _Av[1] * dp[5] ); + _Av[3] = (float) ( (Av[3]*(1+dp[3]) - Av[4] * dp[2] ) * D ); + _Av[4] = (float) ( (Av[4]*(1+dp[0]) - Av[3] * dp[1] ) * D ); + _Av[5] = Av[5] - ( _Av[3] * dp[4] + _Av[4] * dp[5] ); + + // Check if new (intermediate) affine distortion is reasonable + double n1 = hypot( _Av[0], _Av[3] ); // norm of first column of A = A (1, 0)^T + double n2 = hypot( _Av[1], _Av[4] ); // norm of second column of A = A (0, 1)^T + + // Check for extreme foreshortening + if( n1 < 0.2 || n1 > 5 ) + { + pt_status = 0; + break; + } + if( n2 < 0.2 || n2 > 5 ) + { + pt_status = 0; + break; + } + // Check for extreme distortion + if( fabs(_Av[0]*_Av[1] + _Av[3]*_Av[4]) > 0.92*n1*n2 ) + { + pt_status = 0; + break; + } + + // Copy values and check parameter change + float delta = 0; + for(int i=0; i<6; ++i) + { + float dA = Av[i] - _Av[i]; + delta += dA * dA; + Av[i] = _Av[i]; + } + + alpha = dp[6]; +#ifdef DO_SCALE + beta = dp[7] * 255; +#else + beta = dp[7]; +#endif + + if( delta < criteria.epsilon ) + break; + } + + featuresB[i].x = Av[2]; + featuresB[i].y = Av[5]; + + matrixB[4*i+0] = Av[0]; + matrixB[4*i+1] = Av[1]; + matrixB[4*i+2] = Av[3]; + matrixB[4*i+3] = Av[4]; + + status[i] = (char)pt_status; + if( error && pt_status ) + { + // calc error + double err = 0; + for( y = 0; y < patchSize.height; y++ ) + { + const float* pi = patchI + y*patchSize.width; + const float* pj = patchJ + y*patchSize.width; + + for( x = 0; x < patchSize.width; x++ ) + { + double t = alpha*pi[x] + beta - pj[x]; + err += t * t; + } + } + error[i] = (float)sqrt(err); + } + } // end of point processing loop (i) + } + + const CvMat* imgJ; + const CvPoint2D32f* featuresA; + const CvAffineConsistencyPatch* affinePatchA; + CvPoint2D32f* featuresB; + float* matrixB; + char* status; + float* error; + CvTermCriteria criteria; + CvSize winSize; +}; + struct LKTrackerInvoker { LKTrackerInvoker( const CvMat* _imgI, const CvMat* _imgJ, @@ -1020,10 +1253,283 @@ int flags; }; +} + +CV_IMPL void +cvInitAffineConsistencyPatch( CvAffineConsistencyPatch *patch, CvSize winSize ) +{ + if( !patch ) + CV_Error( CV_StsNullPtr, "" ); + + if( winSize.width <= 0 || winSize.height <= 0 ) + CV_Error( CV_StsOutOfRange, "window size must be positive" ); + + /* clear existing values */ + memset( patch, 0, sizeof *patch ); + + /* allocate memory for patch (f), gradients (fx, fy) and inverse G = sum_x h(x)*h(x)^T */ + int w = 2*winSize.width + 1; + int h = 2*winSize.height + 1; + + patch->f = cvCreateMat(w, h, CV_32FC1); + patch->fx = cvCreateMat(w, h, CV_32FC1); + patch->fy = cvCreateMat(w, h, CV_32FC1); + patch->invG = cvCreateMat(8, 8, CV_64FC1); +} + +CV_IMPL void +cvClearAffineConsistencyPatchData( CvAffineConsistencyPatch *patch ) +{ + if( patch ) + { + cvReleaseMat( &patch->f ); + cvReleaseMat( &patch->fx ); + cvReleaseMat( &patch->fy ); + cvReleaseMat( &patch->invG ); + + memset( patch, 0, sizeof *patch ); + } +} + +CV_IMPL void +cvSwapAffineConsistencyPatch( CvAffineConsistencyPatch *a, CvAffineConsistencyPatch *b ) +{ + std::swap(*a, *b); +} + +CV_IMPL void +cvAffineConsistencyCheck( const void* arrB, + const CvPoint2D32f * featuresA, + const CvAffineConsistencyPatch * patchA, + CvPoint2D32f* featuresB, + float *matrixB, + int count, CvSize winSize, + char *status, float *error, + CvTermCriteria criteria ) +{ + cv::AutoBuffer buffer; + cv::AutoBuffer _status; + + CvMat stubB, *imgB = (CvMat*)arrB; + CvSize imgSize; + imgB = cvGetMat( imgB, &stubB ); + + if( CV_MAT_TYPE( imgB->type ) != CV_8UC1 ) + CV_Error( CV_StsUnsupportedFormat, "" ); + + imgSize = cvGetMatSize( imgB ); + + if( count == 0 ) + return; + + if( !featuresA || !featuresB ) + CV_Error( CV_StsNullPtr, "Some of arrays of point coordinates are missing" ); + + if( count < 0 ) + CV_Error( CV_StsOutOfRange, "The number of tracked points is negative" ); + + if( winSize.width <= 1 || winSize.height <= 1 ) + CV_Error( CV_StsBadSize, "Invalid search window size" ); + + if( !status ) + { + _status.allocate(count); + status = _status; + } + + memset( status, 1, count ); + if( error ) + memset( error, 0, count*sizeof(error[0]) ); + + cv::parallel_for(cv::BlockedRange(0, count), + cv::AffineConsistencyCheckInvoker(imgB, featuresA, patchA, + featuresB, matrixB, status, error, + criteria, winSize)); } +CV_IMPL void cvGetAffineConsistencyPatches( const CvArr* arrA, + const CvPoint2D32f* prev_features, + CvAffineConsistencyPatch* initial_patches, + float* affine_matrix, + int count, + CvSize winSize ) +{ + static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ + CvMat stubA, *imgA = (CvMat*) arrA; + imgA = cvGetMat( imgA, &stubA ); + cv::Mat G(8, 8, CV_64FC1); + float *patchI, *tmpBuffer; + CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); + int patchLen = patchSize.width * patchSize.height; + int patchStep = patchSize.width * sizeof( patchI[0] ); + CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 ); + int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2); + int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] ); + CvSize imgSize; + float eps = (float)MIN(winSize.width, winSize.height); + + if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) + CV_Error( CV_StsUnsupportedFormat, "" ); + + imgSize = cvGetMatSize( imgA ); + + cv::AutoBuffer buf(patchLen + srcPatchLen); + patchI = buf; + tmpBuffer = patchI + srcPatchLen; + + for( int i=0; idata.ptr; + float * const Ix = (float*) initial_patches[i].fx->data.ptr; + float * const Iy = (float*) initial_patches[i].fy->data.ptr; + + if( u.x < -eps || u.x >= imgSize.width+eps || + u.y < -eps || u.y >= imgSize.height+eps || + icvGetRectSubPix_8u32f_C1R( imgA->data.ptr, imgA->step, + imgSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 ) + { + // point is outside the first image. take the next + continue; + } + + /* repack patchI into initial_patches.f (remove borders) */ + for( int k = 0; k < patchSize.height; k++ ) + memcpy( I + k * patchSize.width, + patchI + (k + 1) * srcPatchSize.width + 1, patchStep ); + + icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy, + (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize, + smoothKernel, tmpBuffer ); + + // Compute G = sum h*h' + // Where h = (x Ix, y Ix, x Iy, y Iy, Ix, Iy, I, 1) + + G = cvScalar(0); + for( int y=-winSize.height, yy=1, yyy=0; yy < srcPatchSize.height-1; ++yyy, ++yy, ++y ) + { + // Scale i to be in the range between 0 and 1 instead of 0 and 255 + // This also scales ix and iy + CvSize sz = cvGetSize(initial_patches[i].fx); + const float * const i = patchI + yy*srcPatchSize.width + 1; + const float * const ix = Ix + yyy*patchSize.width; + const float * const iy = Iy + yyy*patchSize.width; + + for( int x=-winSize.width, xx=0; xx < srcPatchSize.width-2; ++xx, ++x ) + { +#ifdef DO_SCALE + G.at(0,0) += (x*ix[xx]) * (x*ix[xx]) / (255 * 255); + G.at(0,1) += (x*ix[xx]) * (y*ix[xx]) / (255 * 255); + G.at(0,2) += (x*ix[xx]) * (x*iy[xx]) / (255 * 255); + G.at(0,3) += (x*ix[xx]) * (y*iy[xx]) / (255 * 255); + G.at(0,4) += (x*ix[xx]) * (ix[xx]) / (255 * 255); + G.at(0,5) += (x*ix[xx]) * (iy[xx]) / (255 * 255); + G.at(0,6) += (x*ix[xx]) * (i[xx]) / (255 * 255); + G.at(0,7) += (x*ix[xx]) / 255; + + G.at(1,1) += (y*ix[xx]) * (y*ix[xx]) / (255 * 255); + G.at(1,2) += (y*ix[xx]) * (x*iy[xx]) / (255 * 255); + G.at(1,3) += (y*ix[xx]) * (y*iy[xx]) / (255 * 255); + G.at(1,4) += (y*ix[xx]) * (ix[xx]) / (255 * 255); + G.at(1,5) += (y*ix[xx]) * (iy[xx]) / (255 * 255); + G.at(1,6) += (y*ix[xx]) * (i[xx]) / (255 * 255); + G.at(1,7) += (y*ix[xx]) / 255; + + G.at(2,2) += (x*iy[xx]) * (x*iy[xx]) / (255 * 255); + G.at(2,3) += (x*iy[xx]) * (y*iy[xx]) / (255 * 255); + G.at(2,4) += (x*iy[xx]) * (ix[xx]) / (255 * 255); + G.at(2,5) += (x*iy[xx]) * (iy[xx]) / (255 * 255); + G.at(2,6) += (x*iy[xx]) * (i[xx]) / (255 * 255); + G.at(2,7) += (x*iy[xx]) / 255; + + G.at(3,3) += (y*iy[xx]) * (y*iy[xx]) / (255 * 255); + G.at(3,4) += (y*iy[xx]) * (ix[xx]) / (255 * 255); + G.at(3,5) += (y*iy[xx]) * (iy[xx]) / (255 * 255); + G.at(3,6) += (y*iy[xx]) * (i[xx]) / (255 * 255); + G.at(3,7) += (y*iy[xx]) / 255; + + G.at(4,4) += (ix[xx]) * (ix[xx]) / (255 * 255); + G.at(4,5) += (ix[xx]) * (iy[xx]) / (255 * 255); + G.at(4,6) += (ix[xx]) * (i[xx]) / (255 * 255); + G.at(4,7) += (ix[xx]) / 255; + + G.at(5,5) += (iy[xx]) * (iy[xx]) / (255 * 255); + G.at(5,6) += (iy[xx]) * (i[xx]) / (255 * 255); + G.at(5,7) += (iy[xx]) / 255 ; + + G.at(6,6) += (i[xx]) * (i[xx]) / (255 * 255); + G.at(6,7) += (i[xx]) / 255; + + G.at(7,7) += 1; +#else + G.at(0,0) += (x*ix[xx]) * (x*ix[xx]) ; + G.at(0,1) += (x*ix[xx]) * (y*ix[xx]) ; + G.at(0,2) += (x*ix[xx]) * (x*iy[xx]) ; + G.at(0,3) += (x*ix[xx]) * (y*iy[xx]) ; + G.at(0,4) += (x*ix[xx]) * (ix[xx]) ; + G.at(0,5) += (x*ix[xx]) * (iy[xx]) ; + G.at(0,6) += (x*ix[xx]) * (i[xx]) ; + G.at(0,7) += (x*ix[xx]) ; + + G.at(1,1) += (y*ix[xx]) * (y*ix[xx]) ; + G.at(1,2) += (y*ix[xx]) * (x*iy[xx]) ; + G.at(1,3) += (y*ix[xx]) * (y*iy[xx]) ; + G.at(1,4) += (y*ix[xx]) * (ix[xx]) ; + G.at(1,5) += (y*ix[xx]) * (iy[xx]) ; + G.at(1,6) += (y*ix[xx]) * (i[xx]) ; + G.at(1,7) += (y*ix[xx]) ; + + G.at(2,2) += (x*iy[xx]) * (x*iy[xx]) ; + G.at(2,3) += (x*iy[xx]) * (y*iy[xx]) ; + G.at(2,4) += (x*iy[xx]) * (ix[xx]) ; + G.at(2,5) += (x*iy[xx]) * (iy[xx]) ; + G.at(2,6) += (x*iy[xx]) * (i[xx]) ; + G.at(2,7) += (x*iy[xx]) ; + + G.at(3,3) += (y*iy[xx]) * (y*iy[xx]) ; + G.at(3,4) += (y*iy[xx]) * (ix[xx]) ; + G.at(3,5) += (y*iy[xx]) * (iy[xx]) ; + G.at(3,6) += (y*iy[xx]) * (i[xx]) ; + G.at(3,7) += (y*iy[xx]) ; + + G.at(4,4) += (ix[xx]) * (ix[xx]) ; + G.at(4,5) += (ix[xx]) * (iy[xx]) ; + G.at(4,6) += (ix[xx]) * (i[xx]) ; + G.at(4,7) += (ix[xx]) ; + + G.at(5,5) += (iy[xx]) * (iy[xx]) ; + G.at(5,6) += (iy[xx]) * (i[xx]) ; + G.at(5,7) += (iy[xx]) ; + + G.at(6,6) += (i[xx]) * (i[xx]) ; + G.at(6,7) += (i[xx]) ; + + G.at(7,7) += 1; + +#endif + } + } + + // Fill lower half (symmetric) + for( int k=1; k<8; ++k ) + for( int m=0; m(k,m) = G.at(m,k); + + // Invert G and store in invG + cv::Mat invGref = initial_patches[i].invG; + invGref = G.inv(); + + // Set matrix to identity. + affine_matrix[4*i + 0] = 1; + affine_matrix[4*i + 1] = 0; + affine_matrix[4*i + 2] = 0; + affine_matrix[4*i + 3] = 1; + } +} + CV_IMPL void cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB, void* pyrarrA, void* pyrarrB, @@ -1103,7 +1609,7 @@ CV_Error( CV_StsNullPtr, "Some of arrays of point coordinates are missing" ); if( count < 0 ) - CV_Error( CV_StsOutOfRange, "The number of tracked points is negative or zero" ); + CV_Error( CV_StsOutOfRange, "The number of tracked points is negative" ); if( winSize.width <= 1 || winSize.height <= 1 ) CV_Error( CV_StsBadSize, "Invalid search window size" );