#define USE_AFFINE

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);

}

#ifdef USE_AFFINE

CvAffineConsistencyPatch* affpatch;

float *affmatrices;

float *error;

#endif

#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<MAX_COUNT; ++i)

            {

                cvInitAffineConsistencyPatch(affpatch + i, cvSize(win_size,win_size));

            }

#endif

#ifdef USE_AFFINE

            cvGetAffineConsistencyPatches( grey, points[1], affpatch, affmatrices, count, cvSize(win_size, win_size) );

#endif 

#ifdef USE_AFFINE

            cvAffineConsistencyCheck( grey, points[0], affpatch, points[1], affmatrices, count, cvSize(win_size, win_size), status, error, cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03) );

            const double err_thresh = (win_size*2+1)*(win_size*2+1) * 5.0;

            for( i = 0; i < count; i++ )

            {

                // let's say avg. per pixel error should be max 5

                // Then error threshold is patchLen * 5

                status[i] &= (error[i] < err_thresh);

            }

#endif

#ifdef USE_AFFINE

                cvSwapAffineConsistencyPatch(affpatch + k - 1, affpatch + i);

                memcpy(affmatrices + 4*(k-1), affmatrices + 4*i, 4*sizeof *affmatrices);

                draw_affine_feature(image, affmatrices + 4*i, points[1][i], CV_RGB(0,255,0), 10);

#else

                draw_feature(image, points[1][i]);

#endif

#ifdef USE_AFFINE

            cvGetAffineConsistencyPatches( grey, points[1] + count - 1, 

                affpatch + count - 1, affmatrices + count - 1, 

                1, cvSize(win_size, win_size));

#endif

typedef struct CvAffineConsistencyPatch

{

    CvMat*  f;

    CvMat*  fx;

    CvMat*  fy;

    CvMat*  invG; // = ( sum h(x) h(x)^T )^-1, with h(x) = (x fx, y fx, x fy, y fy, fx, fy, f, 1)^T

} CvAffineConsistencyPatch;

CVAPI(void) cvInitAffineConsistencyPatch( CvAffineConsistencyPatch *p, CvSize win_size );

CVAPI(void) cvClearAffineConsistencyPatch( CvAffineConsistencyPatch *p );

CVAPI(void) cvSwapAffineConsistencyPatch( CvAffineConsistencyPatch *a, CvAffineConsistencyPatch *b );

/* This is much like cvCalcOpticalFlowPyrLK, but modified according to

   Zinsser, Graessl and Niemann, "Efficient Feature Tracking for Long Video Sequences".

   Also, this does _not_ use a pyramid as it expects the feature positions to be roughly

   correct on entry. This should only be used to check for consistency. */

CVAPI(void)  cvAffineConsistencyCheck( const CvArr*  curr,

                                       const CvPoint2D32f* prev_features,

                                       const CvAffineConsistencyPatch* initial_patches,

                                       CvPoint2D32f* curr_features,

                                       float*    affine_matrix,

                                       int       count,

                                       CvSize    win_size,

                                       char*     status,

                                       float*    track_error,

                                       CvTermCriteria criteria );

/* This function cuts out the patches needed for cvAffineConsistencyCheck) */

CVAPI(void)  cvGetAffineConsistencyPatches( const CvArr*  curr,

                                            const CvPoint2D32f* prev_features, 

                                            CvAffineConsistencyPatch* initial_patches,

                                            float*    affine_matrix,

                                            int       count,

                                            CvSize    win_size );

#define DO_SCALE

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<float> 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; k<patchLen; ++k)

                    patchJ[k] = CV_8TO32F(0);

                if( icvGetQuadrangleSubPix_8u32f_C1R( imgJ->data.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;

};

}

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<uchar> buffer;

    cv::AutoBuffer<char> _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<float> buf(patchLen + srcPatchLen);

    patchI = buf;

    tmpBuffer = patchI + srcPatchLen;

    for( int i=0; i<count; i++ )

    {

            const CvPoint2D32f & u = prev_features[i];

            float * const I = (float*) initial_patches[i].f->data.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<double>(0,0) += (x*ix[xx]) * (x*ix[xx]) / (255 * 255);

                    G.at<double>(0,1) += (x*ix[xx]) * (y*ix[xx]) / (255 * 255);

                    G.at<double>(0,2) += (x*ix[xx]) * (x*iy[xx]) / (255 * 255);

                    G.at<double>(0,3) += (x*ix[xx]) * (y*iy[xx]) / (255 * 255);

                    G.at<double>(0,4) += (x*ix[xx]) * (ix[xx])   / (255 * 255);

                    G.at<double>(0,5) += (x*ix[xx]) * (iy[xx])   / (255 * 255);

                    G.at<double>(0,6) += (x*ix[xx]) * (i[xx])    / (255 * 255);

                    G.at<double>(0,7) += (x*ix[xx])              /  255;

                    G.at<double>(1,1) += (y*ix[xx]) * (y*ix[xx]) / (255 * 255);

                    G.at<double>(1,2) += (y*ix[xx]) * (x*iy[xx]) / (255 * 255);

                    G.at<double>(1,3) += (y*ix[xx]) * (y*iy[xx]) / (255 * 255);

                    G.at<double>(1,4) += (y*ix[xx]) * (ix[xx])   / (255 * 255);

                    G.at<double>(1,5) += (y*ix[xx]) * (iy[xx])   / (255 * 255);

                    G.at<double>(1,6) += (y*ix[xx]) * (i[xx])    / (255 * 255);

                    G.at<double>(1,7) += (y*ix[xx])              /  255;

                    G.at<double>(2,2) += (x*iy[xx]) * (x*iy[xx]) / (255 * 255);

                    G.at<double>(2,3) += (x*iy[xx]) * (y*iy[xx]) / (255 * 255);

                    G.at<double>(2,4) += (x*iy[xx]) * (ix[xx])   / (255 * 255);

                    G.at<double>(2,5) += (x*iy[xx]) * (iy[xx])   / (255 * 255);

                    G.at<double>(2,6) += (x*iy[xx]) * (i[xx])    / (255 * 255);

                    G.at<double>(2,7) += (x*iy[xx])              /  255;

                    G.at<double>(3,3) += (y*iy[xx]) * (y*iy[xx]) / (255 * 255);

                    G.at<double>(3,4) += (y*iy[xx]) * (ix[xx])   / (255 * 255);

                    G.at<double>(3,5) += (y*iy[xx]) * (iy[xx])   / (255 * 255);

                    G.at<double>(3,6) += (y*iy[xx]) * (i[xx])    / (255 * 255);

                    G.at<double>(3,7) += (y*iy[xx])              /  255;

                    G.at<double>(4,4) += (ix[xx])   * (ix[xx])   / (255 * 255);

                    G.at<double>(4,5) += (ix[xx])   * (iy[xx])   / (255 * 255);

                    G.at<double>(4,6) += (ix[xx])   * (i[xx])    / (255 * 255);

                    G.at<double>(4,7) += (ix[xx])                /  255;

                    G.at<double>(5,5) += (iy[xx])   * (iy[xx])   / (255 * 255);

                    G.at<double>(5,6) += (iy[xx])   * (i[xx])    / (255 * 255);

                    G.at<double>(5,7) += (iy[xx])                /  255 ;

                    G.at<double>(6,6) += (i[xx])    * (i[xx])    / (255 * 255);

                    G.at<double>(6,7) += (i[xx])                 /  255;

                    G.at<double>(7,7) += 1;

#else

                    G.at<double>(0,0) += (x*ix[xx]) * (x*ix[xx]) ;

                    G.at<double>(0,1) += (x*ix[xx]) * (y*ix[xx]) ;

                    G.at<double>(0,2) += (x*ix[xx]) * (x*iy[xx]) ;

                    G.at<double>(0,3) += (x*ix[xx]) * (y*iy[xx]) ;

                    G.at<double>(0,4) += (x*ix[xx]) * (ix[xx])   ;

                    G.at<double>(0,5) += (x*ix[xx]) * (iy[xx])   ;

                    G.at<double>(0,6) += (x*ix[xx]) * (i[xx])    ;

                    G.at<double>(0,7) += (x*ix[xx])              ;

                    G.at<double>(1,1) += (y*ix[xx]) * (y*ix[xx]) ;

                    G.at<double>(1,2) += (y*ix[xx]) * (x*iy[xx]) ;

                    G.at<double>(1,3) += (y*ix[xx]) * (y*iy[xx]) ;

                    G.at<double>(1,4) += (y*ix[xx]) * (ix[xx])   ;

                    G.at<double>(1,5) += (y*ix[xx]) * (iy[xx])   ;

                    G.at<double>(1,6) += (y*ix[xx]) * (i[xx])    ;

                    G.at<double>(1,7) += (y*ix[xx])              ;

                    G.at<double>(2,2) += (x*iy[xx]) * (x*iy[xx]) ;

                    G.at<double>(2,3) += (x*iy[xx]) * (y*iy[xx]) ;

                    G.at<double>(2,4) += (x*iy[xx]) * (ix[xx])   ;

                    G.at<double>(2,5) += (x*iy[xx]) * (iy[xx])   ;

                    G.at<double>(2,6) += (x*iy[xx]) * (i[xx])    ;

                    G.at<double>(2,7) += (x*iy[xx])              ;

                    G.at<double>(3,3) += (y*iy[xx]) * (y*iy[xx]) ;

                    G.at<double>(3,4) += (y*iy[xx]) * (ix[xx])   ;

                    G.at<double>(3,5) += (y*iy[xx]) * (iy[xx])   ;

                    G.at<double>(3,6) += (y*iy[xx]) * (i[xx])    ;

                    G.at<double>(3,7) += (y*iy[xx])              ;

                    G.at<double>(4,4) += (ix[xx])   * (ix[xx])   ;

                    G.at<double>(4,5) += (ix[xx])   * (iy[xx])   ;

                    G.at<double>(4,6) += (ix[xx])   * (i[xx])    ;

                    G.at<double>(4,7) += (ix[xx])                ;

                    G.at<double>(5,5) += (iy[xx])   * (iy[xx])   ;

                    G.at<double>(5,6) += (iy[xx])   * (i[xx])    ;

                    G.at<double>(5,7) += (iy[xx])                ;

                    G.at<double>(6,6) += (i[xx])    * (i[xx])    ;

                    G.at<double>(6,7) += (i[xx])                 ;

                    G.at<double>(7,7) += 1;

#endif

                }

            }

            // Fill lower half (symmetric)

            for( int k=1; k<8; ++k )

                for( int m=0; m<k; ++m )

                    G.at<double>(k,m) = G.at<double>(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_Error( CV_StsOutOfRange, "The number of tracked points is negative" );